[Production of balance trolley] (6) Programming of positional PID, vertical loop and speed loop (super detailed explanation)

  Hello everyone, I am Xiaozheng. In this article, I will give a detailed explanation on the programming of positional PID algorithm , vertical loop , speed loop, etc., so that each friend can have a clearer understanding of the programming logic of these three concepts.

1. Upright ring (PD controller)

1. Chinese formula
 upright ring output = Kp1 × angle deviation + Kd × differential of
 angle deviation // angle deviation = real angle-expected angle

2. English formula
 vertical loop PD controller: Kp×Ek+Kd×Ek_D
 (Ek: angle deviation; Ek_D: differential of angle deviation)

• Ek=true angle-expected angle (Angle-Med, measured by gyroscope MPU6050)
• Ek_D=True angular velocity (gyro_Y, measured by the gyroscope MPU6050)

3. Software programming is
  based on theoretical formulas for software programming. I believe this code should be clearer and easier to understand after reading the above explanation.

/*****************  
直立环PD控制器：Kp*Ek+Kd*Ek_D
入口：Med:机械中值(期望角度)，Angle:真实角度，gyro_Y:真实角速度
出口：直立环输出
******************/
int Vertical(float Med,float Angle,float gyro_Y) 
{
    
    
  int PWM_out;
  
  PWM_out = Vertical_Kp*(Angle-Med)+Vertical_Kd*(gyro_Y-0);
  
  return PWM_out;
} 

2. Speed ​​loop (PI controller)

1. Chinese official

 Speed ​​loop output = Kp2 × motor speed deviation + Ki2 × integral of
 motor speed deviation // motor speed deviation = real speed-desired speed

2. English official

 Speed ​​loop PI controller: Kp×Ek+Ki×Ek_S
 (Ek: motor speed deviation; Ek_S: integral of motor speed deviation)

• Ek=real speed-desired speed (real speed: left motor speed + right motor speed; desired speed: 0)
• Ek_S=accumulation of speed deviation

3. Low-pass filtering

  During this period, low-frequency filtering is needed. We use the vertical ring as the main and the speed ring as a supplement. The speed ring is a disturbance to the vertical ring, and the ultimate goal is to stand upright. The low-frequency filtering function is to make the waveform smoother, filter out high-frequency interference, and prevent excessive speed from affecting the normal operation of the vertical ring.

4. Integral width

  Through comparison, the points are limited to change within the specified range and cannot be exceeded.

5. Software programming

  According to the theoretical formula for software programming, I believe this code should be clear and easy to understand after reading the above explanation.

/*****************  
速度环PI控制器：Kp*Ek+Ki*Ek_S(Ek_S：偏差的积分)
******************/
int Velocity(int Target,int encoder_left,int encoder_right)
{
    
    
  // 定义成静态变量，保存在静态存储器，使得变量不丢掉
  static int PWM_out,Encoder_Err,Encoder_S,EnC_Err_Lowout,EnC_Err_Lowout_last;
  float a=0.7;
  
  // 1.计算速度偏差
  //舍去误差--我的理解：能够让速度为"0"的角度，就是机械中值。
  Encoder_Err = ((encoder_left+encoder_right)-Target);
  
  // 2.对速度偏差进行低通滤波
  // low_out = (1-a)*Ek+a*low_out_last
  EnC_Err_Lowout = (1-a)*Encoder_Err + a*EnC_Err_Lowout_last; // 使得波形更加平滑，滤除高频干扰，放置速度突变
  EnC_Err_Lowout_last = EnC_Err_Lowout;   // 防止速度过大影响直立环的正常工作
  
  // 3.对速度偏差积分出位移
  Encoder_S+=EnC_Err_Lowout;
  
  // 4.积分限幅
  Encoder_S=Encoder_S>10000?10000:(Encoder_S<(-10000)?(-10000):Encoder_S);
  
  // 5.速度环控制输出
  PWM_out = Velocity_Kp*EnC_Err_Lowout+Velocity_Ki*Encoder_S;
  
  return PWM_out;
}

Three, steering ring

1. Chinese official

 Steering ring output = coefficient × Z-axis angular velocity
 (Z-axis angular velocity is measured by the gyroscope MPU6050)

2. Software programming

  The programming of the steering ring is relatively simple, we only need to set a parameter to adjust the angular velocity of the Z axis.

/*****************  
转向环：系数*Z轴角速度
******************/
int Turn(int gyro_Z)
{
    
    
  int PWM_out;
  
  PWM_out = (-0.6)*gyro_Z;
  
  return PWM_out;
}

Four, control function

1. Collect encoder data and MPU6050 angle information

•  Encoder data: left motor speed, right motor speed
  (the two motors are installed relative to each other, exactly 180 degrees apart, in order to have the same encoder output polarity, you need to reverse one of them)
•  MPU6050 data: angle data, angular velocity data, angular acceleration data

2. Press the data into the closed-loop control and calculate the control output

•  Vertical ring output
•  Speed ​​loop output
•  Steering ring output

3. Load the control output to the motor to complete the final control

•  Left motor output (encoder is placed relative to each other)
•  Right motor output
•  Limiting
•  Assignment

4. The control interrupt function
 must first determine whether an interrupt request is accepted, that is, whether the ANT pin of the MPU6050 is at a low level (that is, an interrupt occurs), and then clear the interrupt flag bit, and proceed to the next three steps (that is, the above three Steps).

void EXTI9_5_IRQHandler(void)
{
    
    
  int PWM_out;
  if(EXTI_GetITStatus(EXTI_Line5)!=0) // 一级判定
  {
    
    
    if(PBin(5)==0)    // 二级判断
    {
    
     
      EXTI_ClearITPendingBit(EXTI_Line5); // 清除中断标志位
      // 1.采集编码器数据&MPU6050角度信息
      // 电机是相对安装，刚好相差180度，为了编码器输出极性一致，就需要对其中一个取反
      Encoder_Left  = -Read_Speed(2); 
      Encoder_Right = Read_Speed(4);
      
      mpu_dmp_get_data(&Pitch,&Roll,&Yaw);	    // 读取角度
      MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);  // 读取角速度
      MPU_Get_Accelerometer(&aacx,&aacy,&aacz); // 读取加速度
      // 2.将数据压入闭环控制中，计算出控制输出量
			Velocity_out=Velocity(Target_Speed,Encoder_Left,Encoder_Right); // 速度环
      Vertical_out=Vertical(Velocity_out+Med_Angle,Roll,gyrox);			  // 直立环
			Turn_out=Turn(gyroz);	
      
      PWM_out=Vertical_out;//最终输出
      
      // 3.把控制输出量加载到电机上，完成最终控制
      MOTO1 = PWM_out-Turn_out; // 左电机
      MOTO2 = PWM_out+Turn_out; // 右电机
      Limit(&MOTO1,&MOTO2);     // PWM限幅
      Load(MOTO1,MOTO2);        // 加载到电机上
    }
  }
}

Five, the entire control function source code

1、control.c

#include "control.h"

float Med_Angle=0;      // 机械中值，能使得小车真正平衡住的角度 
float Target_Speed=0;	  // 期望速度。---二次开发接口，用于控制小车前进后退及其速度。
float 
  Vertical_Kp=0,
  Vertical_Kd=0;     // 直立环Kp、Kd
float 
  Velocity_Kp=0,     // 速度环Kp、Ki（正反馈）
  Velocity_Ki=0;
float 
  Turn_Kp=0;

int Vertical_out,Velocity_out,Turn_out; // 直立环&速度环&转向环的输出变量

int Vertical(float Med,float Angle,float gyro_Y); // 函数声明
int Velocity(int Target,int encoder_left,int encoder_right);
int Turn(int gyro_Z);

void EXTI9_5_IRQHandler(void)
{
    
    
  int PWM_out;
  if(EXTI_GetITStatus(EXTI_Line5)!=0) // 一级判定
  {
    
    
    if(PBin(5)==0)    // 二级判断
    {
    
     
      EXTI_ClearITPendingBit(EXTI_Line5); // 清除中断标志位
      // 1.采集编码器数据&MPU6050角度信息
      // 电机是相对安装，刚好相差180度，为了编码器输出极性一致，就需要对其中一个取反
      Encoder_Left  = -Read_Speed(2); 
      Encoder_Right = Read_Speed(4);
      
      mpu_dmp_get_data(&Pitch,&Roll,&Yaw);	    // 读取角度
      MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);  // 读取角速度
      MPU_Get_Accelerometer(&aacx,&aacy,&aacz); // 读取加速度
      // 2.将数据压入闭环控制中，计算出控制输出量
			Velocity_out=Velocity(Target_Speed,Encoder_Left,Encoder_Right); // 速度环
      Vertical_out=Vertical(Velocity_out+Med_Angle,Roll,gyrox);			  // 直立环
			Turn_out=Turn(gyroz);	
      
      PWM_out=Vertical_out;//最终输出
      
      // 3.把控制输出量加载到电机上，完成最终控制
      MOTO1 = PWM_out-Turn_out; // 左电机
      MOTO2 = PWM_out+Turn_out; // 右电机
      Limit(&MOTO1,&MOTO2);     // PWM限幅
      Load(MOTO1,MOTO2);        // 加载到电机上
    }
  }
}

/*****************  
直立环PD控制器：Kp*Ek+Kd*Ek_D

入口：Med:机械中值(期望角度)，Angle:真实角度，gyro_Y:真实角速度
出口：直立环输出
******************/
int Vertical(float Med,float Angle,float gyro_Y) 
{
    
    
  int PWM_out;
  
  PWM_out = Vertical_Kp*(Angle-Med)+Vertical_Kd*(gyro_Y-0);
  
  return PWM_out;
} 

/*****************  
速度环PI控制器：Kp*Ek+Ki*Ek_S(Ek_S：偏差的积分)
******************/
int Velocity(int Target,int encoder_left,int encoder_right)
{
    
    
  // 定义成静态变量，保存在静态存储器，使得变量不丢掉
  static int PWM_out,Encoder_Err,Encoder_S,EnC_Err_Lowout,EnC_Err_Lowout_last;
  float a=0.7;
  
  // 1.计算速度偏差
  //舍去误差--我的理解：能够让速度为"0"的角度，就是机械中值。
  Encoder_Err = ((encoder_left+encoder_right)-Target);
  // 2.对速度偏差进行低通滤波
  // low_out = (1-a)*Ek+a*low_out_last
  EnC_Err_Lowout = (1-a)*Encoder_Err + a*EnC_Err_Lowout_last; // 使得波形更加平滑，滤除高频干扰，放置速度突变
  EnC_Err_Lowout_last = EnC_Err_Lowout;   // 防止速度过大影响直立环的正常工作
  // 3.对速度偏差积分出位移
  Encoder_S+=EnC_Err_Lowout;
  // 4.积分限幅
  Encoder_S=Encoder_S>10000?10000:(Encoder_S<(-10000)?(-10000):Encoder_S);
  
  // 5.速度环控制输出
  PWM_out = Velocity_Kp*EnC_Err_Lowout+Velocity_Ki*Encoder_S;
  
  return PWM_out;
}

/*****************  
转向环：系数*Z轴角速度
******************/
int Turn(int gyro_Z)
{
    
    
  int PWM_out;
  
  PWM_out = Turn_Kp*gyro_Z;
  
  return PWM_out;
}

  The above is the sixth lecture of the series of articles on the balance car-the explanation of the software programming of position PID, vertical loop and speed loop . The author of the series of articles on the balance car is continuously updating . If there is an error in the article or your friends have questions about the above content, you are welcome to leave a message in the comment area, and Xiaozheng will reply to you as soon as possible after seeing it!
[Balance car production] (7) Cascade PID tuning and balance results display (super detailed) https://blog.csdn.net/weixin_44270218/article/details/113786386