基于STM32F407VET6开发板通过Arduino进行13线霍尔编码器电机PID调速
步骤一：硬/软件准备
硬件部分：
1.STM32F407VET6开发板
2.13线霍尔编码器
3.AT8236电机驱动
4.VS code / Platformio
5.星瞳串口波形显示器
步骤二：硬件线路连接及相关端口定义
相关宏定义：
#include
//A电机端口定义
#define Motor_A_IN1 PE9 //输入1
#define Motor_A_IN2 PE11 //输入2
#define Motor_A_countA PA15 //编码器A
#define Motor_A_countB PB3 //编码器B
//B电机端口定义
#define Motor_B_IN1 PE13 //输入1
#define Motor_B_IN2 PE14 //输入2
#define Motor_B_countA PB4 //编码器A
#define Motor_B_countB PB5 //编码器B
//C电机端口定义
#define Motor_C_IN1 PE5 //输入1
#define Motor_C_IN2 PE6 //输入2
#define Motor_C_countA PA0 //编码器A
#define Motor_C_countB PA1 //编码器B
//D电机端口定义
#define Motor_D_IN1 PB15 //输入1
#define Motor_D_IN2 PB14 //输入2
#define Motor_D_countA PD12 //编码器A
#define Motor_D_countB PD13 //编码器B 步骤三：相关模块函数
1.电机两输入IN1/IN2及编码器AB相中断定时器使能函数
//引脚初始化
void Motor_Init(){
//A电机
pinMode(Motor_A_IN2,OUTPUT); //驱动芯片控制引脚
pinMode(Motor_A_IN1,OUTPUT); //驱动芯片控制引脚，PWM调速
pinMode(Motor_A_countA,INPUT); //编码器A引脚
pinMode(Motor_A_countB,INPUT); //编码器B引脚
//B电机
pinMode(Motor_B_IN2,OUTPUT); //驱动芯片控制引脚
pinMode(Motor_B_IN1,OUTPUT); //驱动芯片控制引脚，PWM调速
pinMode(Motor_B_countA,INPUT); //编码器A引脚
pinMode(Motor_B_countB,INPUT); //编码器B引脚
//C电机
pinMode(Motor_C_IN2,OUTPUT); //驱动芯片控制引脚
pinMode(Motor_C_IN1,OUTPUT); //驱动芯片控制引脚，PWM调速
pinMode(Motor_C_countA,INPUT); //编码器A引脚
pinMode(Motor_C_countB,INPUT); //编码器B引脚
//D电机
pinMode(Motor_D_IN2,OUTPUT); //驱动芯片控制引脚
pinMode(Motor_D_IN1,OUTPUT); //驱动芯片控制引脚，PWM调速
pinMode(Motor_D_countA,INPUT); //编码器A引脚
pinMode(Motor_D_countB,INPUT); //编码器B引脚
//驱动芯片控制引脚全部拉低
digitalWrite(Motor_A_IN2,LOW); //A电机
digitalWrite(Motor_A_IN1,LOW);
digitalWrite(Motor_B_IN2,LOW); //B电机
digitalWrite(Motor_B_IN1,LOW);
digitalWrite(Motor_C_IN2,LOW); //C电机
digitalWrite(Motor_C_IN1,LOW);
digitalWrite(Motor_D_IN2,LOW); //D电机
digitalWrite(Motor_D_IN1,LOW);
}2.增量式PID控制函数（ABCD电机共用）
int Incremental_Pid_A(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; //增量输出
}3.中断电机脉冲读取函数
/***************************
* 中断函数：读A轮脉冲
*
**************************/
void Read_Moto_A(){
motorA++;
}
/**************************
* 中断函数：读B轮脉冲
*
*************************/
void Read_Moto_B(){
motorB++;
}
/**************************
* 中断函数：读C轮脉冲
*
*************************/
void Read_Moto_C(){
motorC++;
}
/**************************
* 中断函数：读D轮脉冲
*
*************************/
void Read_Moto_D(){
motorD++;
}4.电机转速读取函数(cm/s)
/***********************************
* 电机实际速度计算：
* 已知参数：
* 车轮直径100mm,
* AC轮子一圈：13*30=390脉冲（RISING）
* BD轮子一圈：13*30=390脉冲（RISING）
* 单位时间读两个轮子脉冲读取两个轮子脉冲
***********************************/
void Read_Moto_V(){
unsigned long nowtime=0;
motorA=0;
motorB=0;
motorC=0;
motorD=O;
nowtime=millis()+50;//读50毫秒
attachInterrupt(digitalPinToInterrupt(Motor_A_countA),Read_Moto_A,RISING);//左轮脉冲开中断计数
attachInterrupt(digitalPinToInterrupt(Motor_B_countA),Read_Moto_B,RISING);//右轮脉冲开中断计数
attachInterrupt(digitalPinToInterrupt(Motor_C_countA),Read_Moto_C,RISING);//左轮脉冲开中断计数
attachInterrupt(digitalPinToInterrupt(Motor_D_countA),Read_Moto_D,RISING);//左轮脉冲开中断计数
while(millis()5.电机速度控制/模式选择函数函数
/**************************************************************************
函数功能：设置电机工作模式和运动速度
入口参数：工作模式，pwm
返回 值：无
**************************************************************************/
void Set_Pwm(int mode,int speed_A,int speed_B,int speed_C,int speed_D){
if(mode==1){
//前进模式
//A电机
digitalWrite(Motor_A_IN2,LOW);
analogWrite(Motor_A_IN1,speed_A);
//B电机
digitalWrite(Motor_B_IN2,LOW);
analogWrite(Motor_B_IN1,speed_B);
//C电机
digitalWrite(Motor_C_IN2,LOW);
analogWrite(Motor_C_IN1,speed_C);
//D电机
digitalWrite(Motor_D_IN2,LOW);
analogWrite(Motor_D_IN1,speed_D);
}else if(mode==2){
//后退模式
//A电机
digitalWrite(Motor_A_IN2,speed_A);
analogWrite(Motor_A_IN1,LOW);
//B电机
digitalWrite(Motor_B_IN2,speed_B);
analogWrite(Motor_B_IN1,LOW);
//C电机
digitalWrite(Motor_A_IN2,speed_C);
analogWrite(Motor_A_IN1,LOW);
//D电机
digitalWrite(Motor_B_IN2,speed_D);
analogWrite(Motor_B_IN1,LOW);
}else if(mode==3){
//左旋转模式
//A电机
digitalWrite(Motor_A_IN2,speed_A);
analogWrite(Motor_A_IN1,LOW);
//B电机
digitalWrite(Motor_B_IN2,LOW);
analogWrite(Motor_B_IN1,speed_B);
//C电机
digitalWrite(Motor_C_IN2,speed_C);
analogWrite(Motor_C_IN1,LOW);
//D电机
digitalWrite(Motor_D_IN2,LOW);
analogWrite(Motor_D_IN1,speed_D);
}else if(mode==4){
//右旋转模式
//A电机
digitalWrite(Motor_A_IN2,LOW);
analogWrite(Motor_A_IN1,speed_A);
//B电机
digitalWrite(Motor_B_IN2,speed_B);
analogWrite(Motor_B_IN1,LOW);
//C电机
digitalWrite(Motor_C_IN2,LOW);
analogWrite(Motor_C_IN1,speed_C);
//D电机
digitalWrite(Motor_D_IN2,speed_D);
analogWrite(Motor_D_IN1,LOW);
}else if(mode==5){
//左平移模式
//A电机
digitalWrite(Motor_A_IN2,LOW);
analogWrite(Motor_A_IN1,speed_A);
//B电机
digitalWrite(Motor_B_IN2,speed_B);
analogWrite(Motor_B_IN1,LOW);
//C电机
digitalWrite(Motor_C_IN2,speed_C);
analogWrite(Motor_C_IN1,LOW);
//D电机
digitalWrite(Motor_D_IN2,speed_D);
analogWrite(Motor_D_IN1,LOW);
}else if(mode==6){
//右平移模式
//A电机
digitalWrite(Motor_A_IN2,speed_A);
analogWrite(Motor_A_IN1,LOW);
//B电机
digitalWrite(Motor_B_IN2,LOW);
analogWrite(Motor_B_IN1,speed_B);
//C电机
digitalWrite(Motor_C_IN2,LOW);
analogWrite(Motor_C_IN1,speed_C);
//D电机
digitalWrite(Motor_D_IN2,speed_D);
analogWrite(Motor_D_IN1,LOW);
}
}5.PID串口调参函数
//PID串口调试函数
//格式：参数(kp/ki/kp) 数值
//示例：ki 2.1
void PID_test(){
while (Serial.available() > 0) { // 串口收到字符数大于零。
if(Serial.find("kp"))
{
kp = Serial.parseFloat();
}
if(Serial.find("ki"))
{
ki = Serial.parseFloat();
}
if(Serial.find("kd"))
{
kd = Serial.parseFloat();
}
}
}
步骤四：整合相关模块函数
#include
//A电机端口定义
#define Motor_A_IN1 PE9 //输入1
#define Motor_A_IN2 PE11 //输入2
#define Motor_A_countA PA15 //编码器A
#define Motor_A_countB PB3 //编码器B
//B电机端口定义
#define Motor_B_IN1 PE13 //输入1
#define Motor_B_IN2 PE14 //输入2
#define Motor_B_countA PB4 //编码器A
#define Motor_B_countB PB5 //编码器B
//C电机端口定义
#define Motor_C_IN1 PE5 //输入1
#define Motor_C_IN2 PE6 //输入2
#define Motor_C_countA PA0 //编码器A
#define Motor_C_countB PA1 //编码器B
//D电机端口定义
#define Motor_D_IN1 PB14 //输入1
#define Motor_D_IN2 PB15 //输入2
#define Motor_D_countA PD12 //编码器A
#define Motor_D_countB PD13 //编码器B
//定义相关变量
volatile float motorA=0;//中断变量，A轮子脉冲计数
volatile float motorB=0;//中断变量，B轮子脉冲计数
volatile float motorC=0;//中断变量，C轮子脉冲计数
volatile float motorD=0;//中断变量，D轮子脉冲计数
float V_A=0; //A轮速度 单位cm/s
float V_B=0; //B边轮速 单位cm/s
float V_C=0; //C轮速度 单位cm/s
float V_D=0; //D轮速度 单位cm/s
int v1=0; //单位cm/s
int v2=0; //单位cm/s
int v3=0;
int v4=0;
float Target_V_A=40,Target_V_B=40,Target_V_C=40,Target_V_D=40; //单位cm/s 经过测试最大速度为 ：150 cm/s
int Pwm_A=0,Pwm_B=0,Pwm_C=0,Pwm_D=0 ; //左右轮PWM
//PID变量(根据个人情况调试)
float kp=1,ki=0.15,kd=0; //PID参数
//函数声明
void Motor_Init();//电机初始化
void Read_Moto_V();//读取电机速度 单位：cm/s
void Read_Moto_A(); //读取编码器脉冲
void Read_Moto_B();
void Read_Moto_C();
void Read_Moto_D();
void Set_Pwm(int mode,int speed_A,int speed_B,int speed_C,int speed_D);//pwm设置
int Incremental_Pid_A(int current_speed,int target_speed);//pid函数
int Incremental_Pid_B(float current_speed,float target_speed);
int Incremental_Pid_C(float current_speed,float target_speed);
int Incremental_Pid_D(float current_speed,float target_speed);
void PID_test();//离线kp ki kd 调试
/**************************************
* //Arduino初始化函数
*************************************/
void setup() {
Motor_Init();//电机端口初始化
Serial.begin(9600);//开启串口
}
/***************************************
* Arduino主循环
*
***************************************/
void loop() {
Read_Moto_V();//读取脉冲计算速度
Pwm_A=Incremental_Pid_A(V_A,Target_V_A);//A轮PI运算
Pwm_B=Incremental_Pid_B(V_B,Target_V_B);//B轮PI运算
Pwm_C=Incremental_Pid_C(V_C,Target_V_C);//C轮PI运算
Pwm_D=Incremental_Pid_D(V_D,Target_V_D);//D轮PI运算
Serial.print("V_A: ");
Serial.print(V_A);
Serial.print("cm/s, V_B: ");
Serial.print(V_B);
Serial.print("cm/s, V_C: ");
Serial.print(V_C);
Serial.print("cm/s, V_D: ");
Serial.print(V_D);
Serial.println("cm/s");
Set_Pwm(1,Pwm_A,Pwm_B,Pwm_C,Pwm_D); //设置速度
// PID_test();//调试参数
}
/*********************************************************
* 函数功能：增量式PI控制器(A电机)
* 入口参数：当前速度（编码器测量值），目标速度
* 返回 值：电机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_Pid_A(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; //增量输出
}
//B电机速度增量式PID控制器
int Incremental_Pid_B(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; //增量输出
}
//C电机速度增量式PID控制器
int Incremental_Pid_C(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; //增量输出
}
//D电机速度增量式PID控制器
int Incremental_Pid_D(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_A,int speed_B,int speed_C,int speed_D){
if(mode==1){
//前进模式
//A电机
digitalWrite(Motor_A_IN2,LOW);
analogWrite(Motor_A_IN1,speed_A);
//B电机
digitalWrite(Motor_B_IN2,LOW);
analogWrite(Motor_B_IN1,speed_B);
//C电机
digitalWrite(Motor_C_IN2,LOW);
analogWrite(Motor_C_IN1,speed_C);
//D电机
digitalWrite(Motor_D_IN2,LOW);
analogWrite(Motor_D_IN1,speed_D);
}else if(mode==2){
//后退模式
//A电机
digitalWrite(Motor_A_IN2,speed_A);
analogWrite(Motor_A_IN1,LOW);
//B电机
digitalWrite(Motor_B_IN2,speed_B);
analogWrite(Motor_B_IN1,LOW);
//C电机
digitalWrite(Motor_A_IN2,speed_C);
analogWrite(Motor_A_IN1,LOW);
//D电机
digitalWrite(Motor_B_IN2,speed_D);
analogWrite(Motor_B_IN1,LOW);
}else if(mode==3){
//左旋转模式
//A电机
digitalWrite(Motor_A_IN2,speed_A);
analogWrite(Motor_A_IN1,LOW);
//B电机
digitalWrite(Motor_B_IN2,LOW);
analogWrite(Motor_B_IN1,speed_B);
//C电机
digitalWrite(Motor_C_IN2,speed_C);
analogWrite(Motor_C_IN1,LOW);
//D电机
digitalWrite(Motor_D_IN2,LOW);
analogWrite(Motor_D_IN1,speed_D);
}else if(mode==4){
//右旋转模式
//A电机
digitalWrite(Motor_A_IN2,LOW);
analogWrite(Motor_A_IN1,speed_A);
//B电机
digitalWrite(Motor_B_IN2,speed_B);
analogWrite(Motor_B_IN1,LOW);
//C电机
digitalWrite(Motor_C_IN2,LOW);
analogWrite(Motor_C_IN1,speed_C);
//D电机
digitalWrite(Motor_D_IN2,speed_D);
analogWrite(Motor_D_IN1,LOW);
}else if(mode==5){
//左平移模式
//A电机
digitalWrite(Motor_A_IN2,LOW);
analogWrite(Motor_A_IN1,speed_A);
//B电机
digitalWrite(Motor_B_IN2,speed_B);
analogWrite(Motor_B_IN1,LOW);
//C电机
digitalWrite(Motor_C_IN2,speed_C);
analogWrite(Motor_C_IN1,LOW);
//D电机
digitalWrite(Motor_D_IN2,speed_D);
analogWrite(Motor_D_IN1,LOW);
}else if(mode==6){
//右平移模式
//A电机
digitalWrite(Motor_A_IN2,speed_A);
analogWrite(Motor_A_IN1,LOW);
//B电机
digitalWrite(Motor_B_IN2,LOW);
analogWrite(Motor_B_IN1,speed_B);
//C电机
digitalWrite(Motor_C_IN2,LOW);
analogWrite(Motor_C_IN1,speed_C);
//D电机
digitalWrite(Motor_D_IN2,speed_D);
analogWrite(Motor_D_IN1,LOW);
}
}
//引脚初始化
void Motor_Init(){
//A电机
pinMode(Motor_A_IN2,OUTPUT); //驱动芯片控制引脚
pinMode(Motor_A_IN1,OUTPUT); //驱动芯片控制引脚，PWM调速
pinMode(Motor_A_countA,INPUT); //编码器A引脚
pinMode(Motor_A_countB,INPUT); //编码器B引脚
//B电机
pinMode(Motor_B_IN2,OUTPUT); //驱动芯片控制引脚
pinMode(Motor_B_IN1,OUTPUT); //驱动芯片控制引脚，PWM调速
pinMode(Motor_B_countA,INPUT); //编码器A引脚
pinMode(Motor_B_countB,INPUT); //编码器B引脚
//C电机
pinMode(Motor_C_IN2,OUTPUT); //驱动芯片控制引脚
pinMode(Motor_C_IN1,OUTPUT); //驱动芯片控制引脚，PWM调速
pinMode(Motor_C_countA,INPUT); //编码器A引脚
pinMode(Motor_C_countB,INPUT); //编码器B引脚
//D电机
pinMode(Motor_D_IN2,OUTPUT); //驱动芯片控制引脚
pinMode(Motor_D_IN1,OUTPUT); //驱动芯片控制引脚，PWM调速
pinMode(Motor_D_countA,INPUT); //编码器A引脚
pinMode(Motor_D_countB,INPUT); //编码器B引脚
//驱动芯片控制引脚全部拉低
digitalWrite(Motor_A_IN2,LOW); //A电机
digitalWrite(Motor_A_IN1,LOW);
digitalWrite(Motor_B_IN2,LOW); //B电机
digitalWrite(Motor_B_IN1,LOW);
digitalWrite(Motor_C_IN2,LOW); //C电机
digitalWrite(Motor_C_IN1,LOW);
digitalWrite(Motor_D_IN2,LOW); //D电机
digitalWrite(Motor_D_IN1,LOW);
}
/***********************************
* 电机实际速度计算：
* 已知参数：
* 车轮直径100mm,
* 左边轮子一圈：13*30=390脉冲（RISING）
* 右边轮子一圈：13*30=390脉冲（RISING）
* 单位时间读两个轮子脉冲读取两个轮子脉冲
***********************************/
void Read_Moto_V(){
unsigned long nowtime=0;
motorA=0;
motorB=0;
motorC=0;
motorD=0;
nowtime=millis()+50;//读50毫秒
attachInterrupt(digitalPinToInterrupt(Motor_A_countA),Read_Moto_A,RISING);//A轮脉冲开中断计数
attachInterrupt(digitalPinToInterrupt(Motor_B_countA),Read_Moto_B,RISING);//B轮脉冲开中断计数
attachInterrupt(digitalPinToInterrupt(Motor_C_countA),Read_Moto_C,RISING);//C轮脉冲开中断计数
attachInterrupt(digitalPinToInterrupt(Motor_D_countA),Read_Moto_D,RISING);//D轮脉冲开中断计数
while(millis() 0) { // 串口收到字符数大于零。
if(Serial.find("kp"))
{
kp = Serial.parseFloat();
}
if(Serial.find("ki"))
{
ki = Serial.parseFloat();
}
if(Serial.find("kd"))
{
kd = Serial.parseFloat();
}
}
}
步骤五：具体PID调参过程(雾)
本人pid调参过程极其痛苦，很久才发现方向错误，所以本人不想经历第二次调参过程，所以没有相应过程图片，可以参考一下最后调参完成的波形：
不过下面给大家一些过来人的建议：
对于增量式pid调参，尤其是对于13线霍尔编码器，首先调ki，尽量选择10附近的值采用二分法测量，测量过程对于变化较大的波形应及时舍去，当波形满足略微振荡，在调整时间t_s内调整次数在3-5次可以确保调整时间t_s较小，而最大超调量M_p也较小。接着调节kp，这个主要影响其稳定性，可以从10左右的ki开始下调，直至M_p基本等于零，这样就基本完成pi调试，最后kd我的结果为0，查阅相关资料后，发现大部分情况低精度速度增量编码器很难会用到kd，所以大家参考我这种应用场景就可以忽略。
最后祝大家早日找到属于你的kp和ki！（完结撒花）
步骤六：特别鸣谢up
cbirdfly.