標(biāo)題: STM32平衡小車(chē)的代碼 PID參數(shù)的調(diào)節(jié)和陀螺儀原始數(shù)據(jù)的處理 [打印本頁(yè)]
作者: 133456789    時(shí)間: 2020-12-29 12:34
標(biāo)題: STM32平衡小車(chē)的代碼 PID參數(shù)的調(diào)節(jié)和陀螺儀原始數(shù)據(jù)的處理
主要是PID參數(shù)的調(diào)節(jié)和陀螺儀原始數(shù)據(jù)的處理
遠(yuǎn)離DMP這種速度慢消耗大的方法，希望有點(diǎn)幫助

單片機(jī)源程序如下:
  1. #include "led.h"
  2. #include "delay.h"
  3. #include "key.h"
  4. #include "sys.h"
  5. #include "lcd.h"
  6. #include "usart.h"
  7. #include "mpu6050.h"   
  8. #include "inv_mpu.h"
  9. #include "inv_mpu_dmp_motion_driver.h"
  10. #include "oled.h"
  11. #include "pwm.h"
  12. #include "function.h"
  13. #include "fuzzy.h"
  14. #include "time.h"
  15. #include "FTM.h"
  16. #include "rtu.h"

  18. char receive;
  19. float result;
  20. angle ress;
  21. p_angle res;
  22. speed_s spe_L;
  23. speed_ss speed_L;
  24. speed_s spe_R;
  25. speed_ss speed_R;
  26. out_b outt;
  27. out_c out;
  28. int ab;
  29. float roll;
  30. int ti=9;
  31. float ta;
  32. int ti_r;
  33. int ti_l;
  34. float CN_timer2;
  35. float CN_timer3;
  36. int time;
  37. int32_t rtu_send[6];
  38. int main(void)
  39. {       
  40.         float mm=0;
  41.         int P=64;
  42.         float I=0;
  43.         float D=0.005;
  44.         float i;
  45.         float d;
  46.         float e;
  47.         int integer,point;
  48.         float pitch,yaw;                 //歐拉角
  49.         short aacx,aacy,aacz;                  //加速度傳感器原始數(shù)據(jù)
  50.         short gyrox,gyroy,gyroz;        //陀螺儀原始數(shù)據(jù)
  51.         short temp;                                              //溫度       
  52.         int m;
  53.   out=&outt;
  54.   res=&ress;
  55.         speed_L=&spe_L;
  56.         speed_R=&spe_R;
  57.         speed_L->speed_last=0;
  58.         speed_R->speed_last=0;
  59.         ta=0;
  60.         ti=80;
  61.         res->angle_sum=0;
  62.         res->angle_error=0;
  63.         res->angle_last=0;
  64.   res->angle_now=0;
  65.         m=0;
  66.   delay_init();        //延時(shí)初始化
  67.         OLED_Init();       
  68.         pwm_init();
  69.         time1_init();
  70.         uart_init(115200);
  71.         Hardware_init();
  72.         ti=0;
  73.        
  74.   ab=1000;
  75.   OLED_ShowString(40,2 ,"Stop");
  76.         while(1)
  77.         {
  78. //                if(ti<0)  
  79. //                          ti=0;
  80. //          
  81. //                if(ab>100)
  82. //                        ab=100;
  83. //                  ti=ab;
  84. //       
  85.                 if(ti<0)  ti=0;
  86.                 if(ti>100)  ti=100;
  87.                 if(ab<0)   ab=0;
  88.                 if(ab>0)   ab=100;
  89.                
  90.                         OLED_ShowString(0,0 ,"Direction:");
  91.                 OLED_ShowString(0,4 ,"Speed:");
  92.                 OLED_ShowNum(40,6,100-ti,3,24);
  93.                 OLED_ShowString(96,6 ,"%");
  94.                 //  OLED_ShowNum(0,2,ta,3,32);
  95.    switch (receive)
  96.                         {
  97.                                 case 'A':
  98.                                             ti+=5;
  99.                                       receive='p';
  100.                                          break;
  101.                            case 'B':
  102.                                             ti-=5;
  103.                                       receive='p';
  104.                                       break;
  105.                                 case 'C':
  106.                                             forward();
  107.                                       OLED_Clear();
  108.                                             OLED_ShowString(40,2 ,"Forward");
  109.                                       receive='p';
  110.                                       break;
  111.                                         case 'D':
  112.                                             back();
  113.                                       OLED_Clear();
  114.                                             OLED_ShowString(40,2 ,"Back");
  115.                                       receive='p';
  116.                                       break;
  117.                                         case 'E' :
  118.                                                   stop();
  119.                                             ti=0;;
  120.                                             OLED_Clear();
  121.                                             OLED_ShowString(40,2 ,"Stop");
  122.                                             receive='p';
  123.                                       break;
  124.                                   case 'F' :
  125.                                                   left();
  126.                                             OLED_Clear();
  127.                                             OLED_ShowString(40,2 ,"Turn_left");
  128.                                             receive='p';
  129.                                       break;
  130.                                         case 'G' :
  131.                                                   right();
  132.                                             OLED_Clear();
  133.                                             OLED_ShowString(40,2 ,"Turn_right");
  134.                                             receive='p';
  135.                                       break;
  136.                                                 case 'H' :
  137.                                                   up();
  138.                                                 ti=50;
  139.                                             OLED_Clear();
  140.                                             OLED_ShowString(40,2 ,"up");
  141.                                             receive='p';
  142.                                       break;
  143.                                                 case 'I' :
  144.                                                   down();
  145.                                                 ti=50;
  146.                                             OLED_Clear();
  147.                                             OLED_ShowString(40,2 ,"down");
  148.                                             receive='p';
  149.                                       break;
  150.                                                 case 'J' :
  151.                                                   ab-=5;
  152.                                             OLED_Clear();
  153.                                             receive='p';
  154.                                       break;
  155.                                                 case 'K' :
  156.                                                   ab+=5;
  157.                                             OLED_Clear();
  158.                                             receive='p';
  159.                                       break;
  160.                                        
  161.                         }
  162.         }
  163.         Hardware_init();       
  164.         uart_init(115200);
  165.         //time4_init();
  166.         usart_init_();
  167.         time1_init();
  168.         Encoder_Init_TIM2();
  169.         Encoder_Init_TIM3();
  170.         while(mpu_dmp_init())
  171.         {
  172.         OLED_ShowString(0,2 ,"MPU6050 initting");
  173.         }
  174.   OLED_Clear();
  175.         res->angle_P=81;
  176.         res->angle_I=0.001;
  177.         res->angle_sum=0;
  178.         res->angle_D=18;
  179.         mm=0;
  180.         OLED_ShowString(0,2 ,"roll:");
  181.         OLED_ShowString(2,0 ,"L:");
  182.         OLED_ShowString(60,0 ,"R:");         
  183.         OLED_ShowString(100,2 ,".");
  184.         OLED_ShowString(0,6 ,"P:");
  185.         OLED_ShowString(46,6,"I:");       
  186.         OLED_ShowString(90,6 ,"D:");
  187.                          while(1)
  188.         {
  189.                   switch (receive)
  190.                         {
  191.                                 case 'T':
  192.                                             ta=ta+1;
  193.                                       receive='p';
  194.                                          break;
  195.                                 case 'J':
  196.                                               ta-=1;
  197.                                       receive='p';
  198.                                          break;   
  199.         case 'A':
  200.               res->angle_P+=1;
  201.                receive='p';       
  202.                break;
  203.                                 case 'C':
  204.               res->angle_P-=1        ;
  205.                receive='p';
  206.                break;
  207.          case 'D':
  208.               res->angle_D+=1;
  209.                receive='p';       
  210.                break;
  211.                                 case 'E':
  212.                res->angle_D-=1;
  213.                receive='p';
  214.                break;                       
  215.                                  case 'F' :
  216.                          res->angle_I+=0.001;
  217.                                        receive = 'p';
  218.                            break;
  219.                     case 'B':
  220.                     back();
  221.                                      res->angle_I-=0.001;
  222.                            break;
  223.                      case 'S':
  224.                                              stop();
  225.                                               receive = 'p';
  226.                            break;         
  227.                                 case 'M':
  228.                      ti+=1;
  229.                                        receive = 'p';
  230.                            break;
  231.                      case 'N':
  232.                                               receive = 'p';
  233.                            ti-=1;
  234.                            break;       
  235.                                
  236.                         }
  237.            if(mpu_dmp_get_data(&pitch,&roll,&yaw)==0)
  238.                  {       
  239.                         MPU_Get_Accelerometer(&aacx,&aacy,&aacz);        //得到加速度傳感器數(shù)據(jù)
  240.                         MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);        //得到陀螺儀數(shù)據(jù)         
  241.                   temp=roll*10;         
  242.                 if(temp<0)
  243.                                   {
  244.                                      OLED_ShowString(50,2 ,"-");
  245.                                                  temp=-temp;
  246.                                   } else               
  247.                                           OLED_ShowString(50,2 ,"+");
  248.         integer = temp/10;
  249.         point   = temp%10;       
  250.                 time=(2000-(res->angle_P*mm+res->angle_I*res->angle_sum-res->angle_D*res->angle_error));
  251.         if(time<1)
  252.                                  {
  253.                                                 time=1;
  254.                                  }                                                 
  255.                 if(res->angle_now<0)
  256.                 {
  257.              forward();
  258.                          mm=-res->angle_now;
  259.                  }
  260.                  else
  261.                  {       
  262.                            back();
  263.                            mm=res->angle_now;
  264.                  }
  265.                  OLED_ShowNum(60,2,integer,3,18);
  266.                           OLED_ShowNum(108,2,point,1,8);       
  267.                     OLED_ShowNum(12,6,(res->angle_P),3,18);
  268.                     OLED_ShowNum(60,6,(res->angle_I)*1000,3,18);
  269.         OLED_ShowNum(102,6,res->angle_D,3,18);
  270.                           OLED_ShowNum(0,4,mm,3,18);               
  271.                 if(res->angle_error<0.5)
  272.                 {
  273.                       OLED_ShowString(68,4 ,"-");
  274.                       OLED_ShowNum(76,4,-res->angle_error,4,20);       
  275.                 }
  276.                         else
  277.                         {
  278.                             OLED_ShowString(68,4 ,"+");
  279.                             OLED_ShowNum(76,4,res->angle_error,4,20);
  280.                         }                               
  281. //                    OLED_ShowNum(20,0,(int)((s16)(TIM2->CNT)),3,16);         
  282. //                    OLED_ShowNum(80,0,(int)((s16)(TIM3->CNT)),3,16);          //顯示左右輪位置
  283.                         rtu_send[0]=res->angle_now*100;
  284.                         rtu_send[1]=2000-time;
  285.                         rtu_send_data(rtu_send,2);
  286.                   rtu_send_data(rtu_send,2);                    //上位機(jī)發(fā)送
  287.                         OLED_ShowNum(20,0,time,4,20);                    //顯示左右論速度
  288.                   OLED_ShowNum(80,0,time,4,20);
  289.                         while(1)
  290.                         {
  291.             GPIO_ResetBits(GPIOC,GPIO_Pin_13);
  292.                                         delay_ms(1000);                        //顯示左右論速度
  293.                         GPIO_SetBits(GPIOC,GPIO_Pin_13);
  294.                         delay_ms(1000);
  295.                         }
  296.                  }
  297.         }

  299. }
復(fù)制代碼

所有資料51hei提供下載:
兩輪平衡車(chē).7z (257.04 KB, 下載次數(shù): 56)
作者: yimaer    時(shí)間: 2024-7-30 14:07
請(qǐng)問(wèn)，為什么主函數(shù)里有2個(gè)while（1）呢？ switch (receive)，receive變量沒(méi)給初值，怎么進(jìn)的case語(yǔ)句呢



歡迎光臨 (http://www.torrancerestoration.com/bbs/) Powered by Discuz! X3.1