时光荏苒，珍惜当下

机器人搭建与调试2：底层控制ros_arduino_bridge修改 arduino篇

发表于 更新于 分类于 Valine：

  使用Arduino开发板作为底层控制（base_controller），有rosserial_arduino和ros_arduino_bridge两种实现方式。其中 rosserial_arduino 将航迹推演、话题发布等都放在Arduino开发板上，占用资源大；而ros_arduino_bridge在Arduino开发板上只用来控制电机、舵机驱动，将计算部分放在ROS从机中，大大节省Arduino开发板占用资源，此外可外接更多传感器。
  本ROS小车选择ros_arduino_bridge作为底层控制（base_controller）。

ros_arduino_bridge 框架结构（arduino篇）

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_bridge 代码修改主要放在encoder_driver、motor_driver、ROSArduinoBridge、commands上，其它如diff_controller等暂时不需要修改。已整理修改后的代码，见 ROS-robot。

安装底层驱动功能包 ros_arduino_bridge

打开虚拟机终端，运行以下命令安装ros_arduino_bridge 源码：

$ cs  ($ cd ~/catkin_ws/src 命令简化)
$ https://github.com/hbrobotics/ros_arduino_bridge
$ cm  ($ cd ~/catkin_ws && catkin_make 命令简化)
（图）ros_arduino_bridge github

  其中Arduino开发板上使用的程序在 /ros_arduino_firmware/src/libraries/ROSArduinoBridge/ 文件夹中，可复制出来，编译修改烧写到Arduino开发板中。

电机驱动代码修改（motor_driver）

引脚连接

6WD扩展版 Arduino开发板
5V 5V
GND GND
SDA SDA
SCL SCL

驱动原理

根据扩展版上PCA9685PW模块控制AM2857电机驱动模块驱动4路520编码电机（非I/O直接驱动）。

（图）以M1 M2电机为例
（图）AM2857电机驱动模块真值表

Adafruit-PWM-Servo-Driver库函数有pwm.setPin()、drive.setPWM()两种驱动方式。本文选择drive.setPWM()，其驱动方式如下：

编号 电机 扩展板引脚 PCA9685 前进 后退
M1 左前电机 AIN2 LED8 setPWM(8, 0, 0); setPWM(8, 0, spd);
AIN1 LED9 setPWM(9, 0, spd); setPWM(9, 0, 0);
M2 左后电机 BIN2 LED10 setPWM(10, 0, 0); setPWM(10, 0, spd);
BIN1 LED11 setPWM(11, 0, spd); setPWM(11, 0, 0);
M5 右前电机 3AIN1 LED14 setPWM(14, 0, spd); setPWM(14, 0, 0);
3AIN2 LED15 setPWM(15, 0, 0); setPWM(15, 0, spd);
M6 右后电机 3BIN1 LED12 setPWM(12, 0, spd); setPWM(12, 0, 0);
3BIN2 LED13 setPWM(13, 0, 0); setPWM(13, 0, spd);

源代码

  由于6WD扩展版PCA9685模块吸流而导致电流小问题，不能直接通过电机驱动模块引脚驱动电机，只能采用PCA9685模块间接驱动电机，PWM可选范围为0-4095，所以MAX_PWM设置为4095。（如果采用Arduino开发板通过电机驱动模块直接驱动电机，PWM引脚范围为 0-255，MAX_PWM则设置为255）

  电机驱动需要I2Cdev、Adafruit-PWM-Servo-Driver两个库。下载后放在Arduino安装目录下libraries文件夹中即可。

motor_driver.h
1
2
3
4
5
6
7
/* *************************************************************
  电机驱动模块AM2857
   ************************************************************ */

void initMotorController();
void setMotorSpeed(int i, int spd);
void setMotorSpeeds(int leftSpeed, int rightSpeed);
motor_driver.ino
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
// 加载I2C和电机驱动库
#include "commands.h";
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
Adafruit_PWMServoDriver drive = Adafruit_PWMServoDriver(0x41);

// 定义最大PWM
#define MAX_PWM  4095

boolean directionLeft = false;
boolean directionRight = false;
  
// 初始化电机控制
 void initMotorController() {
  
     drive.begin();
     drive.setPWMFreq(50);
     
     drive.setPWM(9, 0, 0);
     drive.setPWM(8, 0, 0);
     drive.setPWM(10, 0, 0);
     drive.setPWM(11, 0, 0);
     
     drive.setPWM(12, 0, 0);
     drive.setPWM(13, 0, 0);
     drive.setPWM(15, 0, 0);
     drive.setPWM(14, 0, 0);  
  }

// 确定方向
 boolean direction(int i){
   if(i == LEFT){
      return directionLeft;
   }
   else{
      return directionRight;
   }
}

// PWM控制 
 void setMotorSpeed(int i, int spd) {
    if(spd > MAX_PWM){
      spd = MAX_PWM;
    }
    else if(spd < -MAX_PWM){
      spd = -1 * MAX_PWM;
    }
    
    if (i == LEFT){
        if(spd>=0){
            directionLeft = FORWARDS;
             drive.setPWM(9, 0, spd);
             drive.setPWM(8, 0, 0);
             drive.setPWM(10, 0, 0);
             drive.setPWM(11, 0, spd);
        }else if(spd < 0){
            directionLeft = BACKWARDS;
            spd = -1 * spd; 
             drive.setPWM(9, 0, 0);
             drive.setPWM(8, 0, spd);
             drive.setPWM(10, 0, spd);
             drive.setPWM(11, 0, 0);
        }
    }
    else {
        if(spd>=0){
            directionRight = FORWARDS;
             drive.setPWM(12, 0, spd);
             drive.setPWM(13, 0, 0);
             drive.setPWM(15, 0, 0);
             drive.setPWM(14, 0, spd);	
        }else if(spd<0){
            directionRight = BACKWARDS;
            spd = -1 * spd;
             drive.setPWM(12, 0, 0);
             drive.setPWM(13, 0, spd);
             drive.setPWM(15, 0, spd);
             drive.setPWM(14, 0, 0);
        }
    }
  }
  
//左右两轮电机控制
  void setMotorSpeeds(int leftSpeed, int rightSpeed) {
    setMotorSpeed(LEFT, leftSpeed);
    setMotorSpeed(RIGHT, rightSpeed);
  }

编码计数代码修改（encoder_driver）

引脚连接

6WD扩展版 Arduino开发板
M1B 4
M1A 18
M6B 5
M6A 19

编码计数原理

  测速的编码器是双通道霍尔效应编码器，它包含一个磁栅和磁敏检测电路，输出两个通道正交相位角为90度的方波。该编码器单路每圈脉冲13CPR(Counts Per Revolution，每转脉冲的个数)。一方面由于每圈又可以分一个上升沿和一个下降沿，另一方面该编码器拥有AB双相输出，所以每转一圈，该编码器的AB双相上下沿总共可以输出52CPR。

（图）以M1 M2电机为例

  小车轮子转一圈，还需要考虑减速比1:30（编码器侧转30圈，小车的轮子才转一圈）。所以小车轮子转一圈，可以产生52*30=1560个脉冲信号。这方面计算在ROS从机树莓派上完成。

  本ROS小车使用Arduino mega 2560开发板完成编码计数。Arduino mega 2560开发板有六个外部中断，触发外部中断方式有四种。本文使用下降沿触发外部中断计数，每当触发一次外部中断，就计数一次。

attachInterrupt(interrupt, ISR, mode) 外部中断配置函数
- interrupt: 中断号，不同Arduino开发板中断号不同。
- ISR: 中断处理函数。此函数不带参数，没有返回值。
- mode: 中断触发方式。
    - LOW: 低电平触发。
    - CHANGE:管脚状态改变触发（ 电平变化，高电平变低电平、低电平变高电平）。
    - RISING:上升沿触发。
    - FALLING:下降沿触发。

源代码

encoder_driver.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
/* *************************************************************
   Arduino mega 外部中断：
   中断号 int0  ----- 引脚号 2 
   中断号 int1  ----- 引脚号 3
   中断号 int2  ----- 引脚号 21
   中断号 int3  ----- 引脚号 20
   中断号 int4  ----- 引脚号 19
   中断号 int5  ----- 引脚号 18   
   ************************************************************ */

// 左轮
#define ENC_LEFT_PINA 18
#define ENC_LEFT_PINB 4

// 右轮
#define ENC_RIGHT_PINA 19
#define ENC_RIGHT_PINB 5

   
void initEncoders();
long readEncoder(int i);
void resetEncoders();
encoder_driver.ino
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
#include "motor_driver.h"
#include "encoder_driver.h"
#include "commands.h";

int left_enc_pos = 0L;
int right_enc_pos = 0L;

// 初始化
void initEncoders(){
  pinMode(ENC_LEFT_PINA, INPUT);
  pinMode(ENC_LEFT_PINB, INPUT);
  attachInterrupt(5, encoderLeftISR,FALLING);  
    
  pinMode(ENC_RIGHT_PINA, INPUT);
  pinMode(ENC_RIGHT_PINB, INPUT);
  attachInterrupt(4, encoderRightISR,FALLING);

}

//每次产生中断，根据左轮方向计数器加1或减1
void encoderLeftISR(){
    if(digitalRead(ENC_LEFT_PINB)==HIGH){
      left_enc_pos++;
    }
    else{
      left_enc_pos--;
    }
}

//每次产生中断，根据右轮方向计数器加1或减1
void encoderRightISR(){
    if(digitalRead(ENC_RIGHT_PINB)==HIGH){
      right_enc_pos--;
    }
    else{
      right_enc_pos++;
    }
}

  /* Wrap the encoder reading function */
  long readEncoder(int i) {
    if (i == LEFT) return 0-left_enc_pos;
    else return right_enc_pos;
  }

  /* Wrap the encoder reset function */
  void resetEncoder(int i) {
    if (i == LEFT){
      left_enc_pos=0L;
      return;
    } else { 
      right_enc_pos=0L;
      return;
    }
  }

void resetEncoders() {
  resetEncoder(LEFT);
  resetEncoder(RIGHT);
  }

主代码修改（ros_arduino_bridge）

ros_arduino_bridge.ino
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
// 加载库
#include "Arduino.h"
#include "commands.h"
#include "motor_driver.h"
#include "encoder_driver.h"
#include "diff_controller.h"
#include "mpu6050.h"

// 串口波特率
#define BAUDRATE     57600 

// PID轮询速率 30次每秒
#define PID_RATE     30     // Hz
const int PID_INTERVAL = 1000 / PID_RATE;
unsigned long nextPID = PID_INTERVAL;

// 电机命令执行时间2s
#define AUTO_STOP_INTERVAL 2000
long lastMotorCommand = AUTO_STOP_INTERVAL;

// 定义电机PWM控制范围
#define MAX_PWM        4095

/* Variable initialization */
int arg = 0;
int index = 0;
char chr;
char cmd;
char argv1[32];
char argv2[32];
long arg1;
long arg2;

// 重置命令
void resetCommand() {
  cmd = NULL;
  memset(argv1, 0, sizeof(argv1));
  memset(argv2, 0, sizeof(argv2));
  arg1 = 0;
  arg2 = 0;
  arg = 0;
  index = 0;
}

// 执行命令
int runCommand() {
  int i = 0;
  char *p = argv1;
  char *str;
  int pid_args[8];
  arg1 = atoi(argv1);
  arg2 = atoi(argv2);

  switch (cmd) {
    case GET_BAUDRATE: 
      Serial.println(BAUDRATE);
      break;
    case READ_PIDIN:
      Serial.print(readPidIn(LEFT));
      Serial.print(" ");
      Serial.println(readPidIn(RIGHT));
      break;
    case READ_PIDOUT:
      Serial.print(readPidOut(LEFT));
      Serial.print(" ");
      Serial.println(readPidOut(RIGHT));
      break;
    case PIN_MODE:
      if (arg2 == 0) pinMode(arg1, INPUT);
      else if (arg2 == 1) pinMode(arg1, OUTPUT);
      Serial.println("OK");
      break; 
    case READ_ENCODERS:
      Serial.print(readEncoder(LEFT));
      Serial.print(" ");
      Serial.println(readEncoder(RIGHT));
      break;
    case READ_MPU6050:
      send_imudata();
      break;
    case RESET_ENCODERS:
      resetEncoders();
      resetPID();
      Serial.println("OK");
      break;
    case MOTOR_SPEEDS:
      lastMotorCommand = millis();
      if (arg1 == 0 && arg2 == 0) {
        setMotorSpeeds(0, 0);
        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++;
      }
      left_Kp = pid_args[0];
      left_Kd = pid_args[1];
      left_Ki = pid_args[2];
      left_Ko = pid_args[3];

      right_Kp = pid_args[4];
      right_Kd = pid_args[5];
      right_Ki = pid_args[6];
      right_Ko = pid_args[7];
      Serial.println("OK");
      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;
    default:
      Serial.println("Invalid Command");
      break;
  }
}

// 初始化
void setup() {
  Serial.begin(BAUDRATE);
  initEncoders();
  initMotorController();
  initmpu6050();
  resetPID();
}

// 循环执行
void loop() {

  while (Serial.available() > 0) {
    chr = Serial.read();
    if (chr == 13) {
      if (arg == 1) argv1[index] = NULL;
      else if (arg == 2) argv2[index] = NULL;
      runCommand();
      resetCommand();
    }
    else if (chr == ' ') {
      if (arg == 0) arg = 1;
      else if (arg == 1)  {
        argv1[index] = NULL;
        arg = 2;
        index = 0;
      }
      continue;
    }
    else {
      if (arg == 0) {
        cmd = chr;
      }
      else if (arg == 1) {
        argv1[index] = chr;
        index++;
      }
      else if (arg == 2) {
        argv2[index] = chr;
        index++;
      }
    }
  }

  if (millis() > nextPID) {
    updatePID();
    nextPID += PID_INTERVAL;
  }

  if ((millis() - lastMotorCommand) > AUTO_STOP_INTERVAL) {
    setMotorSpeeds(0, 0);
    moving = 0;
  }
}
commands.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
#define GET_BAUDRATE   'b'
#define READ_ENCODERS  'e'
#define MOTOR_SPEEDS   'm'
#define RESET_ENCODERS 'r'
#define UPDATE_PID     'u'
#define READ_PIDOUT    'f'
#define READ_PIDIN     'i'
#define READ_MPU6050   'z'

#define PIN_MODE       'c'
#define DIGITAL_READ   'd'
#define ANALOG_READ    'a'
#define DIGITAL_WRITE  'w'
#define ANALOG_WRITE   'x'

#define LEFT            0
#define RIGHT           1
#define FORWARDS true
#define BACKWARDS false

PID代码修改（diff_controller）

需要指出的是，它的左右轮的PID用的是同一套Kp,Kd,Ki,Ko， 如果你的两个电机特性差异比较大，就要对这块做下优化。(已添加PID调速部分)

diff_controller.ino
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
// 定义电机PWM控制范围
#define MAX_PWM        4095

/* 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
  int PrevInput;                // last input
  int ITerm;                    //integrated term
  long output;                    // last motor setting
}
SetPointInfo;

SetPointInfo leftPID, rightPID;

/* PID Parameters */
int Kp = 20;
int Kd = 12;
int Ki = 0;
int Ko = 50;

int left_Kp=Kp;
int left_Kd=Kd;
int left_Ki=Ki;
int left_Ko=Ko;

int right_Kp=Kp;
int right_Kd=Kd;
int right_Ki=Ki;
int right_Ko=Ko;

unsigned char moving = 0; // is the base in motion?

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 dorightPID(SetPointInfo * p) {
  long Perror;
  long output;
  int input;

  input = p->Encoder - p->PrevEnc;
  Perror = p->TargetTicksPerFrame - input;
  output = (right_Kp * Perror - right_Kd * (input - p->PrevInput) + p->ITerm) / right_Ko;
  p->PrevEnc = p->Encoder;
  
  output += p->output;
  if (output >= MAX_PWM)
    output = MAX_PWM;
  else if (output <= -MAX_PWM)
    output = -MAX_PWM;
  else
    p->ITerm += Ki * Perror;

  p->output = output;
  p->PrevInput = input;
}

/* PID routine to compute the next motor commands */
void doleftPID(SetPointInfo * p) {
  long Perror;
  long output;
  int input;

  //Perror = p->TargetTicksPerFrame - (p->Encoder - p->PrevEnc);
  input = p->Encoder - p->PrevEnc;
  Perror =p->TargetTicksPerFrame + input;

  output = (left_Kp * Perror - left_Kd * (input - p->PrevInput) + p->ITerm) / left_Ko;
  p->PrevEnc = p->Encoder;

  output += p->output;

  if (output >= MAX_PWM)
    output = MAX_PWM;
  else if (output <= -MAX_PWM)
    output = -MAX_PWM;
  else
    p->ITerm += Ki * Perror;

  p->output = output;
  p->PrevInput = input;
}


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){
    if (leftPID.PrevInput != 0 || rightPID.PrevInput != 0) resetPID();
    return;
  }

  /* Compute PID update for each motor */
  dorightPID(&rightPID);//执行右马达PID
  doleftPID(&leftPID);//执行左马达PID

  /* 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;
}

串口调试

  将修改好的代码烧录进Arduino开发板中。打开串口，设置串口波特率为 57600。

查看编码计数，发送命令 e
清除编码计数，发送命令 r
驱动电机转动，发送命令 m 200 200 （默认设置轮子仅仅移动2秒，你可以通过修改源码中的AUTO_STOP_INTERVAL改变这个值）
其他命令可参考 commands.h 文件

参考

完
+