基于STM32的倒立摆源码

#include "control.h" int Balance_Pwm,Position_Pwm,Count_Position,Flag_qb2,Count; int TIM1_UP_IRQHandler(void) { static int cnt=0,cnt1=0; if(TIM1->SR&0X0001) { cnt1++; TIM1->SR&=~(1<<0); if(cnt1==100) { printf("Way_Turn=%d\n",Way_Turn); printf("Position_Pwm=%d\n",Position_Pwm); printf("Balance_Pwm=%d\n",Balance_Pwm); printf("Angle_Balance=%f\n\n\n",Angle_Balance); cnt1=0; } if(Way_Turn==1) //Way_Turn==1 { static int flag=0,first=1; if(first) { AIN1=0; AIN2=0; first=0; } Angle_Balance=Get_Adc_Average(7,5); if(flag==0&&Angle_Balance>=0&&Angle_Balance<=50) { AIN1=0; AIN2=1; PWMA=2000; delay_ms(200); AIN1=0; AIN2=0; } else if(flag==1&&Angle_Balance>=4000) { AIN1=1; AIN2=0; PWMA=2000; delay_ms(200); AIN1=0; AIN2=0; } else { AIN1=0; AIN2=0; } flag++; if(flag==2) flag=0; } else if(Way_Turn==2) //Way_Turn==2 { static int first=1; if(first==1) { AIN1=0; AIN2=0; delay_ms(5000); delay_ms(5000); delay_ms(5000); delay_ms(5000); first=0; } cnt++; if(cnt==30) { static int flag=0,first=0; cnt=0; if(flag==0) { AIN1=0; AIN2=1; PWMA=4000; if(first==0) { delay_ms(50); first=1; } } else { AIN1=1; AIN2=0; PWMA=4000; if(first==1) { delay_ms(400); first=2; } } flag++; if(flag==2) flag=0; } } else if(Way_Turn==3) //Way_Turn!=0&&Way_Turn!=1&&Way_Turn!=2 { if(1) { Angle_Balance=Get_Adc_Average(7,15); D_Angle_Balance= Angle_Balance-Last_Angle_Balance ; Balance_Pwm =balance(Angle_Balance); if(Flag_qb2==0) { if(Angle_Balance<(ZHONGZHI+100)&&Angle_Balance>(ZHONGZHI-100))Count_Position++; // if(Count_Position>60)Flag_qb2=1, Count_Position=0,TIM2->CNT=10000; //在平衡位置倒立超过300ms 开启位置控制 } if(1) { Encoder=Read_Encoder(); if(++Count>4) Position_Pwm=Position(Encoder),Count=0; } if(Way_Turn%2==0) { machine=Balance_Pwm; TIM2->CNT=10000; } else { machine=Balance_Pwm-Position_Pwm; } limit_Pwm(); if(close()==0) Set_Pwm(machine); Last_Angle_Balance=Angle_Balance; } } else if(Way_Turn!=0&&Way_Turn!=1&&Way_Turn!=2&&Way_Turn!=3) //Way_Turn!=0&&Way_Turn!=1&&Way_Turn!=2 { static int first=1,first1=1; if(first1==1) { AIN1=0; AIN2=0; first1=0; delay_ms(5000); delay_ms(5000); delay_ms(5000); delay_ms(5000); } Angle_Balance=Get_Adc_Average(7,15); if(first==1) { AIN1=0; AIN2=1; PWMA=7200; delay_ms(60); AIN1=1; AIN2=0; PWMA=7200; delay_ms(260); first=0; } else if(first==0) { AIN1=0; AIN2=0; first=2; } if(1) { D_Angle_Balance= Angle_Balance-Last_Angle_Balance ; Balance_Pwm =balance(Angle_Balance); if(Flag_qb2==0) { if(Angle_Balance<(ZHONGZHI+100)&&Angle_Balance>(ZHONGZHI-100))Count_Position++; // if(Count_Position>60)Flag_qb2=1, Count_Position=0,TIM2->CNT=10000; //在平衡位置倒立超过300ms 开启位置控制 } if(1) { Encoder=Read_Encoder(); if(++Count>4) Position_Pwm=Position(Encoder),Count=0; } if(Way_Turn%2==1) { machine=Balance_Pwm; TIM2->CNT=10000; } else { machine=Balance_Pwm-Position_Pwm; } limit_Pwm(); if(close()==0) Set_Pwm(machine); Last_Angle_Balance=Angle_Balance; } } } return 0; } int balance(float Angle) { float Bias; //倾角偏差 static float Last_Bias,D_Bias; //PID相关变量 int balance; //PWM返回值 Bias=Angle-ZHONGZHI; //求出平衡的角度中值 和机械相关 D_Bias=Bias-Last_Bias; //求出偏差的微分 进行微分控制 balance=-Balance_KP*Bias- D_Bias*Balance_KD; //===计算倾角控制的电机PWM PD控制 Last_Bias=Bias; //保持上一次的偏差 return balance; } int Position(int Encoder) { static float Position_PWM,Last_Position,Position_Bias,Position_Differential; static float Position_Least; Position_Least =Encoder-Position_Zero; //===获取偏差 Position_Bias *=0.8; Position_Bias += Position_Least*0.2; //===一阶低通滤波器 Position_Differential=Position_Bias-Last_Position; //===获取偏差变化率 Last_Position=Position_Bias; //保存上一次的偏差 Position_PWM=Position_Bias*Position_KP+Position_Differential*Position_KD; //===速度控制 return Position_PWM; } void Set_Pwm(int machine) { if(machine>0) AIN2=1, AIN1=0; else AIN2=0, AIN1=1; PWMA=abs(machine); } void limit_Pwm(void) { int Amplitude=6900; if(machine<-Amplitude) machine=-Amplitude; if(machine>Amplitude) machine=Amplitude; } u8 close() { u8 temp; if(Flag_Stop==1||Angle_Balance<(ZHONGZHI-1000)||Angle_Balance>(ZHONGZHI+1000)) { temp=1; AIN1=0; AIN2=0; TIM2->CNT=10000; } else temp=0; return temp; } int abs(int a) { int temp; if(a<0) temp=-a; else temp=a; return temp; }