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

 找回密码
 立即注册

QQ登录

只需一步，快速开始

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

搜索
»论坛 › 嵌入式/单片机论坛 › 51单片机 › 51单片机平衡小车6轴MPU6050+互补滤波+PWM电机参考程序
返回列表 发新帖
查看: 4372|回复: 6
收起左侧

51单片机平衡小车6轴MPU6050+互补滤波+PWM电机参考程序

[复制链接]
ID:479765 发表于 2019-6-1 11:55 | 显示全部楼层 |阅读模式
51平衡小车 mpu650 参考程序

单片机源程序如下:
  1. /***********************************************************************
  2. // 两轮自平衡车最终版控制程序（6轴MPU6050+互补滤波+PWM电机）
  3. // 单片机STC12C5A60S2
  4. // 晶振：20M
  5. ***********************************************************************/

  7. #include <REG52.H>        
  8. #include <math.h>     
  9. #include <stdio.h>   
  10. #include <INTRINS.H>

  12. typedef unsigned char  uchar;
  13. typedef unsigned short ushort;
  14. typedef unsigned int   uint;

  16. //******功能模块头文件*******

  18. #include "DELAY.H"                    //延时头文件
  19. #include "STC_ISP.H"            //程序烧录头文件
  20. #include "SET_SERIAL.H"                //串口头文件
  21. #include "SET_PWM.H"                //PWM头文件
  22. #include "MOTOR.H"                        //电机控制头文件
  23. #include "MPU6050.H"                //MPU6050头文件



  27. //******角度参数************

  29. float Gyro_y;        //Y轴陀螺仪数据暂存
  30. float Angle_gy;      //由角速度计算的倾斜角度
  31. float Accel_x;             //X轴加速度值暂存
  32. float Angle_ax;      //由加速度计算的倾斜角度
  33. float Angle;         //小车最终倾斜角度
  34. uchar value;                 //角度正负极性标记        

  36. //******PWM参数*************

  38. int   speed_mr;                 //右电机转速
  39. int   speed_ml;                 //左电机转速
  40. int   PWM_R;         //右轮PWM值计算
  41. int   PWM_L;         //左轮PWM值计算
  42. float PWM;           //综合PWM计算
  43. float PWMI;                         //PWM积分值

  45. //******电机参数*************

  47. float speed_r_l;        //电机转速
  48. float speed;        //电机转速滤波
  49. float position;            //位移

  51. //******蓝牙遥控参数*************
  52. uchar remote_char;
  53. char  turn_need;
  54. char  speed_need;

  56. //*********************************************************
  57. //定时器100Hz数据更新中断
  58. //*********************************************************

  60. void Init_Timer1(void)        //10毫秒@20MHz,100Hz刷新频率
  61. {
  62.         AUXR &= 0xBF;                //定时器时钟12T模式
  63.         TMOD &= 0x0F;                //设置定时器模式
  64.         TMOD |= 0x10;                //设置定时器模式
  65.         TL1 = 0xE5;                    //设置定时初值
  66.         TH1 = 0xBE;                    //设置定时初值
  67.         TF1 = 0;                    //清除TF1标志
  68.         TR1 = 1;                    //定时器1开始计时
  69. }



  73. //*********************************************************
  74. //中断控制初始化
  75. //*********************************************************

  77. void Init_Interr(void)         
  78. {
  79.         EA = 1;     //开总中断
  80.     EX0 = 1;    //开外部中断INT0
  81.     EX1 = 1;    //开外部中断INT1
  82.     IT0 = 1;    //下降沿触发
  83.     IT1 = 1;    //下降沿触发
  84.         ET1 = 1;    //开定时器1中断
  85. }



  89. //******卡尔曼参数************
  90.                
  91. float code Q_angle=0.001;  
  92. float code Q_gyro=0.003;
  93. float code R_angle=0.5;
  94. float code dt=0.01;                          //dt为kalman滤波器采样时间;
  95. char  code C_0 = 1;
  96. float xdata Q_bias, Angle_err;
  97. float xdata PCt_0, PCt_1, E;
  98. float xdata K_0, K_1, t_0, t_1;
  99. float xdata Pdot[4] ={0,0,0,0};
  100. float xdata PP[2][2] = { { 1, 0 },{ 0, 1 } };


  103. //*********************************************************
  104. // 卡尔曼滤波
  105. //*********************************************************

  107. //Kalman滤波，20MHz的处理时间约0.77ms；

  109. void Kalman_Filter(float Accel,float Gyro)               
  110. {
  111.         Angle+=(Gyro - Q_bias) * dt; //先验估计

  113.         
  114.         Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分

  116.         Pdot[1]=- PP[1][1];
  117.         Pdot[2]=- PP[1][1];
  118.         Pdot[3]=Q_gyro;
  119.         
  120.         PP[0][0] += Pdot[0] * dt;   // Pk-先验估计误差协方差微分的积分
  121.         PP[0][1] += Pdot[1] * dt;   // =先验估计误差协方差
  122.         PP[1][0] += Pdot[2] * dt;
  123.         PP[1][1] += Pdot[3] * dt;
  124.                
  125.         Angle_err = Accel - Angle;        //zk-先验估计
  126.         
  127.         PCt_0 = C_0 * PP[0][0];
  128.         PCt_1 = C_0 * PP[1][0];
  129.         
  130.         E = R_angle + C_0 * PCt_0;
  131.         
  132.         K_0 = PCt_0 / E;
  133.         K_1 = PCt_1 / E;
  134.         
  135.         t_0 = PCt_0;
  136.         t_1 = C_0 * PP[0][1];

  138.         PP[0][0] -= K_0 * t_0;                 //后验估计误差协方差
  139.         PP[0][1] -= K_0 * t_1;
  140.         PP[1][0] -= K_1 * t_0;
  141.         PP[1][1] -= K_1 * t_1;
  142.                
  143.         Angle        += K_0 * Angle_err;         //后验估计
  144.         Q_bias        += K_1 * Angle_err;         //后验估计
  145.         Gyro_y   = Gyro - Q_bias;         //输出值(后验估计)的微分=角速度

  147. }



  151. //*********************************************************
  152. // 倾角计算（卡尔曼融合）
  153. //*********************************************************

  155. void Angle_Calcu(void)         
  156. {
  157.         //------加速度--------------------------

  159.         //范围为2g时，换算关系：16384 LSB/g
  160.         //角度较小时，x=sinx得到角度（弧度）, deg = rad*180/3.14
  161.         //因为x>=sinx,故乘以1.3适当放大

  163.         Accel_x  = GetData(ACCEL_XOUT_H);          //读取X轴加速度
  164.         Angle_ax = (Accel_x - 1100) /16384;   //去除零点偏移,计算得到角度（弧度）
  165.         Angle_ax = Angle_ax*1.2*180/3.14;     //弧度转换为度,


  168.     //-------角速度-------------------------

  170.         //范围为2000deg/s时，换算关系：16.4 LSB/(deg/s)

  172.         Gyro_y = GetData(GYRO_YOUT_H);              //静止时角速度Y轴输出为-30左右
  173.         Gyro_y = -(Gyro_y + 30)/16.4;         //去除零点偏移，计算角速度值,负号为方向处理
  174.         //Angle_gy = Angle_gy + Gyro_y*0.01;  //角速度积分得到倾斜角度.        

  176.         
  177.         //-------卡尔曼滤波融合-----------------------

  179.         Kalman_Filter(Angle_ax,Gyro_y);       //卡尔曼滤波计算倾角


  182.         /*//-------互补滤波-----------------------

  184.         //补偿原理是取当前倾角和加速度获得倾角差值进行放大，然后与
  185.     //陀螺仪角速度叠加后再积分，从而使倾角最跟踪为加速度获得的角度
  186.         //0.5为放大倍数，可调节补偿度；0.01为系统周期10ms        
  187.                
  188.         Angle = Angle + (((Angle_ax-Angle)*0.5 + Gyro_y)*0.01);*/
  189.                                                                                                                           
  190. }  



  194. //*********************************************************
  195. //电机转速和位移值计算
  196. //*********************************************************

  198. void Psn_Calcu(void)         
  199. {
  200.         
  201.         speed_r_l =(speed_mr + speed_ml)*0.5;
  202.         speed *= 0.7;                                  //车轮速度滤波
  203.         speed += speed_r_l*0.3;        
  204.         position += speed;                          //积分得到位移
  205.         position += speed_need;
  206.         if(position<-6000) position = -6000;
  207.         if(position> 6000) position =  6000;

  209.          
  210. }


  213. static float code Kp  = 9.0;       //PID参数
  214. static float code Kd  = 2.6;            //PID参数
  215. static float code Kpn = 0.01;      //PID参数
  216. static float code Ksp = 2.0;            //PID参数

  218. //*********************************************************
  219. //电机PWM值计算
  220. //*********************************************************

  222. void PWM_Calcu(void)         
  223. {
  224.    
  225.         if(Angle<-40||Angle>40)               //角度过大，关闭电机
  226.         {  
  227.           CCAP0H = 0;
  228.       CCAP1H = 0;
  229.           return;
  230.         }
  231.         PWM  = Kp*Angle + Kd*Gyro_y;          //PID：角速度和角度
  232.         PWM += Kpn*position + Ksp*speed;      //PID：速度和位置
  233.         PWM_R = PWM + turn_need;
  234.         PWM_L = PWM - turn_need;
  235.         PWM_Motor(PWM_L,PWM_R);
  236.          
  237. }




  242. //*********************************************************
  243. //手机蓝牙遥控
  244. //*********************************************************

  246. void Bluetooth_Remote(void)         
  247. {

  249.         remote_char = receive_char();                                   //接收蓝牙串口数据

  251.         if(remote_char ==0x02) speed_need = -80;           //前进
  252.         else if(remote_char ==0x01) speed_need = 80;   //后退
  253.              else speed_need = 0;                                           //不动

  255.     if(remote_char ==0x03) turn_need = 15;                   //左转
  256.         else if(remote_char ==0x04) turn_need = -15;   //右转
  257.              else turn_need = 0;                                           //不转
  258.          
  259. }


  262. /*=================================================================================*/

  264. //*********************************************************
  265. //main
  266. //*********************************************************
  267. void main()
  268. {

  270.         delaynms(500);           //上电延时
  271.         Init_PWM();               //PWM初始化
  272.     Init_Timer0();     //初始化定时器0，作为PWM时钟源
  273.         Init_Timer1();     //初始化定时器1
  274.         Init_Interr();     //中断初始化
  275.         Init_Motor();           //电机控制初始化
  276.         Init_BRT();                   //串口初始化（独立波特率）
  277.         InitMPU6050();     //初始化MPU6050
  278.         delaynms(500);            

  280.         while(1)
  281.         {
  282.            
  283.          Bluetooth_Remote();

  285.         }
  286. }


  289. /*=================================================================================*/

  291. //********timer1中断***********************

  293. void Timer1_Update(void) interrupt 3
  294. {
  295.   
  296.    TL1 = 0xE5;                    //设置定时初值10MS
  297.    TH1 = 0xBE;
  298.    
  299.    //STC_ISP();                    //程序下载
  300.    Angle_Calcu();                  //倾角计算
  301.    Psn_Calcu();                    //电机位移计算
  302.    PWM_Calcu();                    //计算PWM值
  303.    
  304.    speed_mr = speed_ml = 0;         

  306. }


  309. //********右电机中断***********************

  311. void INT_L(void) interrupt 0
  312. {

  314.    if(SPDL == 1)  { speed_ml++; }                 //左电机前进
  315.    else                      { speed_ml--; }                 //左电机后退
  316.    LED = ~LED;

  318. }


  321. //********左电机中断***********************

  323. void INT_R(void) interrupt 2
  324. {

  326.    if(SPDR == 1)  { speed_mr++; }                 //右电机前进
  327.    else                      { speed_mr--; }                 //右电机后退
  328.    LED = ~LED;

  330. }
复制代码

所有资料51hei提供下载:
51_MPU6050_Kalman_PWM_remote.rar (56.26 KB, 下载次数: 146)

评分

参与人数 1黑币 +50 收起 理由
admin + 50 共享资料的黑币奖励！

查看全部评分

回复

使用道具 举报

ID:205015 发表于 2019-7-3 09:00 | 显示全部楼层
一个月了，竟然无人来顶贴，，楼主是否成功了，后续交流学习下哦。
回复

使用道具 举报

ID:591400 发表于 2019-7-27 19:53 | 显示全部楼层
角度漂移情况如何？
回复

使用道具 举报

ID:233015 发表于 2019-8-24 08:07 来自手机 | 显示全部楼层
顶，楼主是否成功了。
回复

使用道具 举报

ID:479765 发表于 2019-8-24 12:07 | 显示全部楼层
brave_ruan 发表于 2019-8-24 08:07
顶，楼主是否成功了。

我用stm32做了 已成功
回复

使用道具 举报

ID:253767 发表于 2019-8-25 07:40 | 显示全部楼层
谢谢分享！！！
回复

使用道具 举报

ID:479765 发表于 2019-8-25 12:25 | 显示全部楼层
http://www.51hei.com/bbs/dpj-168542-1.html
回复

使用道具 举报

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

本版积分规则

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

Powered by 单片机教程网

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