久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费

標題: STM32自平衡小車源代碼 [打印本頁]

作者: liukong    時間: 2017-3-16 23:19
標題: STM32自平衡小車源代碼
平衡小車代碼

全部源碼下載:
STM32自平衡小車源代碼.7z (231.18 KB, 下載次數: 100)


部分代碼預覽:
  1. #include "stm32f10x.h"
  2. #include "MPU6050.h"
  3. #include "motor.h"
  4. #include "control.h"
  5. #include <math.h>
  6. //******角度參數************

  7. float Gyro_y;        //Y軸陀螺儀數據暫存
  8. float Angle_gy;      //由角速度計算的傾斜角度
  9. float Accel_x;             //X軸加速度值暫存
  10. float Angle_ax;      //由加速度計算的傾斜角度
  11. float Angle;         //小車最終傾斜角度

  12. //******PWM參數*************

  13. int   speed_mr;                 //右電機轉速
  14. int   speed_ml;                 //左電機轉速
  15. float PWM;           //綜合PWM計算
  16. //******電機參數*************

  17. float speed_r_l;        //電機轉速
  18. float speed;        //電機轉速濾波
  19. //******卡爾曼參數************
  20.                
  21. const float Q_angle=0.001;  
  22. const float Q_gyro=0.003;
  23. const float R_angle=0.5;
  24. const float dt=0.01;                          //dt為kalman濾波器采樣時間;
  25. const char  C_0 = 1;
  26. float Q_bias, Angle_err;
  27. float PCt_0, PCt_1, E;
  28. float K_0, K_1, t_0, t_1;
  29. float Pdot[4] ={0,0,0,0};
  30. float PP[2][2] = { { 1, 0 },{ 0, 1 } };

  31. static const float Kp  = 35.0;                                //PID參數
  32. static const float Kd  = 2.5;                                        //PID參數
  33. static const float Ksp = 10.0;                                //PID參數

  34. void TIM4_Configuration(void)
  35. {
  36.   TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;//定義一個定時器結構體變量

  37.   RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);//使能定時器4
  38.   
  39.   TIM_TimeBaseStructure.TIM_Period = (10000 - 1);        //計數10000次,因為從0開始,所以減1
  40.   TIM_TimeBaseStructure.TIM_Prescaler = (72 - 1);        //時鐘72分頻,因為0不分頻,所以減1
  41.   TIM_TimeBaseStructure.TIM_ClockDivision = 0;        // 使用的采樣頻率之間的分頻比例
  42.    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;        //向上計數
  43.   TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);        //初始化定時器4

  44.   TIM_ClearITPendingBit(TIM4, TIM_IT_Update);                //清除定時器4中斷標志位
  45.   TIM_ITConfig(TIM4, TIM_IT_Update, ENABLE);                //打開定時器4中斷

  46.   TIM_Cmd(TIM4, ENABLE);  //計數器使能,開始計數
  47. }

  48. void NVIC_Configuration(void)
  49. {
  50.   NVIC_InitTypeDef NVIC_InitStructure;
  51.   
  52.   NVIC_SetVectorTable(NVIC_VectTab_FLASH, 0x0000);
  53.   NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);

  54. /********************注冊定時器4中斷處理函數***********************/

  55.   NVIC_InitStructure.NVIC_IRQChannel = TIM4_IRQn;
  56.   NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
  57.   NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
  58.   NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
  59.   NVIC_Init(&NVIC_InitStructure);

  60. /******************注冊串口1中斷服務函數************************/
  61.         NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;//配置串口中斷
  62.         NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;//占先式優先級設置為0
  63.         NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1; //副優先級設置為0
  64.         NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE;//中斷禁止
  65.         NVIC_Init(&NVIC_InitStructure);//中斷初始化


  66. }
  67. void Control_Init(void)
  68. {
  69.         TIM4_Configuration();
  70.         NVIC_Configuration();
  71. }
  72. /****************定時器4中斷服務函數***************************************/
  73. void TIM4_IRQHandler(void)
  74. {
  75.         if (TIM_GetITStatus(TIM4, TIM_IT_Update) != RESET)
  76.   {
  77.                 TIM_ClearITPendingBit(TIM4, TIM_IT_Update);

  78.                 Angle_Calculate();// 車體傾角計算
  79.                 Speed_Calculate();//車速計算
  80.                 PWM_Calculate();//電機PWM值計算
  81.                 Motor_Control(PWM);//調節電機參數

  82.         }
  83. }

  84. /*****************卡爾曼濾波**************************************************/
  85. void Kalman_Filter(float Accel,float Gyro)               
  86. {
  87.         Angle+=(Gyro - Q_bias) * dt; //先驗估計
  88.         
  89.         Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先驗估計誤差協方差的微分

  90.         Pdot[1]= -PP[1][1];
  91.         Pdot[2]= -PP[1][1];
  92.         Pdot[3]=Q_gyro;
  93.         
  94.         PP[0][0] += Pdot[0] * dt;   // Pk-先驗估計誤差協方差微分的積分
  95.         PP[0][1] += Pdot[1] * dt;   // =先驗估計誤差協方差
  96.         PP[1][0] += Pdot[2] * dt;
  97.         PP[1][1] += Pdot[3] * dt;
  98.                
  99.         Angle_err = Accel - Angle;        //zk-先驗估計
  100.         
  101.         PCt_0 = C_0 * PP[0][0];
  102.         PCt_1 = C_0 * PP[1][0];
  103.         
  104.         E = R_angle + C_0 * PCt_0;
  105.         
  106.         K_0 = PCt_0 / E;
  107.         K_1 = PCt_1 / E;
  108.         
  109.         t_0 = PCt_0;
  110.         t_1 = C_0 * PP[0][1];

  111.         PP[0][0] -= K_0 * t_0;                 //后驗估計誤差協方差
  112.         PP[0][1] -= K_0 * t_1;
  113.         PP[1][0] -= K_1 * t_0;
  114.         PP[1][1] -= K_1 * t_1;
  115.                
  116.         Angle        += K_0 * Angle_err;         //后驗估計
  117.         Q_bias        += K_1 * Angle_err;         //后驗估計
  118.         Gyro_y   = Gyro - Q_bias;         //輸出值(后驗估計)的微分=角速度

  119. }
  120. void Angle_Calculate(void)
  121. {
  122.         static        uint8_t DataBuffer[2];        //數據緩存

  123. /****************************加速度****************************************/
  124.         I2C_ReadBuffer(DataBuffer, ACCEL_XOUT_H, 2);
  125.         Accel_x  = (short)((DataBuffer[0]<<8)+DataBuffer[1]);          //讀取X軸加速度
  126.         Angle_ax = (Accel_x +220) /16384;   //去除零點偏移,計算得到角度(弧度)
  127.         Angle_ax = Angle_ax*1.2*180/3.14;     //弧度轉換為度,

  128. /****************************角速度****************************************/
  129.         I2C_ReadBuffer(DataBuffer, GYRO_YOUT_H, 2);
  130.         Gyro_y = (short)((DataBuffer[0]<<8)+DataBuffer[1]);              //靜止時角速度Y軸輸出為-18左右
  131.         Gyro_y = (Gyro_y + 18)/16.4;         //去除零點偏移,計算角速度值
  132.         //Angle_gy = Angle_gy + Gyro_y*0.01;  //角速度積分得到傾斜角度,因為卡爾曼計算帶有時間dt,所以此處不用積分

  133. /***************************卡爾曼融合*************************************/
  134.         Kalman_Filter(Angle_ax,Gyro_y);       //卡爾曼濾波計算傾角
  135.         
  136. /****************************互補濾波****************************************/

  137.         /***補償原理是取當前傾角和加速度獲得傾角差值進行放大,然后與
  138. ****陀螺儀角速度疊加后再積分,從而使傾角最跟蹤為加速度獲得的
  139. ****角度 0.5為放大倍數,可調節補償度;0.01為系統周期10ms        
  140. *************************************************************/

  141. //        Angle = Angle + (((Angle_ax-Angle)*0.5 + Gyro_y)*0.01);                //因為加或不加此語句處理效果一樣,故省略,原因搞不清楚。。。

  142. }
  143. void Speed_Calculate(void)
  144. {
  145.                 speed_mr=TIM_GetCounter(TIM2)-0x7fff;
  146.                 speed_ml=TIM_GetCounter(TIM3)-0x7fff;                //讀取編碼器寄存器計數值,并減去中間值,得到速度矢量
  147.                 TIM_SetCounter(TIM2, 0x7fff);
  148.                 TIM_SetCounter(TIM3, 0x7fff);                                                //重置編碼器計數值

  149.                 speed_r_l =(speed_mr + speed_ml)*0.5;
  150.                 speed *= 0.7;                                                          //車輪速度濾波
  151.                 speed += speed_r_l*0.3;        
  152.         
  153. }
  154. void PWM_Calculate(void)
  155. {
  156.                 if(Angle<-40||Angle>40)                //傾角過大就關閉電機,進入死循環,直到復位
  157.                 {
  158.                         Motor_Stop();
  159.                         while(1);
  160.                 }               
  161.                 PWM  = (short)(Kp*Angle + Kd*Gyro_y);          //PID:角速度和角度調節
  162.                  PWM += Ksp*speed;                                                                                                      //PID:車速度調節
  163. }
復制代碼



作者: cm7626    時間: 2017-4-24 08:35
平衡車的代碼夠復雜的 初學者就不用考慮了
作者: ddxx    時間: 2017-12-7 12:17
沒錢下載啊。
作者: lemon0210    時間: 2017-12-7 14:51
只有代碼嗎
作者: wj1994    時間: 2017-12-11 13:35
有人可以說一下有原理圖嗎?
作者: 張碭碭    時間: 2018-5-17 16:41
濾波原理




歡迎光臨 (http://m.zg4o1577.cn/bbs/) Powered by Discuz! X3.1
主站蜘蛛池模板: 日韩精品小视频 | 日韩免费在线观看视频 | 亚欧在线观看 | 日本黄a三级三级三级 | 欧美日韩在线精品 | 免费视频久久久 | 欧美精品二区三区四区免费看视频 | 国产伦精品一区二区三毛 | 国模精品视频一区二区 | av高清在线 | 精品视频在线免费观看 | 欧美一级久久 | 青青草视频网站 | 午夜免费观看视频 | 九九热在线观看 | 亚洲一区二区在线视频 | 国产精品伦子伦免费视频 | 天天操天天碰 | 黑人精品xxx一区一二区 | 国产免费高清视频 | 欧美一级在线观看 | 久久精品欧美一区二区 | 97精品国产97久久久久久免费 | 成人欧美激情 | 国产精品婷婷 | 黄色一级大片在线免费看国产一 | 免费久久久| 草草视频在线 | av免费网站 | 中文字幕在线观看网址 | 91女人18毛片水多国产 | 成av人片一区二区三区久久 | 午夜看片 | 久久久久女人精品毛片九一 | 久久久久人| 欧美日韩一区二区三区视频 | 久久激情视频 | 一本色道久久综合亚洲精品小说 | 欧美日韩在线免费 | 欧美一级做性受免费大片免费 | 一区二区不卡 |