21ic电子技术开发论坛 › 开源创客 › 机器人 › 两轮自平衡小车源代码（24楼）
123456下一页
返回列表 发新帖我要提问

两轮自平衡小车源代码（24楼）

[复制链接]
 楼主| zoomone 发表于 2011-10-10 21:11 | 显示全部楼层 |阅读模式
平衡小车
以前调平衡时用的别人编的小工具，感觉不是太复杂，于是心血来潮打算自己编个，刚好借这个机会学一下VC。

自己试着还挺好用，这是我拍的一个视频  http://player.youku.com/player.php/sid/XMzExNTU0MzI0/v.swf

本帖子中包含更多资源

您需要 登录 才可以下载或查看，没有账号？注册

×
回复 赏

举报

相关帖子
 楼主| zoomone 发表于 2011-10-10 21:21 | 显示全部楼层
单片机那边每60ms发送一次数据（其他时间间隔应该也可以），发送格式如下：
复制
  1.  
    

  2. USART_T(angle*57.3);      //角度     angle单位是弧度，乘以57.3变成角度
    

  3. USART_T(angle_dot*57.3);  //角速度
    

  4. USART_T(255);             //这个255是一定要发送的，作为一次发送的结束标志
回复 赏

举报

goodboy3021 发表于 2011-10-11 07:02 | 显示全部楼层
谢谢楼上，能否把你的串口通信部分发上来参考一下，调试好几天总出错！
回复 赏

举报

Cortex-M0 发表于 2011-10-11 10:21 | 显示全部楼层
回复 赏

举报

Cortex-M0 发表于 2011-10-11 10:23 | 显示全部楼层
做的不错，转贴过来~~~

要是能增加波特率可任意设置，就更完美了~~~
回复 赏

举报

 楼主| zoomone 发表于 2011-10-11 10:24 | 显示全部楼层
本帖最后由 zoomone 于 2011-10-11 10:45 编辑

代码都是对的，后面的注释可能有出入，我没有仔细核实，以代码为准。
复制
  1. CLKCON=0x2c;  //内部16.6M，两分频后8.3Mhz
复制
  1. USART_INI()
    

  2. {
    

  3. SCON=0X50;
    

  4. T2MOD=0X80;           //不对sysclk再分频
    

  5. T2CON=0X30;           //收发都以t2计数
    

  6. RCAP2H=0XFF;
    

  7. RCAP2L=0XE5;   //波特率9600，计算得来，非查表
    

  8. TR2=1;                   //开T2
    

  9. }
复制
  1. void USART_T( unsigned char dat )
    

  2. { 
    

  3. SBUF=dat;
    

  4. while(!TI);
    

  5. TI=0;
    

  6. }
回复 赏

举报

 楼主| zoomone 发表于 2011-10-11 10:37 | 显示全部楼层
5# Cortex-M0

自制个小工具，没想整的太麻烦:)
回复 赏

举报

 楼主| zoomone 发表于 2011-10-11 11:07 | 显示全部楼层
本帖最后由 zoomone 于 2011-10-11 11:11 编辑

我把我的滤波函数帖上来。卡尔曼太大了，所以用的互补滤波。

也是参考别人的，不过参数是改过的（这些参数是我随意改的，有点歪打正着，其实滤波原理我也没仔细想，明白的童鞋就表笑话:lol），大概需要3-5秒钟才能稳定。这个滤波貌似不能自动将平衡点温漂归零，但是利用它将角度和角速度结合到一起了，提高了角度的响应速度。

使用时，先用角度监视工具查出静止时滤波后的温漂值，然后手动在角速度传感器模数转化值上加或减去这个温漂值，达到静态时角速度输出为0的目的。
复制
  1. //-------------------------------------------------------
    

  2. //互补滤波
    

  3. //-------------------------------------------------------
    

  4. static float angle,angle_dot;                 //外部需要引用的变量
    

  5. //-------------------------------------------------------
    

  6. static float bias_cf;
    

  7. static const float dt=0.01;
    

  8. //-------------------------------------------------------
    

  9. void complement_filter(float angle_m_cf,float gyro_m_cf)
    

  10. {
    

  11.         bias_cf*=0.0001;                                  bias_cf+=gyro_m_cf*0.009;                  
    

  12.         angle_dot=gyro_m_cf-bias_cf;                   
    

  13.         angle=(angle+angle_dot*dt)*0.95+angle_m_cf*0.04;        
    

  14.       }        
回复 赏

举报

 楼主| zoomone 发表于 2011-10-12 12:40 | 显示全部楼层
角度、角速度AD转换函数
复制
  1. static float acceler,gyro;        
    

  2. void AD_calculate(void)
    

  3. {
    


  4. 


  5. 

  6. gyro=0.011557*ADC(1)-3.035+105*3.14/180;        //后面105*3.14/180是对角速度零点的修正
    


  7. 

  8. acceler=0.003052*ADC(0)-1.4375;                //计算角度时，只用了加速度传感器的Z轴就够了
    

  9.                                                                         
    

  10. acceler=acceler*1.5+14*3.14/180;  //没有使用arcsin（），因为sin在0度附近变化时可以用sin值代替角度值，这样可以减轻单片机的计算负担。
    

  11.   //*1.5是对角度的适当放大，14*3.14/180是对角度零点的修正
    

  12.                                           
    

  13.    complement_filter(acceler,gyro);
    


  14. 

  15. }
回复 赏

举报

 楼主| zoomone 发表于 2011-10-13 18:02 | 显示全部楼层
本帖最后由 zoomone 于 2011-10-13 18:06 编辑

电机转速计算相关函数

中断初始化
复制
  1. void INT_INI()
    

  2. {
    

  3. EX1=1;
    

  4. EX0=1;           //开中断
    

  5. IT0=1;          //边沿触发
    

  6. IT1=1;
    

  7. PX0L   = 1;                                   // 设置 INT0为高级中断 
    

  8. PX1L   = 1;        
    

  9. }
计数函数
复制

  1. 

  2. void INT_L(void) interrupt 0  using 1
    

  3. {        
    

  4. if(MA==1)
    

  5. {
    

  6.   speed_real_LH++;
    

  7. }
    

  8. else
    

  9. speed_real_LH--;
    

  10. }
    


  11. 


  12. 

  13. void INT_R(void) interrupt 2  using 1
    

  14. {
    

  15. if(MA==1)  //只站立，不跑。所以两轮的转速方向参考相同。 #define MA P4_0
    

  16. {
    

  17.   speed_real_RH++;
    

  18. }
    

  19. else
    

  20. speed_real_RH--;
    

  21. }]
回复 赏

举报

 楼主| zoomone 发表于 2011-10-14 21:14 | 显示全部楼层
本帖最后由 zoomone 于 2011-10-14 21:18 编辑
复制
  1. //-------------------------------------------------------
    

  2. //计算PWM输出值
    

  3. //-------------------------------------------------------        
    

  4. static float K_voltage,K_angle,K_angle_dot,K_position,K_position_dot; //放大比例参数；这里是在main（）函数中对它们初始化
    

  5. static float K_angle_AD,K_angle_dot_AD,K_position_AD,K_position_dot_AD;//辅助放大比例参数
    

  6. static float position,position_dot;
    

  7. static float position_dot_filter;
    

  8. static float PWM;
    

  9.    
    

  10. //-------------------------------------------------------        
    

  11. void PWM_calculate(void)        
    

  12. {
    


  13. 

  14.         K_angle_AD=ADC(2)*0.009;//辅助参数由变阻器ad转化得来，便于调节
    

  15.         K_angle_dot_AD=ADC(3)*0.009;
    

  16.         K_position_AD=ADC(4)*0.009;
    

  17.         K_position_dot_AD=ADC(5)*0.009;
    


  18. 

  19.         
    

  20.         position_dot=(speed_real_LH+speed_real_RH)*0.5;//position_dot是电机转速
    


  21. 

  22.         position_dot_filter*=0.85;                //车轮速度滤波
    

  23.         position_dot_filter+=position_dot*0.15;        
    

  24.         
    

  25.         position+=position_dot_filter;//position电机转过的角度，由position_dot累加而来
    


  26. 

  27.         position+=Speed_Need;
    

  28.         
    

  29.         if (position<-768)                //防止位置误差过大导致的不稳定
    

  30.         {
    

  31.                 position=-768;
    

  32.         }
    

  33.         else if  (position>768)
    

  34.         {
    

  35.                 position=768;
    

  36.         }
    


  37. 

  38.                 PWM = K_angle*angle *K_angle_AD + K_angle_dot*angle_dot *K_angle_dot_AD  +K_position*position *K_position_AD + _position_dot*position_dot_filter *K_position_dot_AD;//PWM是由四个参数累加而来，如果只要求平衡，可以不要position
    


  39. 

  40.         PWM_output (PWM);        
    

  41. }
pwm输出函数
复制
  1. //-------------------------------------------------------
    

  2. //PWM输出
    

  3. //-------------------------------------------------------
    

  4. /*#define MA P4_0
    

  5. #define MB P4_1
    

  6. #define MC P4_2
    

  7. #define MD P3_7*/
    


  8. 

  9. unsigned int PWM_10bit;        // 10位 PWM 输出缓冲区
    

  10. void PWM_output (int PWM_LH)
    

  11. {
    

  12.         if (PWM_LH<0)
    

  13.         {
    

  14.                 FW;     //#define FW MA=1;MB=0;MC=1;MD=0
    


  15. 

  16.                 PWM_LH*=-1;
    

  17.         }
    

  18.         else
    

  19.         {
    

  20.                 BW;   //#define BW MA=0;MB=1;MC=0;MD=1
    

  21.         }
    

  22.         
    

  23.         if (PWM_LH>252)
    

  24.         {
    

  25.                 PWM_LH=252;
    

  26.         }
    

  27.         
    


  28. 

  29.         PWM_10bit=4*PWM_LH;  //因为PWM原为8为pwm计算得来，这里重新定义10位pwm输出，所以，乘以4   
    

  30.                                                 //10位pwm获得方法参考网友Cortex-M0的https://bbs.21ic.com/frame.php?frameon=yes&referer=http%3A//bbs.21ic.com/iclist-11.html
    

  31. }
回复 赏

举报

goodboy3021 发表于 2011-10-14 21:19 | 显示全部楼层
谢谢楼主小朋友分享！！！
回复 赏

举报

 楼主| zoomone 发表于 2011-10-14 21:20 | 显示全部楼层
参考网友Cortex-M0的10位pwm计算函数
复制
  1. void PWM_10(void) interrupt 12          using 1
    

  2. { static unsigned int PWM_10bit_bak;             // 10位 PWM 输出缓冲发送区
    

  3.   static unsigned char PWM_Counter=0;                 // 10位 PWM 输出发送个数计数器
    

  4.   if(PWM_Counter == 0)
    

  5.     { PWM_10bit_bak = PWM_10bit & 0x3ff;         // 10位 PWM 发送开始，装入10位 PWM        初值至输出缓冲发送区
    

  6.       
    

  7.         }
    

  8.   if(PWM_10bit_bak >= 26)
    

  9.     { PWM_10bit_bak -= 26;                                         // 10位 PWM输出缓冲发送区 发送数据>=26, 数据减26暂存
    

  10.           PWMD = 0xff;                                                          // 10位 PWM 发送一次数据0xff
    

  11.         } 
    

  12.   else if(PWM_10bit_bak == 0)                                  // 10位 PWM输出缓冲发送区 发送数据=0
    

  13.     { PWMD = 0x00;                                                          // 10位 PWM 发送一次数据0x00
    

  14.         } 
    

  15.   else 
    

  16.     { PWM_10bit_bak = 0;                                         // 10位 PWM输出缓冲发送区 发送数据<26, 数据乘10，发送之
    

  17.           PWMD = (unsigned char)(PWM_10bit_bak * 10);   
    

  18.         } 
    

  19.   if(++PWM_Counter > 40)
    

  20.         { PWM_Counter = 0;                                                 // 10位 PWM 输出发送个数计数值大于40, 清0 
    

  21.         }
    

  22.   PWMCON &= 0XFD;                   // 清除 PWM 周期计数器溢出标志 PWMIF 
    

  23.  
    

  24. }
回复 赏

举报

Cortex-M0 发表于 2011-10-15 05:30 | 显示全部楼层
呵呵~~~

顶zoomone小盆友细心的讲解，二姨家的菜鸟有福了~~~

不过 sh88f2051 的单路硬件8位PWM 输出，扩展成上述的单路10位硬件pwm计算函数，现在已过时落伍了~~~

最新进展:
不但将 sh88f2051 的单路硬件8位PWM 输出，扩展成双路独立控制的10位硬件pwm计算函数，而且还有效的将两路独立的编码盘测速反馈信号及电机电压反馈信号加入，和双路独立的10位pwm控制信号，一起进入那个淘浆糊的PID算法函数，最终控制两路小电机~~~

结论：史上最“烂”的山寨级 双路伺服电机控制及驱动器 诞生了~~~
回复 赏

举报

Cortex-M0 发表于 2011-10-15 05:33 | 显示全部楼层
无图无真相，先上个用匠人的《串口猎人》的监控图，详细资料整理后上传~~~






本帖子中包含更多资源

您需要 登录 才可以下载或查看，没有账号？注册

×
回复 赏

举报

Cortex-M0 发表于 2011-10-15 05:46 | 显示全部楼层
解释：
用匠人的《串口猎人》依次发送 两路相位相差 90度的 电机控制信号，并定时发送给 sh88f2051系统，再将控制器内监测信号上传给《串口猎人》，通道 No. 0 - 5 的显示波型，依次为 左、右路电压测速信号， 左、右路PWM实际控制电机信号（并非输入控制信号），左、右路编码盘测速反馈信号~~~
回复 赏

举报

 楼主| zoomone 发表于 2011-10-15 09:44 | 显示全部楼层
14# Cortex-M0

单路硬件8位PWM 输出扩展成双路独立控制的10位硬件pwm  牛！

看来应了那句话，只有想不到没有做不到。
回复 赏

举报

奇兵 发表于 2011-10-16 12:33 | 显示全部楼层
呵呵不错的！
回复 赏

举报

 楼主| zoomone 发表于 2011-10-16 20:07 | 显示全部楼层
18# 奇兵
天冷了，添条新裤子
回复 赏

举报

 楼主| zoomone 发表于 2011-10-16 21:06 | 显示全部楼层
T1负责每10ms，对数据更新、输出一次
复制
  1. T1_INI()
    

  2. {
    

  3. TMOD|=0X10;                //方式1,16位
    

  4. //TCON1=0X00;                12分频
    

  5. TH1=0Xf8;
    

  6. TL1=0XFA;
    

  7. TR1=1;
    

  8. ET1=1;           //  允许T1中断
    

  9. }
复制
  1. unsigned char i=0;
    

  2. void T1_isr(void) interrupt 3
    

  3. {
    


  4. 

  5. TH1=0XE4;
    

  6. TL1=0XFA;
    


  7. 

  8. AD_calculate();
    

  9. PWM_calculate();
    


  10. 


  11. 

  12. i++;
    

  13. if(i==6)
    

  14. {
    

  15. i=0;
    

  16.         
    

  17.                         USART_T(angle*57.3);  
    

  18.                         USART_T(angle_dot*57.3);
    

  19.                           USART_T(255);
    

  20. }                        
    

  21. speed_real_RH =0;
    

  22. speed_real_LH         =0;                
    

  23. }
回复 赏

举报

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

本版积分规则

zoomone

3

主题

370

帖子

2

粉丝
快速回复 在线客服 返回列表 返回顶部