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=======================//
Target_Velocity=40;
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
if(Turn_Off(Angle_Balance,BAT_VOL)==1||Direction.Current==TURN_OFF)
Encoder_Integral=0;
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;
if(Direction.Current==TURN_LEFT||Direction.Current==TURN_RIGHT)
{
if(++Turn_Count==1)
Encoder_temp=myabs(encoder_left+encoder_right);
Turn_Convert=50/Encoder_temp;
if(Turn_Convert<0.6)Turn_Convert=0.6;
if(Turn_Convert>3)Turn_Convert=3;
}
else
{
Turn_Convert=0.9;
Turn_Count=0;
Encoder_temp=0;
}
if(Direction.Current==TURN_LEFT){
Turn_Target+=Turn_Convert;
}
else if(Direction.Current==TURN_RIGHT){
Turn_Target-=Turn_Convert;
}
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;
if(Direction.Current==GO_STRAIGHT||Direction.Current==GO_BACK||Direction.Current==KEEP_STOP)
Kd=-1 ;
else
Kd=0;
//=============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;}
TIM16_PWM_Set(myabs(moto2));
if(moto1>0) {BIN1_RESET;BIN2_SET;}
else {BIN1_SET;BIN2_RESET;}
TIM17_PWM_Set(myabs(moto1));
}