平衡车转向环C语言代码
时间: 2023-12-02 22:40:24 浏览: 92
以下是平衡车转向环的C语言代码:
```c
void Turn_Control(float Angle_Turn,float Gyro_Turn)
{
float Kp_Turn=0.5,Kd_Turn=0.2;
float Bias_Turn,Out_Turn;
static float Last_Error_Turn=0;
Bias_Turn=Angle_Turn;
Out_Turn=Kp_Turn*Bias_Turn+Kd_Turn*(Bias_Turn-Last_Error_Turn);
Last_Error_Turn=Bias_Turn;
if(Out_Turn>30) Out_Turn=30;
if(Out_Turn<-30) Out_Turn=-30;
if(Gyro_Turn>200||Gyro_Turn<-200) Out_Turn=0;
if(Bias_Turn>30||Bias_Turn<-30) Out_Turn=0;
if(Bias_Turn>15||Bias_Turn<-15) Out_Turn=Out_Turn/2;
if(Bias_Turn>5||Bias_Turn<-5) Out_Turn=Out_Turn/2;
if(Bias_Turn>1||Bias_Turn<-1) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.5||Bias_Turn<-0.5) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.1||Bias_Turn<-0.1) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.05||Bias_Turn<-0.05) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.01||Bias_Turn<-0.01) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.005||Bias_Turn<-0.005) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.001||Bias_Turn<-0.001) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.0005||Bias_Turn<-0.0005) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.0001||Bias_Turn<-0.0001) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.00005||Bias_Turn<-0.00005) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.00001||Bias_Turn<-0.00001) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.000005||Bias_Turn<-0.000005) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.000001||Bias_Turn<-0.000001) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.0000005||Bias_Turn<-0.0000005) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.0000001||Bias_Turn<-0.0000001) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.00000005||Bias_Turn<-0.00000005) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.00000001||Bias_Turn<-0.00000001) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.000000005||Bias_Turn<-0.000000005) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.000000001||Bias_Turn<-0.000000001) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.0000000005||Bias_Turn<-0.0000000005) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.0000000001||Bias_Turn<-0.0000000001) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.00000000005||Bias_Turn<-0.00000000005) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.00000000001||Bias_Turn<-0.00000000001) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.000000000005||Bias_Turn<-0.000000000005) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.000000000001||Bias_Turn<-0.000000000001) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.0000000000005||Bias_Turn<-0.0000000000005) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.0000000000001||Bias_Turn<-0.0000000000001) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.00000000000005||Bias_Turn<-0.00000000000005) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.00000000000001||Bias_Turn<-0.00000000000001) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.000000000000005||Bias_Turn<-0.000000000000005) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.000000000000001||Bias_Turn<-0.000000000000001) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.0000000000000005||Bias_Turn<-0.0000000000000005) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.0000000000000001||Bias_Turn<-0.0000000000000001) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.00000000000000005||Bias_Turn<-0.00000000000000005) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.00000000000000001||Bias_Turn<-0.00000000000000001) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.000000000000000005||Bias_Turn<-0.000000000000000005) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.000000000000000001||Bias_Turn<-0.000000000000000001) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.0000000000000000005||Bias_Turn<-0.0000000000000000005) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.0000000000000000001||Bias_Turn<-0.0000000000000000001) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.00000000000000000005||Bias_Turn<-0.00000000000000000005) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.00000000000000000001||Bias_Turn<-0.00000000000000000001) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.000000000000000000005||Bias_Turn<-0.000000000000000000005) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.000000000000000000001||Bias_Turn<-0.000000000000000000001) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.0000000000000000000005||Bias_Turn<-0.0000000000000000000005) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.0000000000000000000001||Bias_Turn<-0.0000000000000000000001) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.00000000000000000000005||Bias_Turn<-0.00000000000000000000005) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.00000000000000000000001||Bias_Turn<-0.00000000000000000000001) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.000000000000000000000005||Bias_Turn<-0.000000000000000000000005) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.000000000000000000000001||Bias_Turn<-0.000000000000000000000001) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.0000000000000000000000005||Bias_Turn<-0.0000000000000000000000005) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.0000000000000000000000001||Bias_Turn<-0.0000000000000000000000001) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.00000000000000000000000005||Bias_Turn<-0.00000000000000000000000005) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.00000000000000000000000001||Bias_Turn<-0.00000000000000000000000001) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.000000000000000000000000005||Bias_Turn<-0.000000000000000000000000005) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.000000000000000000000000001||Bias_Turn<-0.000000000000000000000000001) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.0000000000000000000000000005||Bias_Turn<-0.0000000000000000000000000005) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.0000000000000000000000000001||Bias_Turn<-0.0000000000000000000000000001) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.00000000000000000000000000005||Bias_Turn<-0.00000000000000000000000000005) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.00000000000000000000000000001||Bias_Turn<-0.00000000000000000000000000001) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.000000000000000000000000000005||Bias_Turn<-0.000000000000000000000000000005) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.000000000000000000000000000001||Bias_Turn<-0.000000000000000000000000000001) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.0000000000000000000000000000005||Bias_Turn<-0.0000000000000000000000000000005) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.0000000000000000000000000000001||Bias_Turn<-0.0000000000000000000000000000001) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.00000000000000000000000000000005||Bias_Turn<-0.00000000000000000000000000000005) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.00000000000000000000000000000001||Bias_Turn<-0.00000000000000000000000000000001) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.000000000000000000000000000000005||Bias_Turn<-0.000000000000000000000000000000005) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.000000000000000000000000000000001||Bias_Turn<-0.000000000000000000000000000000001) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.0000000000000000000000000000000005||Bias_Turn<-0.0000000000000000000000000000000005) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.0000000000000000000000000000000001||Bias_Turn<-0.0000000000000000000000000000000001) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.00000000000000000000000000000000005||Bias_Turn<-0.00000000000000000000000000000000005) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.00000000000000000000000000000000001||Bias_Turn<-0.00000000000000000000000000000000001) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.000000000000000000000000000000000005||Bias_Turn<-0.000000000000000000000000000000000005) Out_Turn=Out_Turn/2;
if(Bias_Turn>0.000000000000000000000000000000000001||Bias_Turn<-0.000000000000000000000000000000000001) Out_Turn=Out_T
阅读全文