STM32 PID练习代码风洞控制系统通过超声波测距小乒乓球位置PID调节空心杯 -

#include "stm32f10x.h"
#include "hcsr04.h"
#include "pwm.h"
#include "delay.h"
#include "usart.h"
#include "remote.h"
#include "lcd.h"
#include "timer.h"
#include "key.h"
#include "math.h"
#include "motor.h"
float X,Err_X,Err_X_Last,Err_X_Sum;
float Aim_X;
int pwm = 0;
float distance;
u32 time = 0;
u16 count = 0;
u16 count1 = 0;
float X_Pos[100];
float X_pwm[100];
float Kp = 0,Ki = 0.2,Kd = 140;
float data[2];
u8 Key,Key_Last;
u8 Q_n;
void LCD_Display(void);
void Q_1(void);
void Q_2(void);
void Q_3(void);
void Q_4(void);
void Q_5(void);
void Q_6(void);
void Q_7(void);
int main()
{
u16 i = 0;
delay_init(); //延时初始化
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
uart_init(115200);
LCD_Init(); //LCD初始化
HC_SR04_al1_Init();
PWM_Init(3000-1,2-1); //2分频 12KHZ
Remote_Init(); //红外接收初始化
TIM5_Init(200-1,7200-1);//20ms进入一次
KEY_Init();
Motor_Init();
TIM_SetCompare1(TIM3,0);
POINT_COLOR=RED; //设置字体为红色
LCD_ShowString(30,50,200,16,16,"X:");
LCD_ShowString(30,70,200,16,16,"PWM:");
LCD_ShowString(30,90,200,16,16,"Time:");
LCD_ShowString(30,110,200,16,16,"Kd:");
LCD_ShowString(30,130,200,16,16,"Ki:");
Motor_Drive(Start);
//TIM_Cmd(TIM5,DISABLE);
while(1)
{
distance = Senor_Using_3();
if(distance >= 60) distance = 4;
//printf("%f\r\n",distance);
/*
Key = KEY_Scan(0);
switch(Key)
{
case KEY0_PRES: Ki += 0.1; break;
case KEY1_PRES: Ki -= 0.1;break;
case WKUP_PRES: break;
}
*/
//TIM_SetCompare1(TIM3,pwm);
Key = Remote_Scan();
if(Key)
{
switch(Key)
{
case 104:pwm = 0;time = 0;count1 = 0; Q_n = 1;delay_ms(500);break;
case 152:pwm = 0;time = 0;count1 = 0; Q_n = 2;delay_ms(500);break;
case 176:pwm = 0;time = 0;count1 = 0; Q_n = 3;delay_ms(500);break;
case 48:pwm = 0;time = 0;count1 = 0; Q_n = 4;delay_ms(500);break;
case 24:pwm = 0;time = 0;count1 = 0; Q_n = 5;delay_ms(500);break;
case 122:pwm = 0;time = 0;count1 = 0; Q_n = 6;delay_ms(500);break;
case 16:pwm = 0;time = 0;count1 = 0; Q_n = 7;delay_ms(500);break;
case 162:pwm = 0;time = 0;count1 = 0;Q_n = 0;delay_ms(500);break;
}
}
delay_ms(10);
//data[0] = X;
//data[1] = Aim_X;
//Send_wave((u8 *)data);
LCD_Display();
}
}
void LCD_Display(void)
{
LCD_ShowNum(86,50,X,3,16); //显示键值
LCD_ShowNum(86,70,pwm,4,16); //显示按键次数
LCD_ShowNum(86,90,time,4,16); //显示稳定时间
LCD_ShowNum(86,110,Kd,4,16); //显示稳定时间
LCD_Showfloat(86,130,Ki,4,16);
}
void PID_Control()
{
pwm = 1520 + Kp*Err_X + Ki*Err_X_Sum + Kd*(Err_X - Err_X_Last);
//pwm += Kp*Err_X*0.02;
}
void TIM5_IRQHandler()
{
if(TIM_GetITStatus(TIM5,TIM_IT_Update)!=RESET)
{
X = distance - 4;
Err_X = X - Aim_X;
Err_X_Sum += Err_X;
data[0] = X;
data[1] = Aim_X;
//Send_wave((u8 *)data);
//误差和限幅
if(Err_X_Sum >= 1500) Err_X_Sum = 1500;
if(Err_X_Sum <= -1500) Err_X_Sum = -1500;
switch(Q_n)
{
case 0: pwm = 0;break;
case 1: Q_1();break;
case 2: Q_2();break;
case 3: Q_3();break;
case 5: Q_5();break;
case 6: Q_6();break;
case 7: Q_7();break;
}
if(pwm > 2600) pwm = 2600;
Err_X_Last = Err_X;
TIM_SetCompare1(TIM3,pwm);
}
TIM_ClearITPendingBit(TIM5,TIM_IT_Update);//清除中断标志
}
void Q_1()
{
Aim_X = 15.0;
Kp = 80;
Ki = 0.3;
Kd = 200;
PID_Control();
if(X >= 10&&X <= 20)
{
count++;
if(count == 50)
{
count = 0;
time++;
}
}
else
{
count = 0;
time = 0;
}
//TIM_SetCompare1(TIM3,pwm);
}
void Q_2()
{
Aim_X = 12.0;
Kp = 80;
Ki = 0.5;
Kd = 200;
PID_Control();
if(X >= 10&&X <= 20)
{
count++;
if(count == 50)
{
count = 0;
time++;
}
}
else
{
count = 0;
time = 0;
}
}
void Q_3()
{
u8 key;
if(count1 == 0)//第一次的PID参数和位置的初始化
{
Aim_X = 20.0;
Kp = 80;
Ki = 0.3;
Kd = 200;
}else
{
count1 = 1;
}
count1++;
PID_Control();
key = KEY_Scan(0);
if(key == KEY0_PRES)
{
Aim_X -= 1.0;
Ki = 0.8;
time = 0;
}
else if(key == KEY1_PRES)
{
Aim_X += 1.0;
Ki = 0.8;
time = 0;
}
if(fabs(Err_X) <= 1.0)
{
count++;
if(count == 50)
{
count = 0;
time++;
}
}
}
void Q_5()
{
if(count1 == 0)//第一次的PID参数和位置的初始化
{
pwm = 0;
}else
{
count1 = 1;
if(pwm == 2600)
{
count++;
if(count == 50)
{
count = 0;
time++;
}
}
pwm += 20;
}
count1++;
}
void Q_6()
{
if(count1 == 0)//到达AB段维持3秒
{
Aim_X = 8.0;
Kp = 80;
Ki = 0.5;
Kd = 260;
PID_Control();
if(pwm > 2200) pwm = 2200;
if(fabs(Err_X) < 0.5)
{
count++;
if(count == 50)
{
count = 0;
time++;
if(time >= 3)
{
count1 = 1;
time = 0;
}
}
}
}
if(count1 == 1)//到达CD段维持3秒
{
Aim_X = 25.0;
Kp = 80;
Ki = 0.5;
Kd = 220;
PID_Control();
if(X >= 20&&X <= 30)
{
count++;
if(count == 50)
{
count = 0;
time++;
if(time >= 3)
{
count1 = 2;
time = 0;
}
}
}
else
{
count = 0;
time = 0;
}
}
if(count1 == 2)//再次到达AB段维持3秒
{
Aim_X = 8.0;
Kp = 80;
Ki = 0.5;
Kd = 260;
PID_Control();
if(pwm > 2200) pwm = 2200;
if(fabs(Err_X) < 0.5)
{
count++;
if(count == 50)
{
count = 0;
time++;
if(time >= 3)
{
count1 = 3;
time = 0;
}
}
}
}
if(count1 == 3)//再次到达CD段维持3秒
{
Aim_X = 25.0;
Kp = 80;
Ki = 0.4;
Kd = 200;
PID_Control();
if(X >= 20&&X <= 30)
{
count++;
if(count == 50)
{
count = 0;
time++;
if(time >= 3)
{
count1 = 4;
time = 0;
}
}
}
else//不稳则计时清零
{
count = 0;
time = 0;
}
}
if(count1 == 4)//冲出重围
{
if(pwm == 2600)
{
count++;
if(count == 50)
{
count = 0;
time++;
}
}
pwm += 20;
}
}
void Q_7()
{
if(count1 == 0)//第一次的PID参数和位置的初始化
{
pwm = 0;
}else
{
count1 = 1;
if(distance<=41)
{
Aim_X = 15.0;
Kp = 80;
Ki = 0.4;
Kd = 220;
PID_Control();
}
else
{
pwm = 0;
}
if(X >= 10 && X <= 20)
{
count++;
if(count == 50)
{
count = 0;
time++;
}
}
else
{
time = 0;
count = 0;
}
}
count1++;
}

复制代码