用Arduino实现霍尔编码减速电机PI调速（增量式）
这个调速算法是用在搬运机器人上的，一方面之前没有用过PID算法，另一方面在Arduino上实现的资料也比较少，所以我想把自己的尝试过程记录下来，如果能帮到别人再好不过了。
1.硬件线路连接
这里我使用的是用到了两个减速比为30的霍尔编码器减速电机，电机驱动为AQMH2407ND，主控采用Arduino Mega 2560.
这是程序中的端口设置，可以看出硬件线路连接情况。
//左电机端口定义
#define MotorLpin1 35 //控制位3 这是电机驱动板上的，用来控制正，反转和停止的。
#define MotorLpin2 37 //控制位4 这是电机驱动板上的
#define MotorLpwm 3 //使能调速 ENB 这是电机驱动板上的PWM调速接口
#define MotorLcountA 18 //编码器A 中断号：5
#define MotorLcountB 19 //编码器B 中断号：4
//右电机端口定义
#define MotorRpin1 31 //控制位1 这是电机驱动板上的
#define MotorRpin2 33 //控制位2 这是电机驱动板上的
#define MotorRpwm 2 //使能调速 ENA 这是电机驱动板上的PWM调速接口
#define MotorRcountA 20 //编码器A 中断号：3
#define MotorRcountB 21 //编码器B 中断号：22.PI增量调速算法
PID算法有位置式和增量式两种，位置式：考虑从开机到现在所有的偏差，增量式：考虑最近偏差。如果对PID算法不了解，可以看一下这个老师的视频：https://www.bilibili.com/video/BV14E411L7GB/
因为这个小车是要多次启动停止的，再加上很多电机调速算法用的都是增量式，所以我采用的也是增量式。并且在实际的调参过程中，PI的效果是比PID要好的，所以综合下来采用的就是增量式PI调速。
3.增量式调速算法的C语言表达
这个我参考的式平衡小车之家的代码，这里用的是-=,不是+=。因为用-=，调试出来的PI参数是负数：
/*********************************************************
* 函数功能：增量式PI控制器(左轮)
* 入口参数：当前速度（编码器测量值），目标速度
* 返回 值：电机PWM
* 参考资料：
* 增量式离散PID公式：
* Pwm-=Kp*[e(k)-e(k-1)]+Ki*e(k)+Kd*[e(k)-2e(k-1)+e(k-2)]
* e(k):本次偏差
* e(k-1):上一次偏差
* e(k-2):上上次偏差
* Pwm:代表增量输出
* 在速度闭环控制系统里面我们只使用PI控制，因此对PID公式可简化为：
* Pwm-=Kp*[e(k)-e(k-1)]+Ki*e(k)
* e(k):本次偏差
* e(k-1):上一次偏差
* Pwm:代表增量输出
*
* 注意增量式PID先调I,再调P,最后再调D
*********************************************************/
int Incremental_Pi_L(int current_speed,int target_speed){
static float pwm,bias,last_bias,prev_bias; //静态变量存在程序全周期：pwm:增量输出,bias:本次偏差,last_bias:上次偏差,prev_bais_:上上次偏差
bias=current_speed-target_speed; //计算本次偏差e(k)
pwm-=(kp*(bias-last_bias)+ki*bias+kd*(bias-2*last_bias+prev_bias)); //增量式PID控制器
prev_bias=last_bias; //保存上上次偏差
last_bias=bias; //保存上一次偏差
//PWM 限幅度 Arduino的PWM 最高为255 限制在250
if(pwm<-250){
pwm=250;
}
if(pwm>250){
pwm=250;
}
//Serial.println(pwm);
return pwm; //增量输出
}4.霍尔编码器的脉冲检测
这里我只用到了每个轮子的A相，线数是13，电机减速比为30，那么输出轴转动一周，A相编码器会输出390脉冲。这里用来测轮子的速度完全够用了。用Arduino的外部中断读取编码器脉冲，
/***********************************
* 电机实际速度计算：
* 公式：
* 已知参数：
* 车轮直径65mm,
* 左边轮子一圈：360脉冲（RISING）,
* 右边轮子一圈：390脉冲（RISING），
* 单位时间读两个轮子脉冲读取两个轮子脉冲
* 已知道参数：
* 车轮直径65mm,
* 左边轮子一圈：360脉冲（RISING）,
* 右边轮子一圈：390脉冲（RISING），
*
* 外部中断触发模式：
*
* LOW：低电平触发;
* CHANGE：电平变化触发;
* RISING ：上升沿触发(由LOW变为HIGH);
* FALLING：下降沿触发(由HIGH变为LOW);
* HIGH：高电平触发(该中断模式仅适用于Arduino due);
*
*
***********************************/
void Read_Moto_V(){
unsigned long nowtime=0;
motorL=0;
motorR=0;
nowtime=millis()+50;//读50毫秒
attachInterrupt(digitalPinToInterrupt(MotorLcountA),Read_Moto_L,RISING);//左轮脉冲开中断计数
attachInterrupt(digitalPinToInterrupt(MotorRcountA),Read_Moto_R,RISING);//右轮脉冲开中断计数
while(millis()
5.PID参数调试整体程序
这个调试过程我参考的是这篇文章：https://blog.csdn.net/C1664510416/article/details/107229958，PID算法的代码是现成的，改改挪到自己的程序中就能用了，但是要考虑到怎样嵌入到自己的程序中，还有就是参数应该怎么调节，如果是第一次用PID，可能你在把PID代码挪到自己程序中的时候，还是一头雾水，但是一旦你调完了参数，就会豁然开朗了。
曲线绘制我直接用的Arduino自带的串口绘图器，主控将实时速度通过Serial.println(V_L)，传到电脑，打开串口绘图器就可以直接看到曲线。
我这里比较麻烦，就是不断的修改PID参数，然后开启电源让主控控制电机转动，打印数据。
增量式PID的调试与位置式不同，要先调I，再调P，再调D，最后再统一微调。
//左电机端口定义
#define MotorLpin1 35 //控制位3
#define MotorLpin2 37 //控制位4
#define MotorLpwm 3 //使能调速 ENB
#define MotorLcountA 18 //编码器A 中断号：5
#define MotorLcountB 19 //编码器B 中断号：4
//右电机端口定义
#define MotorRpin1 31 //控制位1
#define MotorRpin2 33 //控制位2
#define MotorRpwm 2 //使能调速 ENA
#define MotorRcountA 20 //编码器A 中断号：3
#define MotorRcountB 21 //编码器B 中断号：2
volatile float motorL=0;//中断变量，左轮子脉冲计数
volatile float motorR=0;//中断变量，右轮子脉冲计数
float V_L=0; //左轮速度 单位cm/s
float V_R=0; //右边轮速 单位cm/s
int v1=0; //单位cm/s
int v2=0; //单位cm/s
float Target_V_L=20,Target_V_R=20; //单位cm/s
int Pwm_L=0,Pwm_R=0; //左右轮PWM
//PID变量
float kp=0,ki=0,kd=0; //PID参数
/**************************************
* Arduino初始化函数
*
*************************************/
void setup() {
Motor_Init();//电机端口初始化
Serial.begin(9600);//开启串口
}
/*********************************************************
* 函数功能：增量式PI控制器(左轮)
* 入口参数：当前速度（编码器测量值），目标速度
* 返回 值：电机PWM
* 参考资料：
* 增量式离散PID公式：
* Pwm-=Kp*[e(k)-e(k-1)]+Ki*e(k)+Kd*[e(k)-2e(k-1)+e(k-2)]
* e(k):本次偏差
* e(k-1):上一次偏差
* e(k-2):上上次偏差
* Pwm:代表增量输出
* 在速度闭环控制系统里面我们只使用PI控制，因此对PID公式可简化为：
* Pwm-=Kp*[e(k)-e(k-1)]+Ki*e(k)
* e(k):本次偏差
* e(k-1):上一次偏差
* Pwm:代表增量输出
*
* 注意增量式PID先调I,再调P,最后再调D
*********************************************************/
int Incremental_Pi_L(int current_speed,int target_speed){
static float pwm,bias,last_bias,prev_bias; //静态变量存在程序全周期：pwm:增量输出,bias:本次偏差,last_bias:上次偏差,prev_bais_:上上次偏差
bias=current_speed-target_speed; //计算本次偏差e(k)
pwm-=(kp*(bias-last_bias)+ki*bias+kd*(bias-2*last_bias+prev_bias)); //增量式PID控制器
prev_bias=last_bias; //保存上上次偏差
last_bias=bias; //保存上一次偏差
//PWM 限幅度 Arduino的PWM 最高为255 限制在250
if(pwm<-250){
pwm=250;
}
if(pwm>250){
pwm=250;
}
//Serial.println(pwm);
return pwm; //增量输出
}
//右轮速度增量式PID控制器
int Incremental_Pi_R(float current_speed,float target_speed){
static float pwm,bias,last_bias,prev_bias; //静态变量存在程序全周期：pwm:增量输出,bias:本次偏差,last_bias:上次偏差,prev_bais_:上上次偏差
bias=current_speed-target_speed; //计算本次偏差e(k)
pwm-=(kp*(bias-last_bias)+ki*bias+kd*(bias-2*last_bias+prev_bias)); //增量式PID控制器
prev_bias=last_bias; //保存上上次偏差
last_bias=bias; //保存上一次偏差
//PWM 限幅度 Arduino的PWM 最高为255限制在250
if(pwm<-250){
pwm=250;
}
if(pwm>250){
pwm=250;
}
//Serial.println(pwm);
return pwm; //增量输出
}
/**************************************************************************（测试完成）
函数功能：设置双轮工作模式和运动速度
入口参数：工作模式，左右轮pwm
返回 值：无
**************************************************************************/
void Set_Pwm(int mode,int speed_L,int speed_R){
if(mode==1){
//前进模式
//左电机
digitalWrite(MotorLpin1,LOW);
digitalWrite(MotorLpin2,HIGH);
analogWrite(MotorLpwm,speed_L);
//右电机
digitalWrite(MotorRpin1,HIGH);
digitalWrite(MotorRpin2,LOW);
analogWrite(MotorRpwm,speed_R);
}else if(mode==2){
//后退模式
//左电机
digitalWrite(MotorLpin1,HIGH);
digitalWrite(MotorLpin2,LOW);
analogWrite(MotorLpwm,speed_L);
//右电机
digitalWrite(MotorRpin1,LOW);
digitalWrite(MotorRpin2,HIGH);
analogWrite(MotorRpwm,speed_R);
}else if(mode==3){
//左转模式
//左电机
digitalWrite(MotorLpin1,HIGH);
digitalWrite(MotorLpin2,LOW);
analogWrite(MotorLpwm,speed_L);
//右电机
digitalWrite(MotorRpin1,HIGH);
digitalWrite(MotorRpin2,LOW);
analogWrite(MotorRpwm,speed_R);
}else if(mode==4){
//右转模式
//左电机
digitalWrite(MotorLpin1,LOW);
digitalWrite(MotorLpin2,HIGH);
analogWrite(MotorLpwm,speed_L);
//右电机
digitalWrite(MotorRpin1,LOW);
digitalWrite(MotorRpin2,HIGH);
analogWrite(MotorRpwm,speed_R);
}
}
/**************************************************************************（测试完成）
函数功能：电机端口初始化，控制芯片引脚拉低
入口参数：无
返回 值：无
**************************************************************************/
void Motor_Init(){
//左电机
pinMode(MotorLpin1,OUTPUT); //驱动芯片控制引脚
pinMode(MotorLpin2,OUTPUT); //驱动芯片控制引脚
pinMode(MotorLpwm,OUTPUT); //驱动芯片控制引脚，PWM调速
pinMode(MotorLcountA,INPUT); //左轮编码器A引脚
pinMode(MotorLcountB,INPUT); //左轮编码器B引脚
//右电机
pinMode(MotorRpin1,OUTPUT); //驱动芯片控制引脚
pinMode(MotorRpin2,OUTPUT); //驱动芯片控制引脚
pinMode(MotorRpwm,OUTPUT); //驱动芯片控制引脚，PWM调速
pinMode(MotorRcountA,INPUT); //右轮编码器A引脚
pinMode(MotorRcountB,INPUT); //右轮编码器B引脚
//驱动芯片控制引脚全部拉低
digitalWrite(MotorLpin1,LOW); //左电机
digitalWrite(MotorLpin2,LOW);
digitalWrite(MotorLpwm,LOW);
digitalWrite(MotorRpin1,LOW); //右电机
digitalWrite(MotorRpin2,LOW);
digitalWrite(MotorRpwm,LOW);
}
/***********************************
* 电机实际速度计算：
* 公式：
* 已知参数：
* 车轮直径65mm,
* 左边轮子一圈：390脉冲（RISING）,
* 右边轮子一圈：390脉冲（RISING），
* 单位时间读两个轮子脉冲读取两个轮子脉冲
***********************************/
void Read_Moto_V(){
unsigned long nowtime=0;
motorL=0;
motorR=0;
nowtime=millis()+50;//读50毫秒
attachInterrupt(digitalPinToInterrupt(MotorLcountA),Read_Moto_L,RISING);//左轮脉冲开中断计数
attachInterrupt(digitalPinToInterrupt(MotorRcountA),Read_Moto_R,RISING);//右轮脉冲开中断计数
while(millis()6.调参过程
参考：https://blog.csdn.net/C1664510416/article/details/107229958
7.最终结果
kp=1.6,ki=0.8,kd=0;
