内容学自：赵虚左老师。学后总结！
实现机器人移动的一种策略是：控制系统会先发布预期的车辆速度信息，然后驱动系统订阅到该信息，不断调整电机转速直至达到预期速度，调速过程中还需要时时获取实际速度并反馈给控制系统，控制系统会计算实际位移并生成里程计信息。
控制系统(ROS端)其实就是典型的发布和订阅实现，而具体到驱动系统(Arduino)层面，需要解决的问题有如下几点：带着问题学更有目标性!
1.Arduino 如何订阅控制系统发布的速度相关信息？
2.Arduino 如何发布实际速度相关信息到控制系统？
3.Arduino 如何驱动电机(正传、反转)？
4.Arduino 如何实现电机测速？
5.Arduino 如何实现电机调速？一、硬件部分
1.1 直流减速电机
需要简单了解电机类型、机械结构以及各项参数，这些是和机器人的负载、极限速度、测速结果等休戚相关的。
电机主体通过输入轴与减速箱相连接，通过减速箱的减速效果，最终外端的输出轴会按照比例(取决于减速箱减速比)降低电机输入轴的转速，当然速度降低之后，将提升电机的力矩。
尾部是AB相霍尔编码器，通过AB编码器输出的波形图，可以判断电机的转向以及计算电机转速。
需要关注的参数：
1.额定扭矩: 额定扭矩和机器人质量以及有效负荷相关，二者正比例相关，额定扭矩越大，可支持的机器人质量以及有效负荷越高；
2.减速比: 电机输入轴与输出轴的减速比例，比如: 减速比为90，意味着电机主体旋转90圈，输出轴旋转1圈。3.减速后转速: 与减速比相关，是电机减速箱输出轴的转速，单位是 rpm(转/分)减速后转速与减速前转速存在转换关系: 减速后转速 = 减速前转速 / 减速比。另外，可以根据官方给定的额定功率下的减速后转速结合车轮参数来确定小车最大速度。4.编码器精度:是指编码器旋转一圈单相(当前编码器有AB两相)输出的脉冲数；5.注意:电机输入轴旋转一圈的同时，编码器旋转一圈.如果输出轴旋转一圈，那么编码器的旋转圈数和减速比一致(比如减速比是90，那么输出轴旋转一圈，编码器旋转90圈)。6.编码器输出的脉冲数计算公式则是: 输出轴旋转一圈产生的脉冲数 = 减速比 * 编码器旋转一圈发送的脉冲数(比如:减速比为90，编码器旋转一圈输出11个脉冲，那么输出轴旋转一圈总共产生 11 * 90 也即990个脉冲)。1.2 电机编码器
M1: 电机电源+(和M2对调可以正反转 )
GND: 编码器电源-
C2: 信号线
C1: 信号线
VCC:编码器电源+
M2: 电机电源-(和M1对调可以正反转)1.3 电机驱动板
电机驱动板可选型号较多，比如:TB6612、L298N、L298P....但是这些电机驱动板与电机相连时，需要使用杜邦线，接线会显得凌乱，下面是一款基于L298P优化的电机驱动板，该驱动板可以使用端子线直接连接电机，接线更规整、美观。
端子线母头对应的引脚(自上而下)
母头1: 4、地线、21、20、5V输入、 5
母头2: 7、地线、18、19、5V输入、 6PS:电机驱动板使用时，需要打开USB接口处的电源开关。二、电机基本控制
实现：控制单个电机转动，先控制电机以某个速率正向转动N秒，再让电机停止N秒，再控制电机以某个速率逆向转动N秒，最后让电机停止N秒，如此循环。
接线：
左电机的M1与M2对应的是引脚4(DIRA)和引脚5(PWMA)，引脚4控制转向，引脚5输出PWM。
右电机的M1与M2对应的是引脚6(PWMB)和引脚7(DIRB)，引脚7控制转向，引脚6输出PWM。代码：
/** 电机转动控制* 1.定义接线中电机对应的引脚* 2.setup 中设置引脚为输出模式* 3.loop中控制电机转动*/int DIRA = 4;
int PWMA = 5;void setup() {//两个引脚都设置为 OUTPUTpinMode(DIRA,OUTPUT);pinMode(PWMA,OUTPUT);
}void loop() {//先正向转动3秒digitalWrite(DIRA,HIGH);analogWrite(PWMA,100);delay(3000);//停止3秒digitalWrite(DIRA,HIGH);analogWrite(PWMA,0);delay(3000);//再反向转动3秒digitalWrite(DIRA,LOW);analogWrite(PWMA,100);delay(3000);//停止3秒digitalWrite(DIRA,LOW);analogWrite(PWMA,0);delay(3000);/** 注意: * 1.可以通过将DIRA设置为HIGH或LOW来控制电机转向，但是哪个标志位正转或反转需要根据需求判断，转向是相对的。* 2.PWM的取值为 [0,255],该值可自己设置。* */
}三、电机测速
3.1 编码器测速原理
使用 AB相增量式编码器测速。
原理：AB相编码器主要构成为A相与B相，每一相每转过单位的角度就发出一个脉冲信号(一圈可以发出N个脉冲信号)，A相、B相为相互延迟1/4周期的脉冲输出，根据延迟关系可以区别正反转，而且通过取A相、B相的上升和下降沿可以进行单频或2倍频或4倍频测速。
3.2 测速举例
假设编码器旋转1圈输出11个脉冲，减速比为 90。伪代码如下：
单频计数:
//设置一个计数器
int count = 0;
//当A为上升沿时
if(B为高电平){count++;
}else {count--;
}
//....
//速度=单位时间内统计的脉冲的个数 / （11*90） / 单位时间
2倍频计数:
//设置一个计数器
int count = 0;
//当A为上升沿时
if(B为高电平){count++;
}else {count--;
}
//当A为下降沿时
if(B为低电平){count++;
}else {count--;
}//....
//速度=单位时间内统计的脉冲的个数 / （11*2*90） / 单位时间
4倍频计数:
//设置一个计数器
int count = 0;
//当A为上升沿时
if(B为高电平){count++;
}else {count--;
}
//当A为下降沿时
if(B为低电平){count++;
}else {count--;
}
//当B为上升沿时
if(A为低电平){count++;
} else {count--;
}
//当B为下降沿时
if(A为高电平){count++;
} else {count--;
}
//....
//速度=单位时间内统计的脉冲的个数 / （11*4*90） / 单位时间3.3 案例实现
案例：统计并输出电机转速。
思路：先统计单位时间内以单频或2倍频或4倍频的方式统计脉冲数，再除以一圈对应的脉冲数，最后再除以时间所得即为电机转速。
核心：计数时，需要在A相或B相的上升沿或下降沿触发时，实现计数，在此需要使用中断引脚与中断函数。
Arduino Mega 2560 的中断引脚:2 (interrupt 0), 3 (interrupt 1),18 (interrupt 5), 19 (interrupt 4), 20 (interrupt 3), 21 (interrupt 2)
3.3.1 编码实现脉冲统计
/** 测速实现:* 阶段1:脉冲数统计* 阶段2:速度计算* * 阶段1:* 1.定义所使用的中断引脚,以及计数器(使用 volatile 修饰)* 2.setup 中设置波特率，将引脚设置为输入模式* 3.使用 attachInterupt() 函数为引脚添加中断出发时机以及中断函数* 4.中断函数编写计算算法，并打印* A.单频统计只需要统计单相上升沿或下降沿* B.2倍频统计需要统计单相的上升沿和下降沿* C.4倍频统计需要统计两相的上升沿和下降沿* 5.上传并查看结果 */
int motor_A = 21;//中端口是2
int motor_B = 20;//中断口是3
volatile int count = 0;//如果是正转，那么每计数一次自增1，如果是反转，那么每计数一次自减1 void count_A(){//单频计数实现//手动旋转电机一圈，输出结果为 一圈脉冲数 * 减速比/*if(digitalRead(motor_A) == HIGH){if(digitalRead(motor_B) == LOW){//A 高 B 低count++; } else {//A 高 B 高count--; }}*///2倍频计数实现//手动旋转电机一圈，输出结果为 一圈脉冲数 * 减速比 * 2if(digitalRead(motor_A) == HIGH){if(digitalRead(motor_B) == HIGH){//A 高 B 高count++; } else {//A 高 B 低count--; }} else {if(digitalRead(motor_B) == LOW){//A 低 B 低count++; } else {//A 低 B 高count--; } }
}//与A实现类似
//4倍频计数实现
//手动旋转电机一圈，输出结果为 一圈脉冲数 * 减速比 * 4
void count_B(){if(digitalRead(motor_B) == HIGH){if(digitalRead(motor_A) == LOW){//B 高 A 低count++;} else {//B 高 A 高count--;}} else {if(digitalRead(motor_A) == HIGH){//B 低 A 高count++;} else {//B 低 A 低count--;}}
}void setup() {Serial.begin(57600);//设置波特率 pinMode(motor_A,INPUT);pinMode(motor_B,INPUT);attachInterrupt(2,count_A,CHANGE);//当电平发生改变时触发中断函数//四倍频统计需要为B相也添加中断attachInterrupt(3,count_B,CHANGE);
}void loop() {//测试计数器输出delay(2000);Serial.println(count);
}3.3.2 转速计算
需要定义一个开始时间(用于记录每个测速周期的开始时刻)，还需要定义一个时间区间(比如50毫秒)，时时获取当前时刻，当当前时刻 - 上传结束时刻 >= 时间区间时，就获取当前计数并根据测速公式计算时时速度，计算完毕，计数器归零，重置开始时间。
核心知识点：当使用中断函数中的变量时，需要先禁止中断noInterrupts()，调用完毕，再重启中断interrupts()。
int reducation = 90;//减速比，根据电机参数设置，比如 15 | 30 | 60
int pulse = 11; //编码器旋转一圈产生的脉冲数该值需要参考商家电机参数
int per_round = pulse * reducation * 4;//车轮旋转一圈产生的脉冲数
long start_time = millis();//一个计算周期的开始时刻，初始值为 millis();
long interval_time = 50;//一个计算周期 50ms
double current_vel;//获取当前转速的函数
void get_current_vel(){long right_now = millis(); long past_time = right_now - start_time;//计算逝去的时间if(past_time >= interval_time){//如果逝去时间大于等于一个计算周期//1.禁止中断noInterrupts();//2.计算转速 转速单位可以是秒，也可以是分钟... 自定义即可current_vel = (double)count / per_round / past_time * 1000 * 60;//3.重置计数器count = 0;//4.重置开始时间start_time = right_now;//5.重启中断interrupts();Serial.println(current_vel);}
}void loop() {delay(10);get_current_vel();}四、电机调速（PID）
4.1 PID控制原理
PID算法是一种经典、简单、高效的动态速度调节方式，P代表比例，I代表积分，D代表微分。
e(t) 作为 PID 控制的输入；
u(t) 作为 PID 控制器的输出和被控对象的输入；
Kp 控制器的比例系数；
Ki 控制器的积分时间，也称积分系数；
Kd 控制器的微分时间，也称微分系数。
P
如果实现上述场景中的车速控制，一种简单的实现方式是: 确定目标速度，获取当前速度，使用
(目标速度-当前速度)*某一系数计算结果为输出的PWM，再获取当前速度，使用(目标速度-当前速度)*某一系数计算结果为输出的PWM并输出...如此循环在上述模型中，调速实现是一个闭环，每一次循环都会根据当前时速与目标时速的差值，再乘以以固定系数，计算出需要输出的PWM值，这其中的系数，称之为比例。
I
上述模型算法中，最终速度与预期速度存在稳态误差，这意味着最终结果可能永远无法达成预期，解决的方法就是使用积分I。每次调速时，输出的PWM还要累加根据积分I计算的结果，以消除静态误差。
D
当I值设置的过大时，可能会出先"超速"的情况，超速之后可能需要多次调整，产生系统震荡，解决这种情况可以使用D微分，当速度越是接近目标速度时，D就会越施加反方向力，减弱P的控制，起到类似”阻尼“的作用。通过D的使用可以减小系统震荡。
综上，PID闭环控制实现是结合了比例、积分和微分的一种控制机制，通过P可以以比例的方式计算输出，通过I可以消除稳态误差，通过D可以减小系统震荡，三者相结合，最终是要快速、精准且稳定的达成预期结果，而要实现该结果，还需要对这三个数值反复测试、调整...
4.2 PID控制实现
在Arduino中PID算法已经被封装了，直接整合调用即可，从而提高程序的安全性与开发效率。该库是：Arduino-PID-Library。
案例：通过PID控制电机转速，预期转速为 80r/m。
4.2.1 添加Arduino-PID-Library
首先在 GitHub 下载 PID 库:
git clone GitHub - br3ttb/Arduino-PID-Library然后将该文件夹移动到 arduino 的 libraries下:
sudo cp -r Arduino-PID-Library /home/用户xxx/Arduino/libraries还要重命名文件夹:
sudo mv Arduino-PID-Library ArduinoPIDLibrary最后重启 ArduinoIDE，即可！
4.2.2 代码
PID调速中，测速是实现闭环的关键实现，所以需要复制之前的电机控制代码以及测速代码。
/** PID 调速实现:* 1.代码准备，复制并修改电机控制以及测速代码* 2.包含PID头文件* 3.创建PID对象* 4.在setup中启用自动调试* 5.调试并更新PWM*/#include <PID_v1.h>int DIRA = 4;
int PWMA = 5;int motor_A = 21; // 中端口是2
int motor_B = 20; // 中断口是3
volatile int count = 0; // 如果是正转，那么每计数一次自增1，如果是反转，那么每计数一次自减1void count_A()
{// 单频计数实现// 手动旋转电机一圈，输出结果为 一圈脉冲数 * 减速比/*if(digitalRead(motor_A) == HIGH){if(digitalRead(motor_B) == LOW){//A 高 B 低count++;} else {//A 高 B 高count--;}}*/// 2倍频计数实现// 手动旋转电机一圈，输出结果为 一圈脉冲数 * 减速比 * 2if (digitalRead(motor_A) == HIGH){if (digitalRead(motor_B) == HIGH){ // A 高 B 高count++;}else{ // A 高 B 低count--;}}else{if (digitalRead(motor_B) == LOW){ // A 低 B 低count++;}else{ // A 低 B 高count--;}}
}// 与A实现类似
// 4倍频计数实现
// 手动旋转电机一圈，输出结果为 一圈脉冲数 * 减速比 * 4
void count_B()
{if (digitalRead(motor_B) == HIGH){if (digitalRead(motor_A) == LOW){ // B 高 A 低count++;}else{ // B 高 A 高count--;}}else{if (digitalRead(motor_A) == HIGH){ // B 低 A 高count++;}else{ // B 低 A 低count--;}}
}int reducation = 90; // 减速比，根据电机参数设置，比如 15 | 30 | 60
int pulse = 11; // 编码器旋转一圈产生的脉冲数该值需要参考商家电机参数
int per_round = pulse * reducation * 4; // 车轮旋转一圈产生的脉冲数
long start_time = millis(); // 一个计算周期的开始时刻，初始值为 millis();
long interval_time = 50; // 一个计算周期 50ms
double current_vel;// 获取当前转速的函数
void get_current_vel()
{long right_now = millis();long past_time = right_now - start_time; // 计算逝去的时间if (past_time >= interval_time){ // 如果逝去时间大于等于一个计算周期// 1.禁止中断noInterrupts();// 2.计算转速 转速单位可以是秒，也可以是分钟... 自定义即可current_vel = (double)count / per_round / past_time * 1000 * 60;// 3.重置计数器count = 0;// 4.重置开始时间start_time = right_now;// 5.重启中断interrupts();Serial.println(current_vel);}
}//-------------------------------------PID-------------------------------------------
// 创建 PID 对象
// 1.当前转速 2.计算输出的pwm 3.目标转速 4.kp 5.ki 6.kd 7.当输入与目标值出现偏差时，向哪个方向控制
double pwm; // 电机驱动的PWM值
double target = 80;
double kp = 1.5, ki = 3.0, kd = 0.1;
PID pid(¤t_vel, &pwm, &target, kp, ki, kd, DIRECT);// 速度更新函数
void update_vel()
{// 获取当前速度get_current_vel();pid.Compute(); // 计算需要输出的PWMdigitalWrite(DIRA, HIGH);analogWrite(PWMA, pwm);
}void setup()
{Serial.begin(57600); // 设置波特率pinMode(18, INPUT);pinMode(19, INPUT);// 两个电机驱动引脚都设置为 OUTPUTpinMode(DIRA, OUTPUT);pinMode(PWMA, OUTPUT);attachInterrupt(2, count_A, CHANGE); // 当电平发生改变时触发中断函数// 四倍频统计需要为B相也添加中断attachInterrupt(3, count_B, CHANGE);pid.SetMode(AUTOMATIC);
}void loop()
{delay(10);update_vel();
}4.2.3 核心代码解释
1.包含PID头文件
#include <PID_v1.h>
2.创建PID对象
//创建 PID 对象
//1.当前转速 2.计算输出的pwm 3.目标转速 4.kp 5.ki 6.kd 7.当输入与目标值出现偏差时，向哪个方向控制
double pwm;//电机驱动的PWM值
double target = 120;
double kp=1.5, ki=3.0, kd=0.1;
PID pid(¤t_vel,&pwm,&target,kp,ki,kd,DIRECT);
3.setup中启用PID自动控制
pid.SetMode(AUTOMATIC);
4.计算输出值
pid.Compute();4.2.4 PID调试
PID控制的最终预期结果，是要快速、精准、稳定的达成预期结果，P主要用于控制响应速度，I主要用于控制精度，D主要用于减小震荡增强系统稳定性，三者的取值是需要反复调试的，调试过程中需要查看系统的响应曲线，根据响应曲线以确定合适的PID值。
在 Arduino 中响应曲线的查看可以借助于 Serial.println() 将结果输出，然后再选择菜单栏的工具下串口绘图器以图形化的方式显示响应结果:
☆PID调试技巧:
参数整定找最佳，从小到大顺序查
先是比例后积分，最后再专把微分加
曲线振属荡很频繁，比例度盘要放大
曲线漂浮绕大湾，比例度盘往小扳
曲线偏离回复慢，积分时间往下降
曲线波动周期长，积分时间再加长
曲线振荡频率快，先把微分降下来
动差大来波动慢。微分时间应加长
理想曲线两个波，前高后低4比1
一看二调多分析，调节质量不会低
