ros_arduino_bridge功能包集
請儘量自行在源碼上修改。
文件樹及說明:這裏引用創客智造
├── README.md
├── ros_arduino_bridge # metapackage (元包)
│ ├── CMakeLists.txt
│ └── package.xml
├── ros_arduino_firmware #固件包,更新到Arduino
│ ├── CMakeLists.txt
│ ├── package.xml
│ └── src
│ └── libraries #庫目錄
│ ├── MegaRobogaiaPololu #針對Pololu電機控制器,MegaRobogaia編碼器的頭文件定義
│ │ ├── commands.h #定義命令頭文件
│ │ ├── diff_controller.h #差分輪PID控制頭文件
│ │ ├── MegaRobogaiaPololu.ino #PID實現文件
│ │ ├── sensors.h #傳感器相關實現,超聲波測距,Ping函數
│ │ └── servos.h #伺服器頭文件
│ └── ROSArduinoBridge #Arduino相關庫定義
│ ├── commands.h #定義命令
│ ├── diff_controller.h #差分輪PID控制頭文件
│ ├── encoder_driver.h #編碼器驅動頭文件,定義插腳(pins)
│ ├── encoder_driver.ino #編碼器驅動實現, 讀取編碼器數據,重置編碼器等
│ ├── motor_driver.h #電機驅動頭文件
│ ├── motor_driver.ino #電機驅動實現,初始化控制器,設置速度
│ ├── ROSArduinoBridge.ino #核心功能實現,
│ ├── sensors.h #傳感器頭文件及實現
│ ├── servos.h #伺服器頭文件,定義插腳,類
│ └── servos.ino #伺服器實現
├── ros_arduino_msgs #消息定義包
│ ├── CMakeLists.txt
│ ├── msg #定義消息
│ │ ├── AnalogFloat.msg #定義模擬IO浮點消息
│ │ ├── Analog.msg #定義模擬IO數字消息
│ │ ├── ArduinoConstants.msg #定義常量消息
│ │ ├── Digital.msg #定義數字IO消息
│ │ └── SensorState.msg #定義傳感器狀態消息
│ ├── package.xml
│ └── srv #定義服務
│ ├── AnalogRead.srv #模擬IO輸入
│ ├── AnalogWrite.srv #模擬IO輸出
│ ├── DigitalRead.srv #數字IO輸入
│ ├── DigitalSetDirection.srv #數字IO設置方向
│ ├── DigitalWrite.srv #數字IO輸入
│ ├── ServoRead.srv #伺服電機輸入
│ └── ServoWrite.srv #伺服電機輸出
└── ros_arduino_python #ROS相關的Python包,用於上位機,樹莓派等開發板或電腦等。
├── CMakeLists.txt
├── config #配置目錄
│ └── arduino_params.yaml #定義相關參數,端口,rate,PID,sensors等默認參數。由arduino.launch調用
├── launch
│ └── arduino.launch #啓動文件
├── nodes
│ └── arduino_node.py #python文件,實際處理節點,由arduino.launch調用,即可單獨調用。
├── package.xml
├── setup.py
└── src #Python類包目錄
└── ros_arduino_python
├── arduino_driver.py #Arduino驅動類
├── arduino_sensors.py #Arduino傳感器類
├── base_controller.py #基本控制類,訂閱cmd_vel話題,發佈odom話題
└── __init__.py #類包默認空文件
各部分源碼:
arduino下位機部分:(如果不能運行,可能是註釋的問題)
- commands.h 命令:
/* Define single-letter commands that will be sent by the PC over the
serial link.
*/
#ifndef COMMANDS_H
#define COMMANDS_H
#define ANALOG_READ 'a' // 模擬讀取
#define GET_BAUDRATE 'b' // 獲取波特率
#define PIN_MODE 'c' // 接口模式
#define DIGITAL_READ 'd' // 數字讀取
#define READ_ENCODERS 'e' // 讀取編碼器數據 輸出爲:左數據,右數據
#define MOTOR_SPEEDS 'm' // 設置馬達速度,脈衝計數 如: m 20 20 會以一個很的速度轉動
#define PING 'p' // 無用
#define RESET_ENCODERS 'r' // 重設編碼器讀數
#define SERVO_WRITE 's' // 舵機相關,我這裏沒有用到
#define SERVO_READ 't' // 舵機相關,我這裏沒有用到
#define UPDATE_PID 'u' // 更新pid參數
#define DIGITAL_WRITE 'w' // 數字端口設置
#define ANALOG_WRITE 'x' // 模擬端口設置
#define READ_PIDOUT 'f' // 讀取pid計算出的速度(脈衝數計數)
#define READ_PIDIN 'i' // 讀取進入pid的速度
#define LEFT 0
#define RIGHT 1
#define FORWARDS true //FORWARDS前進代表bool真值true
#define BACKWARDS false //BACKWARDS後退代表bool假值false
#endif
- ROSArduinoBridge.ino源碼:
//這是在官方包基礎上修改來的
#define USE_BASE // Enable the base controller code 是否使用base controller
//#undef USE_BASE // Disable the base controller code
/* Define the motor controller and encoder library you are using */
//定義電機控制方式,使用ifdef 的方式,和正常的if 語句類似
#ifdef USE_BASE
/* The Pololu VNH5019 dual motor driver shield */
//#define POLOLU_VNH5019
/* The Pololu MC33926 dual motor driver shield */
//#define POLOLU_MC33926
/* The RoboGaia encoder shield */
//#define ROBOGAIA
/* Encoders directly attached to Arduino board */
//#define ARDUINO_ENC_COUNTER
/* L298 Motor driver*/
#define L298_MOTOR_DRIVER //這裏只是借用名稱,之後需要自己更改驅動
#endif
//#define USE_SERVOS // Enable use of PWM servos as defined in servos.h
#undef USE_SERVOS // Disable use of PWM servos //不適用SERVOS
/* Serial port baud rate */
#define BAUDRATE 57600 //波特率
/* Maximum PWM signal */
#define MAX_PWM 255 //最大pwm
#if defined(ARDUINO) && ARDUINO >= 100 //不知到用處
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
/* Include definition of serial commands */
#include "commands.h"
/* Sensor functions */
//#include "sensors.h" //不使用
/* Include servo support if required */
#ifdef USE_SERVOS
#include <Servo.h>
#include "servos.h"
#endif
#ifdef USE_BASE
/* Motor driver function definitions */
#include "motor_driver.h"//加載電機控制頭文件
/* Encoder driver function definitions */
#include "encoder_driver.h"//加載編碼器控制頭文件
/* PID parameters and functions */
#include "diff_controller.h"//加載PID控制
/* Run the PID loop at 30 times per second */
#define PID_RATE 30 // Hz //看英文註釋
/* Convert the rate into an interval */
const int PID_INTERVAL = 1000 / PID_RATE; //看英文註釋
/* Track the next time we make a PID calculation */
unsigned long nextPID = PID_INTERVAL;
/* Stop the robot if it hasn't received a movement command
in this number of milliseconds */
#define AUTO_STOP_INTERVAL 2000
long lastMotorCommand = AUTO_STOP_INTERVAL;
#endif
/* Variable initialization */
// A pair of varibles to help parse serial commands (thanks Fergs)
int arg = 0;
int index = 0;
// Variable to hold an input character
char chr;
// Variable to hold the current single-character command
char cmd;
// Character arrays to hold the first and second arguments
char argv1[16];
char argv2[16];
// The arguments converted to integers
long arg1;
long arg2;
/* Clear the current command parameters */
void resetCommand() {
cmd = NULL;
memset(argv1, 0, sizeof(argv1));
memset(argv2, 0, sizeof(argv2));
arg1 = 0;
arg2 = 0;
arg = 0;
index = 0;
}
/* Run a command. Commands are defined in commands.h */
//運行命令,命令在commands.h中被定義
int runCommand() {
int i = 0;
char *p = argv1;
char *str;
int pid_args[8]; //由於左右電機使用不同的pid參數,一面要4個
arg1 = atoi(argv1);
arg2 = atoi(argv2);
switch (cmd) {
case READ_PIDIN: //與commands.h中的匹配 這裏爲添加的,原代碼中沒有
Serial.print( readPidIn(LEFT));
Serial.print(" ");
Serial.println( readPidIn(RIGHT));
break;
case READ_PIDOUT://爲了方便之後的PID調整
Serial.print( readPidOut(LEFT));
Serial.print(" ");
Serial.println( readPidOut(RIGHT));
break;
case GET_BAUDRATE:
Serial.println(BAUDRATE);
break;
case ANALOG_READ:
Serial.println(analogRead(arg1));
break;
case DIGITAL_READ:
Serial.println(digitalRead(arg1));
break;
case ANALOG_WRITE:
analogWrite(arg1, arg2);
Serial.println("OK");
break;
case DIGITAL_WRITE:
if (arg2 == 0) digitalWrite(arg1, LOW);
else if (arg2 == 1) digitalWrite(arg1, HIGH);
Serial.println("OK");
break;
case PIN_MODE:
if (arg2 == 0) pinMode(arg1, INPUT);
else if (arg2 == 1) pinMode(arg1, OUTPUT);
Serial.println("OK");
break;
/*case PING:
Serial.println(Ping(arg1));
break;*/
#ifdef USE_SERVOS
case SERVO_WRITE:
servos[arg1].setTargetPosition(arg2);
Serial.println("OK");
break;
case SERVO_READ:
Serial.println(servos[arg1].getServo().read());
break;
#endif
#ifdef USE_BASE
case READ_ENCODERS:
Serial.print(readEncoder(LEFT));
Serial.print(" ");
Serial.println(readEncoder(RIGHT));
break;
case RESET_ENCODERS:
resetEncoders();
resetPID();
Serial.println("OK");
break;
case MOTOR_SPEEDS:
/* Reset the auto stop timer */
lastMotorCommand = millis();
if (arg1 == 0 && arg2 == 0) {
setMotorSpeeds(0, 0);
resetPID();
moving = 0;
}
else moving = 1;
leftPID.TargetTicksPerFrame = arg1;
rightPID.TargetTicksPerFrame = arg2;
Serial.println("OK");
break;
case UPDATE_PID:
while ((str = strtok_r(p, ":", &p)) != '\0') {
pid_args[i] = atoi(str);
i++;
}
lKp = pid_args[0];
lKd = pid_args[1];
lKi = pid_args[2];
lKo = pid_args[3];
rKp = pid_args[4];
rKd = pid_args[5];
rKi = pid_args[6];
rKo = pid_args[7];
Serial.println("OK");
break;
#endif
default:
Serial.println("Invalid Command");
break;
}
}
/* Setup function--runs once at startup. */
//記得初始化那些函數
void setup() {
Serial.begin(BAUDRATE);
// Initialize the motor controller if used */
#ifdef USE_BASE
#ifdef ARDUINO_ENC_COUNTER
//set as inputs
DDRD &= ~(1 << LEFT_ENC_PIN_A);
DDRD &= ~(1 << LEFT_ENC_PIN_B);
DDRC &= ~(1 << RIGHT_ENC_PIN_A);
DDRC &= ~(1 << RIGHT_ENC_PIN_B);
//enable pull up resistors
PORTD |= (1 << LEFT_ENC_PIN_A);
PORTD |= (1 << LEFT_ENC_PIN_B);
PORTC |= (1 << RIGHT_ENC_PIN_A);
PORTC |= (1 << RIGHT_ENC_PIN_B);
// tell pin change mask to listen to left encoder pins
PCMSK2 |= (1 << LEFT_ENC_PIN_A) | (1 << LEFT_ENC_PIN_B);
// tell pin change mask to listen to right encoder pins
PCMSK1 |= (1 << RIGHT_ENC_PIN_A) | (1 << RIGHT_ENC_PIN_B);
// enable PCINT1 and PCINT2 interrupt in the general interrupt mask
PCICR |= (1 << PCIE1) | (1 << PCIE2);
#endif
initEncoders();
initMotorController();
resetPID();
#endif
/* Attach servos if used */
#ifdef USE_SERVOS
int i;
for (i = 0; i < N_SERVOS; i++) {
servos[i].initServo(
servoPins[i],
stepDelay[i],
servoInitPosition[i]);
}
#endif
}
/* Enter the main loop. Read and parse input from the serial port
and run any valid commands. Run a PID calculation at the target
interval and check for auto-stop conditions.
*/
void loop() {
while (Serial.available() > 0) {
// Read the next character
chr = Serial.read();
// Terminate a command with a CR
if (chr == 13) {
if (arg == 1) argv1[index] = NULL;
else if (arg == 2) argv2[index] = NULL;
runCommand();
resetCommand();
}
// Use spaces to delimit parts of the command
else if (chr == ' ') {
// Step through the arguments
if (arg == 0) arg = 1;
else if (arg == 1) {
argv1[index] = NULL;
arg = 2;
index = 0;
}
continue;
}
else {
if (arg == 0) {
// The first arg is the single-letter command
cmd = chr;
}
else if (arg == 1) {
// Subsequent arguments can be more than one character
argv1[index] = chr;
index++;
}
else if (arg == 2) {
argv2[index] = chr;
index++;
}
}
}
// If we are using base control, run a PID calculation at the appropriate intervals
#ifdef USE_BASE
if (millis() > nextPID) {
updatePID();
nextPID += PID_INTERVAL;
}
// Check to see if we have exceeded the auto-stop interval
if ((millis() - lastMotorCommand) > AUTO_STOP_INTERVAL) {
;
setMotorSpeeds(0, 0);
moving = 0;
}
#endif
// Sweep servos
#ifdef USE_SERVOS
int i;
for (i = 0; i < N_SERVOS; i++) {
servos[i].doSweep();
}
#endif
}
- encoder_driver.h源碼:
/* *************************************************************
Encoder driver function definitions - by James Nugen
************************************************************ */
long readEncoder(int i);
void resetEncoder(int i);
void resetEncoders();
void initEncoders();//初始化編碼器
void encoderRightISR1();//編碼器中斷服務
void encoderLeftISR1();//編碼器中斷服務
void encoderRightISR();//編碼器中斷服務
void encoderLeftISR();//編碼器中斷服務
- encoder_driver.ino源碼
/* *************************************************************
Encoder definitions
Add an "#ifdef" block to this file to include support for
a particular encoder board or library. Then add the appropriate
#define near the top of the main ROSArduinoBridge.ino file.
************************************************************ */
#ifdef USE_BASE
#include "motor_driver.h"//包含上面的頭文件
#include "commands.h";
/* Wrap the encoder reset function */
volatile long left_enc_pos = 0L;
volatile long right_enc_pos = 0L;
/* 編碼*/
int left_rotate = 0;
int right_rotate = 0;
//初始化編碼器
void initEncoders() {
pinMode(2, INPUT);
pinMode(3, INPUT);
pinMode(19, INPUT);
pinMode(18, INPUT);
attachInterrupt(0, encoderLeftISR, CHANGE);
attachInterrupt(1, encoderLeftISR, CHANGE);
attachInterrupt(4, encoderRightISR, CHANGE);
attachInterrupt(5, encoderRightISR, CHANGE);
}
//編碼器無法分辨正反轉,使用程序進行判斷。這裏的direction(LEFT)爲電機驅動程序塊的
void encoderLeftISR() {
if (direction(LEFT) == BACKWARDS) {
left_enc_pos--;
}
else {
left_enc_pos++;
}
}
void encoderRightISR() {
if (direction(RIGHT) == BACKWARDS) {
right_enc_pos--;
} else {
right_enc_pos++;
}
}
long readEncoder(int i) {
long encVal = 0L;
if (i == LEFT) {
noInterrupts();
encVal = left_enc_pos;
interrupts();
}
else {
noInterrupts();
encVal = right_enc_pos;
interrupts();
}
return encVal;
}
/* Wrap the encoder reset function */
//重置編碼器計數
void resetEncoder(int i) {
if (i == LEFT) {
left_enc_pos = 0L;
return;
} else {
right_enc_pos = 0L;
return;
}
}
/* Wrap the encoder reset function */
//整體封裝
void resetEncoders() {
resetEncoder(LEFT);
resetEncoder(RIGHT);
}
#endif
- diff_controller.h源碼
這裏添加了左右輪的pid算法,適合左右電機差別比較大的情況,pid調整方法請百度。
/* Functions and type-defs for PID control.
Taken mostly from Mike Ferguson's ArbotiX code which lives at:
http://vanadium-ros-pkg.googlecode.com/svn/trunk/arbotix/
*/
/* PID setpoint info For a Motor */
typedef struct {
double TargetTicksPerFrame; // target speed in ticks per frame
long Encoder; // encoder count
long PrevEnc; // last encoder count
/*
* Using previous input (PrevInput) instead of PrevError to avoid derivative kick,
* see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/
*/
int PrevInput; // last input
//int PrevErr; // last error
/*
* Using integrated term (ITerm) instead of integrated error (Ierror),
* to allow tuning changes,
* see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
*/
//int Ierror;
int ITerm; //integrated term
long output; // last motor setting
}
SetPointInfo;
SetPointInfo leftPID, rightPID;
/* PID Parameters */
int lKp = 20;
int lKd = 12;
int lKi = 0;
int lKo = 50;
int rKp = 20;
int rKd = 12;
int rKi = 0;
int rKo = 50;
unsigned char moving = 0; // is the base in motion?
/*
* Initialize PID variables to zero to prevent startup spikes
* when turning PID on to start moving
* In particular, assign both Encoder and PrevEnc the current encoder value
* See http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-initialization/
* Note that the assumption here is that PID is only turned on
* when going from stop to moving, that's why we can init everything on zero.
*/
void resetPID(){
leftPID.TargetTicksPerFrame = 0.0;
leftPID.Encoder = readEncoder(LEFT);
leftPID.PrevEnc = leftPID.Encoder;
leftPID.output = 0;
leftPID.PrevInput = 0;
leftPID.ITerm = 0;
rightPID.TargetTicksPerFrame = 0.0;
rightPID.Encoder = readEncoder(RIGHT);
rightPID.PrevEnc = rightPID.Encoder;
rightPID.output = 0;
rightPID.PrevInput = 0;
rightPID.ITerm = 0;
}
/* PID routine to compute the next motor commands */
void doPIDL(SetPointInfo * p) {
long Perror;
long output;
int input;
//Perror = p->TargetTicksPerFrame - (p->Encoder - p->PrevEnc);
input = p->Encoder - p->PrevEnc;
Perror = p->TargetTicksPerFrame - input;
/*
* Avoid derivative kick and allow tuning changes,
* see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/
* see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
*/
//output = (Kp * Perror + Kd * (Perror - p->PrevErr) + Ki * p->Ierror) / Ko;
// p->PrevErr = Perror;
output = (lKp * Perror - lKd * (input - p->PrevInput) + p->ITerm) / lKo;
p->PrevEnc = p->Encoder;
output += p->output;
// Accumulate Integral error *or* Limit output.
// Stop accumulating when output saturates
if (output >= MAX_PWM)
output = MAX_PWM;
else if (output <= -MAX_PWM)
output = -MAX_PWM;
else
/*
* allow turning changes, see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
*/
p->ITerm += lKi * Perror;
p->output = output;
p->PrevInput = input;
}
void doPIDR(SetPointInfo * p) {
long Perror;
long output;
int input;
//Perror = p->TargetTicksPerFrame - (p->Encoder - p->PrevEnc);
input = p->Encoder - p->PrevEnc;
Perror = p->TargetTicksPerFrame - input;
/*
* Avoid derivative kick and allow tuning changes,
* see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/
* see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
*/
//output = (Kp * Perror + Kd * (Perror - p->PrevErr) + Ki * p->Ierror) / Ko;
// p->PrevErr = Perror;
output = (rKp * Perror - rKd * (input - p->PrevInput) + p->ITerm) / rKo;
p->PrevEnc = p->Encoder;
output += p->output;
// Accumulate Integral error *or* Limit output.
// Stop accumulating when output saturates
if (output >= MAX_PWM)
output = MAX_PWM;
else if (output <= -MAX_PWM)
output = -MAX_PWM;
else
/*
* allow turning changes, see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
*/
p->ITerm += rKi * Perror;
p->output = output;
p->PrevInput = input;
}
/* Read the encoder values and call the PID routine */
void updatePID() {
/* Read the encoders */
leftPID.Encoder = readEncoder(LEFT);
rightPID.Encoder = readEncoder(RIGHT);
/* If we're not moving there is nothing more to do */
if (!moving){
/*
* Reset PIDs once, to prevent startup spikes,
* see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-initialization/
* PrevInput is considered a good proxy to detect
* whether reset has already happened
*/
if (leftPID.PrevInput != 0 || rightPID.PrevInput != 0) resetPID();
return;
}
/* Compute PID update for each motor */
doPIDR(&rightPID);
doPIDL(&leftPID);
/* Set the motor speeds accordingly */
setMotorSpeeds(leftPID.output, rightPID.output);
}
long readPidIn(int i) {
long pidin=0;
if (i == LEFT){
pidin = leftPID.PrevInput;
}else {
pidin = rightPID.PrevInput;
}
return pidin;
}
long readPidOut(int i) {
long pidout=0;
if (i == LEFT){
pidout = leftPID.output;
}else {
pidout = rightPID.output;
}
return pidout;
}
- motor_driver.h源碼
按照對應的格式修改
/***************************************************************
Motor driver function definitions - by James Nugen
*************************************************************/
#ifdef L298_MOTOR_DRIVER
#define RIGHT_MOTOR_BACKWARD 4
#define LEFT_MOTOR_BACKWARD 8
#define RIGHT_MOTOR_FORWARD 5
#define LEFT_MOTOR_FORWARD 9
#endif
//這裏用的不是L298N 只是爲了方便 這裏使用的是藍宙電子的雙路控制板,
//雙PWM輸入差分法
//例如 PWM1 =40 PWM2=0 正轉 PWM1 =0 PWM2=40 反轉
void initMotorController();
void setMotorSpeed(int i, int spd);
void setMotorSpeeds(int leftSpeed, int rightSpeed);
boolean direction(int i);
- motor_driver.ino源碼
/***************************************************************
Motor driver definitions
Add a "#elif defined" block to this file to include support
for a particular motor driver. Then add the appropriate
#define near the top of the main ROSArduinoBridge.ino file.
*************************************************************/
#ifdef USE_BASE
//這裏可以不用看(到下一個註釋)
#ifdef POLOLU_VNH5019
/* Include the Pololu library */
#include "DualVNH5019MotorShield.h"
/* Create the motor driver object */
DualVNH5019MotorShield drive;
/* Wrap the motor driver initialization */
void initMotorController() {
drive.init();
}
/* Wrap the drive motor set speed function */
void setMotorSpeed(int i, int spd) {
if (i == LEFT) drive.setM1Speed(spd);
else drive.setM2Speed(spd);
}
// A convenience function for setting both motor speeds
void setMotorSpeeds(int leftSpeed, int rightSpeed) {
setMotorSpeed(LEFT, leftSpeed);
setMotorSpeed(RIGHT, rightSpeed);
}
#elif defined POLOLU_MC33926
/* Include the Pololu library */
#include "DualMC33926MotorShield.h"
/* Create the motor driver object */
DualMC33926MotorShield drive;
/* Wrap the motor driver initialization */
void initMotorController() {
drive.init();
}
/* Wrap the drive motor set speed function */
void setMotorSpeed(int i, int spd) {
if (i == LEFT) drive.setM1Speed(spd);
else drive.setM2Speed(spd);
}
// A convenience function for setting both motor speeds
void setMotorSpeeds(int leftSpeed, int rightSpeed) {
setMotorSpeed(LEFT, leftSpeed);
setMotorSpeed(RIGHT, rightSpeed);
}
//到這裏不用看
#elif defined L298_MOTOR_DRIVER
boolean directionLeft = false;
boolean directionRight = false;
//和編碼器配合
boolean direction(int i) {
if (i == LEFT) {
return directionLeft;
} else {
return directionRight;
}
}
void initMotorController() {
pinMode(RIGHT_MOTOR_FORWARD, OUTPUT);
pinMode(RIGHT_MOTOR_BACKWARD, OUTPUT);
pinMode(LEFT_MOTOR_FORWARD, OUTPUT);
pinMode(LEFT_MOTOR_BACKWARD, OUTPUT);
}//這是電機的初始化函數,根據驅動板自行修改
void setMotorSpeed(int i, int spd) {
unsigned char reverse = 0;
if (spd < 0)
{
spd = -spd;
reverse = 1;
}
if (spd > 255)
spd = 255;
//這裏是驅動班控制程序,請自行修改
if (i == LEFT) {
if (reverse == 0) {
analogWrite(LEFT_MOTOR_FORWARD, spd);
analogWrite(LEFT_MOTOR_BACKWARD, 0);
directionLEFT = FORWARDS;
}
else if (reverse == 1) {
analogWrite(LEFT_MOTOR_BACKWARD, spd);
analogWrite(LEFT_MOTOR_FORWARD, 0);
directionLEFT = BACKWARDS;
}
}
else {
if (reverse == 0) {
analogWrite(Right_MOTOR_FORWARD, spd);
analogWrite(Right_MOTOR_BACKWARD, 0);
directionRight = FORWARDS;
}
else if (reverse == 1) {
analogWrite(Right_MOTOR_BACKWARD, spd);
analogWrite(Right_MOTOR_FORWARD, 0);
directionRight = BACKWARDS;
}
}//這裏的正反轉需要自己實驗
}
void setMotorSpeeds(int leftSpeed, int rightSpeed) {
setMotorSpeed(LEFT, leftSpeed);
setMotorSpeed(RIGHT, rightSpeed);
}
#else
#error A motor driver must be selected!
#endif
#endif