基于STM32F407VET6开发板通过Arduino进行13线霍尔编码器电机PID调速_stm32f407 arduino

步骤一：硬/软件准备

硬件部分：

1.STM32F407VET6开发板

2.13线霍尔编码器

3.AT8236电机驱动

4.VS code /  Platformio

5.星瞳串口波形显示器

步骤二：硬件线路连接及相关端口定义

 

 相关宏定义：

#include <Arduino.h>
//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()<nowtime); //达到50毫秒关闭中断
  detachInterrupt(digitalPinToInterrupt(Motor_A_countA));//左轮脉冲关中断计数
  detachInterrupt(digitalPinToInterrupt(Motor_B_countA));//右轮脉冲关中断计数
  detachInterrupt(digitalPinToInterrupt(Motor_C_countA));//左轮脉冲关中断计数
  detachInterrupt(digitalPinToInterrupt(Motor_D_countA));//左轮脉冲关中断计数
  V_A=((motorA/390)*10*PI)/0.05;   //单位cm/s
  V_B=((motorB/390)*10*PI)/0.05;   //单位cm/s
  V_C=((motorC/390)*10*PI)/0.05;   //单位cm/s
  V_D=((motorD/390)*10*PI)/0.05;   //单位cm/s
  v1=V_A;
  v2=V_B;
  v3=V_C;
  v4=V_D;
}

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 <Arduino.h>
//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()<nowtime); //达到50毫秒关闭中断
  detachInterrupt(digitalPinToInterrupt(Motor_A_countA));//A轮脉冲关中断计数
  detachInterrupt(digitalPinToInterrupt(Motor_B_countA));//B轮脉冲关中断计数
  detachInterrupt(digitalPinToInterrupt(Motor_C_countA));//C轮脉冲关中断计数
  detachInterrupt(digitalPinToInterrupt(Motor_D_countA));//D轮脉冲关中断计数
  V_A=((motorA/390)*10*PI)/0.05;   //单位cm/s
  V_B=((motorB/390)*10*PI)/0.05;   //单位cm/s
  V_C=((motorC/390)*10*PI)/0.05;   //单位cm/s
  V_D=((motorD/390)*10*PI)/0.05;   //单位cm/s
  v1=V_A;
  v2=V_B;
  v3=V_C;
  v4=V_D;
}
/***************************
 * 中断函数：读A轮脉冲
 *
 **************************/
void Read_Moto_A(){
  motorA++;
}
/**************************
 * 中断函数：读B轮脉冲
 * 
 *************************/
void Read_Moto_B(){
  motorB++;
}
/**************************
 * 中断函数：读B轮脉冲
 * 
 *************************/
void Read_Moto_C(){
  motorC++;
}
/**************************
 * 中断函数：读B轮脉冲
 * 
 *************************/
void Read_Moto_D(){
  motorD++;
}
 
 
//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();
    }  
    }
}

步骤五：具体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.