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