自平衡小车--站立

leo121_3006061

有幸参加社区自平衡小车，经过一段时间的探索，前边也写了几个帖子，这个算是小总结吧。要想自平衡小车达到平衡的需要掌握4点（其实不止）
1.mpu6050的数据采集，及有效数据的提取，我至少一多半的时间消耗在这里了


2.重心的调整，也非常重要，我开始搞成了大头重，结果很难平衡，即使平衡也不稳定，经常像得了帕金森症一样，颤抖，让我想起了现在很火的舞蹈，非洲人穿着鸟服，在那抖动双脚，那感觉跟触电一样一样的

3.pid参数的调整，不同结构的小车P（比例系数）参数和D（微分系数）参数是不相同的，调整起来比较费时。

4.要考虑好嵌套终端优先级的管理，由于传感器比较多，就要考虑中断时间，优先级安排等。

以上4点我认为比较重要的环节，单独列出mpu6050，并不是说其他传感器不重要，其他传感器得到的数值都是直接可以使用的，不需要像mpu6050那样，折磨起人来花样繁多，并且数据需要经过处理才行。
经过n次的尝试和调整，把电池放下来能平衡的站立的时间比较久一些，但是也会跑，这个跑不是我加入了速度环，而是数据漂移的结果，前边的帖子里跟大神请教，有个比较不错的办法就是设定阈值（门限）。

可以先看一下视频，视频里电池拿下来了，站立比较稳，当然不能时间太久，就是前边提到的数据漂移的问题，时间太久小车自己就会跑了，抗干扰性差一些，用力推一下就倒了，倒下去过程电机有做挣扎，显然是扭矩不够。我看社区的朋友已经转向环都实现了，比较牛。

[视频]


先贴下加速度和陀螺仪计算的函数，主要是利用mpu6050采集到的数据进行融合过滤，提取有效数据（每次贴代码就有乱码）

1. void ACC_Ang_cal()
   
2. {
   
3. 
   
4.         MPU_Get_Accelerometer(&aacx,&aacy,&aacz);        //µÃµ½¼ÓËٶȴ«¸ÐÆ÷Êý¾Ý
   
5.         
   
6.         Ang_ACC=(180/Pi*(float)(atan2(aacz,aacy)));
   
7.         if(Ang_ACC>90)
   
8.                 Ang_ACC=90;
   
9.         if(Ang_ACC<-90)
   
10.                 Ang_ACC=-90;
    
11. 
    
12. }
    
13. void Gyro_Ang_cal()
    
14. {
    
15.                 int16_t Gyro_x_zero=0;  //ÍÓÂÝÒǾ²Ö¹Ê±XÖá½ÇËÙ¶È
    
16.                 float Gyro_x_speed_ratio=0.00029386; //ÍÓÂÝÒÇ»ý·ÖϵÊý
    
17.         float Gyro_x_speed;
    
18.    MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);        //µÃµ½ÍÓÂÝÒÇÊý¾Ý
    
19.         Car_Gyrox=(float)(gyrox*1.0-Gyro_x_zero); //xÖáÍÓÂÝÒÇËÙ¶È-¾²Ì¬Ê±Æ«²î
    
20.         Gyro_x_speed=(float)((gyrox*1.0-Gyro_x_zero)*Gyro_x_speed_ratio);
    
21.         Ang_Gyro=(float)(Ang_Gyro+(float)(Gyro_x_speed));
    
22.                
    
23. }
    

复制代码

在主函数代码，采用了互补滤波，这里关键部分实现了占空比转换，要用预分频的值作为分母，ControlPwm为分子，算出占空比，并且判断方向，控制电机正反转及速度

1. ...............设置sctimer pwm初始化及设置部分省略，看前边的帖子
   
2.                                                 ACC_Ang_cal();
   
3.                                                 Gyro_Ang_cal();
   
4.                                                 //PRINTF("Ang_ACC is  %d\t Ang_Gyro is %d\r\n",Ang_ACC,Ang_Gyro);
   
5.                                                 
   
6.                                                 /*»¥²¹Â˲£ balance filter  */
   
7.                                                 Ang_Gyro_tmp=Ang_Gyro+(Ang_ACC-Ang_Gyro)*0.00525;//0.00425Ϊ»¥²¹ÏµÊý£¬ÒÀ¾Ý²¨Ðε÷Õû
   
8.                                                         
   
9.                                                         Ang_Deviation=Ang_Gyro_tmp-Ang_zero;
   
10.                                                 ControlPwm=(int16_t)(P_up*Ang_Deviation+Car_Gyrox*D_up);
    
11.                                         printf("ControlPWM is %d\r\n",ControlPwm);
    
12.                                        
    
13.                                         if(ControlPwm>0)
    
14.                                                 direction=true;
    
15.                                         else{
    
16.                                                 direction=false;
    
17.                                         ControlPwm=-ControlPwm;
    
18.                                         }
    
19.                                         updatedDutycycle=ControlPwm/(sctimerClock/2400000);
    
20.                                         printf("updatedDutycycle is %d\r\n",updatedDutycycle);
    
21.                                         if(updatedDutycycle==0)
    
22.                                                 updatedDutycycle=1;
    
23.                                         if(updatedDutycycle>98)
    
24.                                                 updatedDutycycle=99;
    
25.                                         //printf("ControlPWM is %d\r\n",ControlPwm);        
    
26.                                         if(direction)
    
27.                                         {
    
28.             /* Update PWM duty cycle */
    
29.             SCTIMER_UpdatePwmDutycycle(SCT0, kSCTIMER_Out_4, (updatedDutycycle), eventNumberOutput4);
    
30.                                                 SCTIMER_UpdatePwmDutycycle(SCT0, kSCTIMER_Out_6, (updatedDutycycle), eventNumberOutput6);
    
31.                                                 
    
32.                                                 SCTIMER_UpdatePwmDutycycle(SCT0, kSCTIMER_Out_5, 1, eventNumberOutput5);
    
33.                                                 SCTIMER_UpdatePwmDutycycle(SCT0, kSCTIMER_Out_7, 1, eventNumberOutput7);
    
34.                                         }
    
35.                                         else
    
36.                                         {
    
37.                                                 SCTIMER_UpdatePwmDutycycle(SCT0, kSCTIMER_Out_5, (updatedDutycycle), eventNumberOutput5);
    
38.                                                 SCTIMER_UpdatePwmDutycycle(SCT0, kSCTIMER_Out_7, (updatedDutycycle), eventNumberOutput7);
    
39.                                                 
    
40.                                                 SCTIMER_UpdatePwmDutycycle(SCT0, kSCTIMER_Out_4, 1, eventNumberOutput4);
    
41.                                                 SCTIMER_UpdatePwmDutycycle(SCT0, kSCTIMER_Out_6, 1, eventNumberOutput6);
    
42.                                         }

复制代码

总结其实自平衡小车算法就这几种，也可以考虑用卡尔曼滤波代替互补滤波，据说效果更好，后边有时间可以做个实验。也可以拓展一下想法，别人用蓝牙/wifi控制小车，我们可以充分发挥这块板子的优势语音识别来进行声音控制，也可以考虑采用openmv（即将到来的rt1050可以发挥强大性能优势运行openmv），让小车进行主动跟踪，尽管这次任务马上要结束了，未来我还会继续更新，有条件把我的想法融入进来。由于我这边网络设置原因，上传视频比较慢，最后衷心感谢社区管管doatello帮忙代为上传视频。

阅读全文

qinbin-414122

好帖

doatello

视频已经帮你放进去啦~

nothingness

厉害厉害

okwh

祝贺！！

无薪税绵

围观大神作品。

leo121_3006061

感谢各位鼓励和支持

zhjb1

不错不错，站立起来了，只是突然轰然倒塌，反应都没有，没有看到常见的来回摆到直立现象。不过蛮有意思的。我也想申请一个玩玩，但已很难在此网站申请到了。围观客吧。

leo121_3006061

zhjb1 发表于 2017-11-8 15:58
不错不错，站立起来了，只是突然轰然倒塌，反应都没有，没有看到常见的来回摆到直立现象。不过蛮有意思的。 ...

有晃动，只是电池拿下来没有那么剧烈，另外仔细看看，是我推倒的，电机有反应的，抗干扰能力比较差，会逐步调校好我文中提到的几项参数，增加抗干扰能力。你想参与就长期关注社区活动，有很多这样的活动。感谢关注

小马哥-1650185

可以通过无线模块  控制小车