STM32两轮自平衡小车——PID

代码

#include "control.h"float Med_Angle = -1.4; //机械中值（需调） float Vertical_Kp = -348, //直立环Kp（需调） Vertical_Kd = -0.9; //直立环Kd（需调） float Velocity_Kp = 0.35, //速度环Kp（需调）Velocity_Ki = 0.00175; //速度环Ki（需调）float Turn_Kp=1; //转向环Kp（需调）int Vertical_out,Velocity_out,Turn_out; //直立环&速度环&转向环输出变量void EXTI9_5_IRQHandler(void) {if(EXTI_GetITStatus(EXTI_Line5)!=0) //一级判定{int PWM_out;if(PBin(5)==0) //二级判定{EXTI_ClearITPendingBit(EXTI_Line5); //清除中断标志位/*步骤1.采集编码器数据&MPU6050角度信息*/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.将数据带入直立环&速度环&转向环，计算出控制输出量Vertical_out = Vertical(Med_Angle,Pitch,gyroy);Velocity_out = Velocity(Encoder_Left,Encoder_Right);Turn_out = Turn(gyroz);PWM_out = Vertical_out - Vertical_Kp*Velocity_out; /*****************************************************/ //3.将控制输出量加载至电机MOTO1 = PWM_out-Turn_out; //电机1MOTO2 = PWM_out+Turn_out; //电机2Limit(&MOTO1,&MOTO2); //PWM限幅Load(MOTO1,MOTO2); //加载至电机}} }/**************** 直立环PD:Kp*Ex+Kd*Ex_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 ****************/ int Velocity(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.计算速度偏差Encoder_Err = (encoder_left+encoder_right)-0; //这里左右电机之和没有除以2是为了避免舍去误差//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; }/**************** 转向环：Kp*gyro_Z ****************/ int Turn(int gyro_Z) {int PWM_out;PWM_out = Turn_Kp*gyro_Z;return PWM_out; }

调参

直立环
屏蔽速度环&转向环

1.机械中值
小车本身尽可能不倒下的Pitch值。

2.Kp极性（正负）
极性正确：小车呈直立趋势。
极性错误：小车加速倒下。

3.Kp大小
数值从小到大尝试，直到小车出现大幅低频抖动。

4.Kd极性（正负）
屏蔽直立环的Kp
极性正确：小车轮胎与小车转动方向相同。
极性错误：小车轮胎与小车转动方向相反。

5.Kd大小
加入直立环Kp
数值从小到大尝试，直到小车出现高频振荡。

Kp与Kd最终数值=0.6✖振荡数值。
速度环
Ki=1/200Kp*
1.Kp极性
屏蔽直立环&转向环
极性正确：旋转小车其中一个轮子，两轮同向转动，直至转速最大。
极性错误：旋转小车其中一个轮子，另一轮子反向转动。

2.Kp大小
加入直立环
数值从小到大尝试，直到小车走直线性能优秀，回位效果良好。

转向环
屏蔽直立环&速度环

总结

以上是生活随笔为你收集整理的STM32两轮自平衡小车——PID的全部内容，希望文章能够帮你解决所遇到的问题。

如果觉得生活随笔网站内容还不错，欢迎将生活随笔推荐给好友。