设为首页收藏本站
开启辅助访问 切换到宽版

 找回密码
 立即注册

QQ登录

只需一步，快速开始

热门： 51单片机 | 24小时必答区 | 单片机教程 | 单片机DIY制作 | STM32 | Cortex M3 | 模数电子 | 电子DIY制作 | 音响/功放 | 拆机乐园 | Arduino | 嵌入式OS | 程序设计

搜索
»论坛 › 嵌入式/单片机论坛 › STM32/8 › STM32 PID练习代码 风洞控制系统 通过超声波测距小乒乓 ...
返回列表 发新帖
查看: 4134|回复: 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"


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

  28. void LCD_Display(void);
  29. void Q_1(void);
  30. void Q_2(void);
  31. void Q_3(void);
  32. void Q_4(void);
  33. void Q_5(void);
  34. void Q_6(void);
  35. void Q_7(void);

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

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

  112. void PID_Control()
  113. {
  114.    pwm = 1520 + Kp*Err_X + Ki*Err_X_Sum + Kd*(Err_X - Err_X_Last);
  115.         //pwm += Kp*Err_X*0.02;
  116. }

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

  151.   }
  152.         TIM_ClearITPendingBit(TIM5,TIM_IT_Update);//清除中断标志
  153. }

  155. void Q_1()
  156. {
  157.   Aim_X = 15.0;
  158.         Kp = 80;
  159.         Ki = 0.3;
  160.         Kd = 200;
  161.         PID_Control();
  162.         if(X >= 10&&X <= 20)
  163.         {
  164.            count++;
  165.      if(count == 50)
  166.                  {
  167.                          count = 0;
  168.                    time++;
  169.                          
  170.                  }                         
  171.         }
  172.         else
  173.         {
  174.           count = 0;
  175.                 time = 0;
  176.         }
  177.        
  178.         //TIM_SetCompare1(TIM3,pwm);
  179. }

  181. void Q_2()
  182. {
  183.   Aim_X = 12.0;
  184.         Kp = 80;
  185.         Ki = 0.5;
  186.         Kd = 200;
  187.         PID_Control();
  188.         if(X >= 10&&X <= 20)
  189.         {
  190.            count++;
  191.      if(count == 50)
  192.                  {
  193.                          count = 0;
  194.                    time++;
  195.                  }                         
  196.         }
  197.         else
  198.         {
  199.           count = 0;
  200.                 time = 0;
  201.         }
  202. }

  204. void Q_3()
  205. {
  206.         u8 key;
  207.         if(count1 == 0)//第一次的PID参数和位置的初始化
  208.         {
  209.                 Aim_X = 20.0;
  210.                 Kp = 80;
  211.                 Ki = 0.3;
  212.                 Kd = 200;
  213.         }else
  214.         {
  215.           count1 = 1;
  216.         }
  217.         count1++;
  218.        
  219.         PID_Control();
  220.         key = KEY_Scan(0);
  221.         if(key == KEY0_PRES)
  222.         {
  223.                 Aim_X -= 1.0;
  224.                 Ki = 0.8;
  225.                 time = 0;
  226.         }
  227.         else if(key == KEY1_PRES)
  228.         {
  229.                 Aim_X += 1.0;
  230.                 Ki = 0.8;
  231.                 time = 0;
  232.         }
  233.        
  234.         if(fabs(Err_X) <= 1.0)
  235.         {
  236.            count++;
  237.      if(count == 50)
  238.                  {
  239.                          count = 0;
  240.                    time++;
  241.                  }                         
  242.         }
  243.        
  244. }

  246. void Q_5()
  247. {
  248.         if(count1 == 0)//第一次的PID参数和位置的初始化
  249.         {
  250.           pwm = 0;
  251.         }else
  252.         {
  253.           count1 = 1;
  254.                 if(pwm == 2600)
  255.          {
  256.            count++;
  257.      if(count == 50)
  258.                  {
  259.                          count = 0;
  260.                    time++;
  261.                  }                         
  262.          }
  263.                 pwm += 20;
  264.         }
  265.         count1++;
  266.        
  267. }

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

  390. void Q_7()
  391. {
  392.     if(count1 == 0)//第一次的PID参数和位置的初始化
  393.         {
  394.           pwm = 0;
  395.         }else
  396.         {
  397.           count1 = 1;
  398.                 if(distance<=41)                 
  399.           {
  400.                  
  401.                         Aim_X = 15.0;
  402.                         Kp = 80;
  403.                         Ki = 0.4;
  404.                         Kd = 220;
  405.                         PID_Control();
  406.           }
  407.                 else
  408.                 {
  409.                   pwm = 0;
  410.                 }
  411.                 if(X >= 10 && X <= 20)
  412.                 {
  413.                  count++;
  414.      if(count == 50)
  415.                  {
  416.                          count = 0;
  417.                    time++;
  418.                  }                  
  419.                  
  420.                 }
  421.                 else
  422.      {
  423.                     time = 0;
  424.                           count = 0;
  425.                  }
  426.                          
  427.         }
  428.        
  429.   count1++;
  430. }
复制代码

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

评分

参与人数 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 | 显示全部楼层
楼主 请问一下 你风机的型号是什么
回复

使用道具 举报

返回列表 发新帖
高级模式
B Color Image Link Quote Code Smilies
您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

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

Powered by 单片机教程网

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