找回密码
 立即注册

QQ登录

只需一步,快速开始

搜索
查看: 3491|回复: 3
收起左侧

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

  [复制链接]
ID:293035 发表于 2020-3-25 09:38 | 显示全部楼层 |阅读模式
STM32F1 通过超声波测距小乒乓球位置 PID控制调节空心杯电机转速 从而使小球到达设定的位置。供参考移植

制作出来的实物图如下:
IMG_20190719_144928.jpg

单片机源程序如下:
  1. #include "stm32f10x.h"
  2. #include "hcsr04.h"
  3. #include "pwm.h"
  4. #include "delay.h"
  5. #include "usart.h"
  6. #include "remote.h"
  7. #include "lcd.h"
  8. #include "timer.h"
  9. #include "key.h"
  10. #include "math.h"
  11. #include "motor.h"


  12. float X,Err_X,Err_X_Last,Err_X_Sum;
  13. float Aim_X;
  14. int pwm = 0;
  15. float distance;
  16. u32 time = 0;
  17. u16 count = 0;
  18. u16 count1 = 0;
  19. float X_Pos[100];
  20. float X_pwm[100];
  21. float Kp = 0,Ki = 0.2,Kd = 140;
  22. float data[2];
  23. u8 Key,Key_Last;
  24. u8 Q_n;

  25. void LCD_Display(void);
  26. void Q_1(void);
  27. void Q_2(void);
  28. void Q_3(void);
  29. void Q_4(void);
  30. void Q_5(void);
  31. void Q_6(void);
  32. void Q_7(void);

  33. int main()
  34. {
  35.          u16 i = 0;
  36.    delay_init(); //延时初始化
  37.          NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
  38.          uart_init(115200);
  39.          LCD_Init();  //LCD初始化
  40.          HC_SR04_al1_Init();
  41.          PWM_Init(3000-1,2-1); //2分频  12KHZ
  42.          Remote_Init();                        //红外接收初始化
  43.          TIM5_Init(200-1,7200-1);//20ms进入一次
  44.    KEY_Init();
  45.          Motor_Init();
  46.          TIM_SetCompare1(TIM3,0);
  47.          POINT_COLOR=RED;                //设置字体为红色
  48.          LCD_ShowString(30,50,200,16,16,"X:");
  49.          LCD_ShowString(30,70,200,16,16,"PWM:");       
  50.          LCD_ShowString(30,90,200,16,16,"Time:");
  51.          LCD_ShowString(30,110,200,16,16,"Kd:");
  52.          LCD_ShowString(30,130,200,16,16,"Ki:");
  53.          Motor_Drive(Start);
  54.          //TIM_Cmd(TIM5,DISABLE);
  55.          while(1)
  56.          {
  57.           distance = Senor_Using_3();
  58.                 if(distance >= 60) distance = 4;
  59.                 //printf("%f\r\n",distance);
  60.                  
  61.                 /*
  62.                 Key = KEY_Scan(0);
  63.                 switch(Key)
  64.           {
  65.                           case KEY0_PRES:  Ki += 0.1; break;
  66.                                 case KEY1_PRES:  Ki -= 0.1;break;
  67.                           case WKUP_PRES:  break;
  68.           }
  69.                  */
  70.                  
  71.                 //TIM_SetCompare1(TIM3,pwm);
  72.                  
  73.                  Key = Remote_Scan();
  74.                 if(Key)
  75.                 {
  76.                   switch(Key)
  77.                         {
  78.                           case 104:pwm = 0;time = 0;count1 = 0; Q_n = 1;delay_ms(500);break;
  79.                                 case 152:pwm = 0;time = 0;count1 = 0; Q_n = 2;delay_ms(500);break;
  80.                                 case 176:pwm = 0;time = 0;count1 = 0; Q_n = 3;delay_ms(500);break;
  81.                                 case 48:pwm = 0;time = 0;count1 = 0; Q_n = 4;delay_ms(500);break;
  82.                                 case 24:pwm = 0;time = 0;count1 = 0; Q_n = 5;delay_ms(500);break;
  83.                                 case 122:pwm = 0;time = 0;count1 = 0; Q_n = 6;delay_ms(500);break;
  84.                                 case 16:pwm = 0;time = 0;count1 = 0; Q_n = 7;delay_ms(500);break;
  85.                                 case 162:pwm = 0;time = 0;count1 = 0;Q_n = 0;delay_ms(500);break;
  86.                         }
  87.                 }
  88.                
  89.                 delay_ms(10);
  90.                 //data[0] = X;
  91.                 //data[1] = Aim_X;
  92.                 //Send_wave((u8 *)data);
  93.                
  94.                 LCD_Display();
  95.          }
  96.    
  97. }

  98. void LCD_Display(void)
  99. {
  100.    LCD_ShowNum(86,50,X,3,16);                //显示键值
  101.          LCD_ShowNum(86,70,pwm,4,16);        //显示按键次数
  102.          LCD_ShowNum(86,90,time,4,16);        //显示稳定时间
  103.          LCD_ShowNum(86,110,Kd,4,16);        //显示稳定时间
  104.          LCD_Showfloat(86,130,Ki,4,16);
  105. }

  106. void PID_Control()
  107. {
  108.    pwm = 1520 + Kp*Err_X + Ki*Err_X_Sum + Kd*(Err_X - Err_X_Last);
  109.         //pwm += Kp*Err_X*0.02;
  110. }

  111. void TIM5_IRQHandler()
  112. {
  113.   if(TIM_GetITStatus(TIM5,TIM_IT_Update)!=RESET)
  114.         {
  115.                 X = distance - 4;
  116.                 Err_X = X - Aim_X;
  117.                 Err_X_Sum += Err_X;
  118.                 data[0] = X;
  119.                 data[1] = Aim_X;
  120.                 //Send_wave((u8 *)data);
  121.                
  122.                 //误差和限幅
  123.                 if(Err_X_Sum >= 1500) Err_X_Sum = 1500;
  124.                 if(Err_X_Sum <= -1500) Err_X_Sum = -1500;
  125.                
  126.                 switch(Q_n)
  127.                 {
  128.                         case 0: pwm = 0;break;
  129.                   case 1: Q_1();break;
  130.                         case 2: Q_2();break;
  131.                         case 3: Q_3();break;
  132.                         case 5: Q_5();break;
  133.                         case 6: Q_6();break;
  134.                         case 7: Q_7();break;
  135.                 }
  136.                
  137.                
  138.                 if(pwm > 2600) pwm = 2600;
  139.                
  140.                 Err_X_Last = Err_X;
  141.                
  142.                 TIM_SetCompare1(TIM3,pwm);

  143.   }
  144.         TIM_ClearITPendingBit(TIM5,TIM_IT_Update);//清除中断标志
  145. }

  146. void Q_1()
  147. {
  148.   Aim_X = 15.0;
  149.         Kp = 80;
  150.         Ki = 0.3;
  151.         Kd = 200;
  152.         PID_Control();
  153.         if(X >= 10&&X <= 20)
  154.         {
  155.            count++;
  156.      if(count == 50)
  157.                  {
  158.                          count = 0;
  159.                    time++;
  160.                          
  161.                  }                         
  162.         }
  163.         else
  164.         {
  165.           count = 0;
  166.                 time = 0;
  167.         }
  168.        
  169.         //TIM_SetCompare1(TIM3,pwm);
  170. }

  171. void Q_2()
  172. {
  173.   Aim_X = 12.0;
  174.         Kp = 80;
  175.         Ki = 0.5;
  176.         Kd = 200;
  177.         PID_Control();
  178.         if(X >= 10&&X <= 20)
  179.         {
  180.            count++;
  181.      if(count == 50)
  182.                  {
  183.                          count = 0;
  184.                    time++;
  185.                  }                         
  186.         }
  187.         else
  188.         {
  189.           count = 0;
  190.                 time = 0;
  191.         }
  192. }

  193. void Q_3()
  194. {
  195.         u8 key;
  196.         if(count1 == 0)//第一次的PID参数和位置的初始化
  197.         {
  198.                 Aim_X = 20.0;
  199.                 Kp = 80;
  200.                 Ki = 0.3;
  201.                 Kd = 200;
  202.         }else
  203.         {
  204.           count1 = 1;
  205.         }
  206.         count1++;
  207.        
  208.         PID_Control();
  209.         key = KEY_Scan(0);
  210.         if(key == KEY0_PRES)
  211.         {
  212.                 Aim_X -= 1.0;
  213.                 Ki = 0.8;
  214.                 time = 0;
  215.         }
  216.         else if(key == KEY1_PRES)
  217.         {
  218.                 Aim_X += 1.0;
  219.                 Ki = 0.8;
  220.                 time = 0;
  221.         }
  222.        
  223.         if(fabs(Err_X) <= 1.0)
  224.         {
  225.            count++;
  226.      if(count == 50)
  227.                  {
  228.                          count = 0;
  229.                    time++;
  230.                  }                         
  231.         }
  232.        
  233. }

  234. void Q_5()
  235. {
  236.         if(count1 == 0)//第一次的PID参数和位置的初始化
  237.         {
  238.           pwm = 0;
  239.         }else
  240.         {
  241.           count1 = 1;
  242.                 if(pwm == 2600)
  243.          {
  244.            count++;
  245.      if(count == 50)
  246.                  {
  247.                          count = 0;
  248.                    time++;
  249.                  }                         
  250.          }
  251.                 pwm += 20;
  252.         }
  253.         count1++;
  254.        
  255. }

  256. void Q_6()
  257. {
  258.   if(count1 == 0)//到达AB段维持3秒
  259.         {
  260.           Aim_X = 8.0;
  261.                 Kp = 80;
  262.                 Ki = 0.5;
  263.                 Kd = 260;
  264.                 PID_Control();
  265.                 if(pwm > 2200) pwm = 2200;
  266.                
  267.                 if(fabs(Err_X) < 0.5)
  268.           {
  269.            count++;
  270.      if(count == 50)
  271.                  {
  272.                          count = 0;
  273.                    time++;
  274.                          if(time >= 3)
  275.                          {
  276.                             count1 = 1;
  277.                                   time = 0;
  278.                          }
  279.                  }                         
  280.           }
  281.                
  282.         }
  283.         if(count1 == 1)//到达CD段维持3秒
  284.         {
  285.           Aim_X = 25.0;
  286.                 Kp = 80;
  287.                 Ki = 0.5;
  288.                 Kd = 220;
  289.                 PID_Control();
  290.                 if(X >= 20&&X <= 30)
  291.           {
  292.            count++;
  293.      if(count == 50)
  294.                  {
  295.                          count = 0;
  296.                    time++;
  297.                          if(time >= 3)
  298.                          {
  299.                             count1 = 2;
  300.                                   time = 0;
  301.                          }
  302.                  }                         
  303.           }
  304.                 else
  305.                 {
  306.                   count = 0;
  307.                         time = 0;
  308.                 }
  309.         }
  310.         if(count1 == 2)//再次到达AB段维持3秒
  311.         {
  312.           Aim_X = 8.0;
  313.                 Kp = 80;
  314.                 Ki = 0.5;
  315.                 Kd = 260;
  316.                 PID_Control();
  317.                 if(pwm > 2200) pwm = 2200;
  318.                 if(fabs(Err_X) < 0.5)
  319.           {
  320.            count++;
  321.      if(count == 50)
  322.                  {
  323.                          count = 0;
  324.                    time++;
  325.                          if(time >= 3)
  326.                          {
  327.                             count1 = 3;
  328.                                   time = 0;
  329.                          }
  330.                  }                         
  331.           }
  332.                
  333.         }
  334.         if(count1 == 3)//再次到达CD段维持3秒
  335.         {
  336.           Aim_X = 25.0;
  337.                 Kp = 80;
  338.                 Ki = 0.4;
  339.                 Kd = 200;
  340.                 PID_Control();
  341.                 if(X >= 20&&X <= 30)
  342.           {
  343.            count++;
  344.      if(count == 50)
  345.                  {
  346.                          count = 0;
  347.                    time++;
  348.                          if(time >= 3)
  349.                          {
  350.                             count1 = 4;
  351.                                   time = 0;
  352.                          }
  353.                  }                         
  354.           }
  355.                 else//不稳则计时清零
  356.                 {
  357.                   count = 0;
  358.                         time = 0;
  359.                 }
  360.         }
  361.         if(count1 == 4)//冲出重围
  362.         {
  363.           if(pwm == 2600)
  364.          {
  365.            count++;
  366.      if(count == 50)
  367.                  {
  368.                          count = 0;
  369.                    time++;
  370.                  }                         
  371.          }
  372.                 pwm += 20;
  373.         }
  374.        
  375. }

  376. void Q_7()
  377. {
  378.     if(count1 == 0)//第一次的PID参数和位置的初始化
  379.         {
  380.           pwm = 0;
  381.         }else
  382.         {
  383.           count1 = 1;
  384.                 if(distance<=41)                 
  385.           {
  386.                  
  387.                         Aim_X = 15.0;
  388.                         Kp = 80;
  389.                         Ki = 0.4;
  390.                         Kd = 220;
  391.                         PID_Control();
  392.           }
  393.                 else
  394.                 {
  395.                   pwm = 0;
  396.                 }
  397.                 if(X >= 10 && X <= 20)
  398.                 {
  399.                  count++;
  400.      if(count == 50)
  401.                  {
  402.                          count = 0;
  403.                    time++;
  404.                  }                  
  405.                  
  406.                 }
  407.                 else
  408.      {
  409.                     time = 0;
  410.                           count = 0;
  411.                  }
  412.                          
  413.         }
  414.        
  415.   count1++;
  416. }
复制代码

所有资料51hei提供下载:
风洞系统.7z (231.54 KB, 下载次数: 111)

评分

参与人数 2黑币 +55 收起 理由
kekekeya + 5
admin + 50 共享资料的黑币奖励!

查看全部评分

回复

使用道具 举报

ID:267330 发表于 2020-4-1 09:39 | 显示全部楼层
好东西
回复

使用道具 举报

ID:919766 发表于 2021-5-12 22:39 | 显示全部楼层
大哥,怎么接线啊  看你的超声波的引脚看的我懵逼了
回复

使用道具 举报

ID:846649 发表于 2021-7-14 20:13 | 显示全部楼层
楼主 请问一下 你风机的型号是什么
回复

使用道具 举报

您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

手机版|小黑屋|51黑电子论坛 |51黑电子论坛6群 QQ 管理员QQ:125739409;技术交流QQ群281945664

Powered by 单片机教程网

快速回复 返回顶部 返回列表