自平衡小车－5 结构、调试、平衡
 
 
　　话说到了最后阶段，我还是有些懵懂。
 
　　如图是我的小车的结构：　螺丝固定用一个废光盘作骨架平台，螺丝固定电池盒，没有三个18650电池并列，经升压模块升至10V,送电机驱动做电机电源; 用长虹的移动充电宝eleCloud做板子电源通过USB接头提供。如何固定呢？没办法，我只好用橡皮筋把其他的固定，MPU6050用橡皮筋固定在顶部中央。在连接轮子的底盘固定一长条硬纸板用于翻车时防撞坏。
 
　　总之，我这个　似乎算是橡皮筋捆绑结构。
 
 
试验结果：
　　先用MPU6050测定平衡点数据做为控制目标，PWM基于据他人经验把加速度、角速度在历史、当前、相互之间以各种权重合并为PID需要的两个变量，然后试验PID控制参数。PID参数按经验先调P参数，至观察到摆动，调整D参数至震动不大。或者说　这象一个阻尼过程，使得小车在偏离平衡位置时，通过轮子加速度产生一个力使其回复平衡位置。单车体、轮子全是动态，加速度、角速度等测定有各种干扰，调整好很有难度。据说这时机械专业的倒置单摆问题，是个变动的多力矩综合体系，要实现它需要精确的控制整体质量及其分布、重心位置及其变动范围、轮子加速及其产生的作用、失控条件等等。
 
 
视频：观察平衡
 
 
　　
https://v.youku.com/v_show/id_XMzEzNTc1OTY1Ng==.html?spm=a2h3j.8428770.3416059.1
 
　　最终自平衡最长时间约20秒，且不总是能成功，且还没视频下来，所以只能算办成功。经常问题是，摆动调整很快放大到向其中一侧偏转过大，轮子加速也调整不过来；而在摆动中，轮子启动、旋转、加速减速转动的阻力动力都是不规则的变化的，这还不说地面的摩察力也不固定，最终导致极易失衡。
 
 
视频：　原地平衡代码 ＋ 一个轮子PWM多一个固定的值，实现了小车旋转平衡，犹如一个陀螺。这倒是不会倒了！
 
 
 
https://v.youku.com/v_show/id_XMzEzNTc1NTM4MA==.html?spm=a2h3j.8428770.3416059.1
　　　
　　就我的装置，个人感觉电源部分太重了，轮子的起停变速带来的失衡后果大于对平衡的贡献，结果体系成了正反馈失衡体系了，而不是负反馈平衡体系。
 
估计原因：　1) 这个电机起停变速过于迟滞，也许需要大功率的电机；2)　当然我无法试验更大电机，　但我的装置似乎太重了，极易倾倒，或者说通过电机加速可调的变动范围太小。那么可能减轻重量应该可以。　3) 如果降低重心到轮轴甚至其下，那就近乎不倒翁结构了，应该就容易多了。
改进意见：换电池、降重量、降重心。
 
 
 
 
代码：　这方面好在很多，并无新奇之处。　以下问题存在：　DMP初始化失败。　单个加速度数据获取失败！，　MPU6050设备ID读回的是0x72不是0x68。　
 
计算MPU6050数据：　按模块安装方向测量　确定：　只需要　Acc_Y, gyro_X,其他变化不大忽略。
  1. void MP6050_DataTrans(void)
  2. {
  3.         static float gyro_y_last,acc_x_last;//低通滤波用参数
  4.         short  intacc_x,  intgyro_y , ay,az;
  5. //        MP6050_GetData(MPU_ACCEL_YOUTH_REG,&intacc_x);//读取y轴加速度     只需要y
  6.         MPU_Get_Accelerometer(&ay,&intacc_x,&az);
  7.         acc_x = 0.7*acc_x_last+0.3*intacc_x;                        //对加速度进行低通再滤波
  8.         acc_x_last=acc_x;
  9.         acc_x=acc_x-acc_x_errer;//减去零点漂移                  
  10.         angle_ax=acc_x/16384;//加速底转换成角度(弧度)     g unit       +-2g
  11.         angle_ax=angle_ax*180/3.14159;//弧度转换成角度   ????
  12.         
  13.         MP6050_GetData(MPU_GYRO_XOUTH_REG,&intgyro_y);//读取x轴角速度   只需要X
  14.         gyro_y = 0.7*gyro_y_last+0.3*intgyro_y;                        //对角速度进行低通滤波
  15.         gyro_y_last=gyro_y;        
  16.         gyro_y=gyro_y-gyro_y_errer;        //减去零点漂移
  17.         gyro_y=gyro_y /16.384;        //由陀螺仪值计算角速度         +- 2000 du/s  
  18.         angle_gy=angle_gy+gyro_y*0.004;                //角速度转换成角度                        4ms 测一次        ????                        +gyro_y*0.004 每使用？？
  19.         // 最后合并
  20. //  angle=FILTER_NUMBER*(angle+angle_gy*0.004)+(1-FILTER_NUMBER)*angle_ax;//一阶互补滤波,4ms一次   最后合并
  21.   angle=FILTER_NUMBER*(angle+gyro_y*0.004)+(1-FILTER_NUMBER)*angle_ax;//一阶互补滤波,4ms一次   最后合并
  22.    
  23. //         printf("%d,  %d\r\n",intacc_x,intgyro_y);
  24.  
  25. }
复制代码

 

在中断中　获得MPU6050测量数据，PD计算PWM, 更新PWM:
  1. void ctimer_match1_callback(uint32_t flags)
  2. {
  3.                 ms1++;
  4.                 if((ms1>= 3 )|(ms1== 1) ) //  2*4ms PID计算一次  6050 250Hz 4ms  一次采样
  5.                 {
  6.                         ms1=1;
  7.                         MP6050_DataTrans();
  8. //                        PWM = (int)(Kp*angle + Kd*gyro_y);                                          //角速度和角度PD     
  9.                         PWM = (int)(Kp*angle+ Kd*gyro_y);                                          //角速度和角度PD   *fabs(angle)/8
  10.         //                PWM = (int)(Kp*angle);                                          //角速度和角度PD   *fabs(angle)/8
  11.   //    printf("%f,  %f,  %f, %d\r\n", angle,angle_ax, angle_gy, PWM);
  12. //      printf("%d,  %d,  %d, %d\r\n", angle, angle_gy,gyro_y, PWM);
  13.                 }
  14.         //        return;
  15.  
  16.           //中断一次　变速一次   变速量（加速度）可调  自动加减速    /3
  17.         //  printf("%d\r\n", PWM);
  18.                 if(PWM >0)
  19.   sp = LOWSP + fabs(1.0*PWM/(ms1+1.5));    //10 step   only + 左右轮不同
  20.                 else 
  21.   sp = LOWSP + fabs(1.0*PWM/(ms1+1));    //10 step   only +   +0.2
  22.                         if (sp>upsp)        sp =  upsp;//pwm上限
  23.                 if(PWM <0)
  24.                 {
  25.   SCTIMER_StopTimer(SCT0, kSCTIMER_Counter_L);  // 修改SCTIMER_UpdatePwmDutycycle，提出StopTimer　StartTimer
  26.   SCTIMER_UpdatePwmDutycycle(SCT0, kSCTIMER_Out_6, sp/10 , event6);   //  右轮　前进  +ad 
  27.   SCTIMER_UpdatePwmDutycycle(SCT0, kSCTIMER_Out_7, 1, event7);
  28.   SCTIMER_UpdatePwmDutycycle(SCT0, kSCTIMER_Out_5, sp/10, event5); //  左轮　前进                +ad
  29.   SCTIMER_UpdatePwmDutycycle(SCT0, kSCTIMER_Out_4, 1, event4);
  30.   SCTIMER_StartTimer(SCT0, kSCTIMER_Counter_L);
  31.   GPIO->B[0][25] =0;  GPIO->B[0][26] =1; 　　　//指示等，可看到在平衡点附近闪烁
  32.                 }
  33.                 else
  34. {
  35.   SCTIMER_StopTimer(SCT0, kSCTIMER_Counter_L);
  36.   SCTIMER_UpdatePwmDutycycle(SCT0, kSCTIMER_Out_7, sp/10, event7);
  37.   SCTIMER_UpdatePwmDutycycle(SCT0, kSCTIMER_Out_6, 1, event6);   //  右轮　后退 +16  -ad +ad  
  38.   SCTIMER_UpdatePwmDutycycle(SCT0, kSCTIMER_Out_4, sp/10, event4);
  39.   SCTIMER_UpdatePwmDutycycle(SCT0, kSCTIMER_Out_5, 1, event5); //  左轮　后退
  40.   SCTIMER_StartTimer(SCT0, kSCTIMER_Counter_L);
  41.   GPIO->B[0][25] =1; GPIO->B[0][26] =0; 
  42.                 }        
复制代码

转弯、旋转只需要sp/10＋-ad,   ad =10~20即可