[embedded code] Application of PID balance control in intelligent patrol robot

Attitude balance control

Adding speed negative feedback to vertical control can not achieve the purpose of speed closed loop, but also destroy the vertical control system. Therefore, under the condition of ensuring the priority of vertical control, developers should put speed control in front of vertical control, that is, the result of speed control adjustment is only to change the target value of vertical control.

According to experience, the running speed of the trolley is related to the inclination angle of the trolley. For example, to increase the forward speed of the trolley, it is necessary to increase the forward tilt angle of the trolley. After the tilt angle increases, the wheels need to move forward under the vertical control to keep the trolley balanced and the speed increases; If you want to reduce the forward speed of the trolley, you need to reduce the forward tilt angle of the trolley. After the tilt angle is reduced, the wheels move backward under the action of vertical control to maintain the balance of the trolley and reduce the speed.

Developers connect the speed and vertical controllers in series, in which the output of speed control is used as the input of vertical control and the output of vertical control is used as the output of the system, which is actually a cascade control system. PD control is used for vertical control. Because of the possible noise of the encoder, in order to prevent the noise from being amplified and eliminate the static error of the system, PI control is used for speed control here.

Vertical PD control

int balance(float Angle,float Gyro)
     float Bias;
     int balancePID;
     Bias=Angle-Angle_OFFSET;                       //===Calculate the median angle of balance and mechanical correlation
     balancePID=Balance_Kp*Bias+Gyro*Balance_Kd;   //===Motor PWM PD control with calculated balance control    kp is the P coefficient and kd is the D coefficient 
     return balancePID;

Speed PI control

int velocity(int encoder_left,int encoder_right)
    static float Velocity,Encoder_Least,Encoder,Movement;
    static float Encoder_Integral,Target_Velocity;
    //=============Remote control forward and backward section=======================// 
    if(Direction.Current==GO_STRAIGHT)      Movement=-Target_Velocity/Flag_speed;           //===Forward sign position 1 
    else if(Direction.Current==GO_BACK) Movement=Target_Velocity/Flag_speed;         //===Reverse flag position 1
    else  Movement=0;   
    //=============Speed PI controller=======================// 
    Encoder_Least =(encoder_left+encoder_right)-0;                   
    Encoder *= 0.8;                                                     //===First order low pass filter       
    Encoder += Encoder_Least*0.2;                                       //===First order low pass filter    
    Encoder_Integral +=Encoder;                                       //===Integration out displacement integration time: 10ms
    Encoder_Integral=Encoder_Integral-Movement;                       //===Receive remote control data and control forward and backward
    if(Encoder_Integral>8000)   Encoder_Integral=8000;             //===Integral limiting
    if(Encoder_Integral<-8000)  Encoder_Integral=-8000;              //===Integral limiting  
    Velocity=Encoder*Velocity_Kp+Encoder_Integral*Velocity_Ki;        //===Speed control 
    return Velocity;

Steering control

In addition to maintaining balance, the trolley also involves left and right rotation, so steering control needs to be added, which can be referred to as follows

int turn(int encoder_left,int encoder_right,float gyro)//Steering control
    static float Turn_Target,Turn,Encoder_temp,Turn_Convert=0.9,Turn_Count; 
    float Turn_Amplitude=30/Flag_speed,Kp=32,Kd=0;  
        else if(Direction.Current==TURN_RIGHT){
        else Turn_Target=0;
    if(Turn_Target>Turn_Amplitude)  Turn_Target=Turn_Amplitude;    //===Steering speed limiting
      if(Turn_Target<-Turn_Amplitude) Turn_Target=-Turn_Amplitude;
          Kd=-1 ;        
    //=============Steering PD controller=======================//
    Turn=-Turn_Target*Kp-gyro*Kd;                 //===PD control combined with Z-axis gyroscope
      return Turn;

Motor PWM control

After the above series of control calculations, the developer obtains the PWM value to balance the trolley (Moto1 and Moto2 in the following program).

    Balance_Pwm = balance(Angle_Balance,Gyro_Balance);//===Balanced PID control  
    Velocity_Pwm= velocity(Enconder_left,Enconder_right);//===Speed loop PID control
    Turn_Pwm   = turn(Enconder_left,Enconder_right,Gyro_Turn);//===Steering ring PID control 
    Moto1=Balance_Pwm+Velocity_Pwm-Turn_Pwm;//===Calculate the final PWM of the left wheel motor
    Moto2=Balance_Pwm+Velocity_Pwm+Turn_Pwm;//===Calculate the final PWM of the right wheel motor
    Limit_Pwm();                            //===PWM limiting  
    Set_Pwm(Moto1,Moto2);                   //===Assigned to PWM register 

By giving the PWM amplitude to the corresponding register, you can view the actual motion state of the trolley.

void Set_Pwm(int moto1,int moto2)
    if(moto2>0)     {AIN2_RESET;AIN1_SET;}
    else            {AIN2_SET;AIN1_RESET;}      
    if(moto1>0) {BIN1_RESET;BIN2_SET;}
    else        {BIN1_SET;BIN2_RESET;}

Keywords: pid

Added by markbm on Sat, 04 Sep 2021 04:39:14 +0300