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

 找回密码
 立即注册

QQ登录

只需一步，快速开始

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

搜索
»论坛 › 电子技术分类讨论区 › 智能小车/机器人 › 龙邱自平衡车清华方案自由滑行版(直立行走)源码
返回列表 发新帖
查看: 7380|回复: 3
打印 上一主题 下一主题
收起左侧

龙邱自平衡车清华方案自由滑行版(直立行走)源码

[复制链接]
跳转到指定楼层
楼主
ID:287806 发表于 2018-4-21 18:11 | 只看该作者 |只看大图 回帖奖励 |倒序浏览 |阅读模式
实物图：

平衡车单片机源程序如下:
  1. /*
  2. **********************************************************
  3. 飞思卡尔智能车程序  直立行走例程
  4. 北京龙丘智能科技      
  5. Designed by longqiu
  6. E-mail:chiusir@yahoo.cn
  7. 软件版本:V1.2
  8. ------------------------------------
  9. Code Warrior 5.1
  10. Target : MC9S12XS128
  11. Crystal: 16.000Mhz
  12. busclock:40.000MHz
  13. pllclock:80.000MHz
  14. ------------------------------------  
  15. 程序使用说明：  
  16. 该工程里写出电磁组直立行走的参考方案，通过程序可以实现小车自由滑行。
  17. ==========================================================   
  18. 陀螺仪模块电路说明：   
  19. 角度计算采用清华硬件电路，详见40.pdf(电磁组直立行车参考设计方案20.pdf)
  20. 第54页，图3-23.一个运算放大器实现角度计算   
  21. ==========================================================
  22. 接线说明
  23. 清华版陀螺仪放置的时候，记得ENC-03在下，加速度在上
  24. 放置方向跟电机的接线也有关系的。  
  25. -----------------------------------------
  26. 单片机  | 龙邱陀螺仪模块 |
  27. --------+----------------+---------------
  28. AD0     |  gyro角速度    |
  29. AD1     |  ang角度       |
  30. =========================================
  31. 单片机  |  电机驱动模块  |  车模电机
  32. --------+----------------+---------------
  33. PP7     |  IN4     OUT1  |  右边电机正
  34. PB1     |  IN3     OUT2  |  右边电机负
  35. PB0     |  IN2     OUT3  |  左边电机正
  36. PP3     |  IN1     OUT4  |  左边电机负   
  37. 单片机5V|  Vmcu          |
  38. ===========================================================

  40. 调整步骤：
  41. 1.下载程序到单片机，
  42. 2.进入在线调试状态，
  43. 3.把车竖直立起来达到平衡状态，在线查看寄存器数值
  44.    根据平衡状态下的ANG，确定ANG_center数值
  45.    根据平衡状态下的GYRO，确定GYRO_center数值
  46. 4.中心数值确定后，再调整PD参数
  47.     P系数相当于倒立摆的回复力。这个参数应该大于重力加速度的等效数值车模才能够保持直立。
  48.     随着该参数逐步增大，车模开始能够保持直立。进一步增大该参数会引起车模震荡。
  49.     D系数相当于倒立摆的阻尼力。该参数可以有效抑制车模的的震荡。当该参数过大的时候，车模开始抖动。

  51.     D系数,先令D=0，然后逐步增加P，直到车模发生震荡，再增加D消除震荡，直到车模开始抖动；
  52.     然后增加P，消除抖动；
  53. 5.反复几次后就可以找到适当的P,D。
  54. 6.小车基本平衡后用户可以根据需要添加速度控制和寻线算法，比赛的重点就在这里，各尽其能！

  56. ==========================================================                                                                        

  58. ***********************************************************
  59. */
  60. #include <hidef.h>         
  61. #include "derivative.h"   

  63. #define PWM_DIR0  PORTB_PB0   //电机方向控制
  64. #define PWM_DIR1  PORTB_PB1   //电机方向控制

  66. word AD_CAIJI[2]={0,0};         //采集的AD数值临时存放数组
  67. word AD_QIUHE[2]={0,0};         //求和临时存放数组
  68. word AD_JUNZHI[2]={0,0};        //平均值
  69. volatile byte PIT_CNT=0;        //中断计数
  70. volatile int  ANG=0;            //角度，前倾变大，后倾变小，平衡时候ANG_center =
  71. volatile int  GYRO=0;           //角速度，前倾变小，后倾变大，静止时候为GYRO_center=？

  73. volatile int  PID1_P=15;         //P系数,pwm3   
  74. volatile int  PID1_D=6;          //D系数,pwm3
  75. volatile int  PID2_P=15;         //P系数,pwm7
  76. volatile int  PID2_D=6;          //D系数,pwm7
  77. volatile int  ANG_center =319;  //AD的初始值，每个模块数值都不一样，跟换模块需要重新确定该值
  78. volatile int  GYRO_center=367;  //角速度初始值，电机不动时候静止状态下采集的数值
  79. volatile int  AD_number=20;     //求和数量
  80. volatile word DTY1=0;            //临时变量
  81. volatile word DTY2=0;            //临时变量
  82. volatile word DTYMAX=245;       //PWM占空比最大值

  84. #define FDOWN 253               //前向倒下时的角度数值
  85. #define BDOWN 190               //后向倒下

  87. void Delay_us(word x)
  88. {
  89.   byte i = 0,j = 0;
  90.         while(x--)
  91.         {
  92.           for(i=0;i<20;i++)
  93.             for(j=0;j<2;j++);         
  94.         }
  95.         return;
  96. }
  97. void Delay_ms(word x)
  98. {
  99.   byte i = 0,j = 0;
  100.         while(x--)
  101.         {
  102.           for(i=0;i<200;i++)
  103.             for(j=0;j<200;j++);         
  104.         }
  105.         return;           
  106. }

  108. /****************************初始化设备**************************************/
  109. void fAD_Init(void)
  110. {
  111.     ATD0CTL1=0b00100000;//   10 位精度
  112.     ATD0CTL2=0b01000000;//   禁止外部触发,标志位快速清零,中断禁止
  113.     ATD0CTL3=0b10100000;/*   7;右对齐无符号;
  114.                              6~3:转换序列长度为4;
  115.                              No FIFO模式,Freeze模式下继续转换?  */
  116.     ATD0CTL4=0b00000111;//   4AD采样周期,ATDClock=[BusClock*0.5]/[PRS+1]  ; PRS=15, divider=32 ?
  117.     ATD0CTL5=0b00110000;//   特殊通道禁止,多通道采样,扫描模式连续采样,开始为 AN0
  118.     ATD0DIEN=0b00000000;//   禁止数字输入   
  119.     return;
  120. }
  121. //-----PWM初始化程序------//
  122. void fPWM_Init(void)
  123. {
  124.     PWME=0x00;        /* 禁止PWM输出*/
  125.     PWMCTL=0x00;      //通道不级联
  126.     PWMCAE=0X00;      //0左对齐 1中心对齐
  127.     PWMPRCLK=0X11;    //2分频
  128.     PWMCLK=0XFF;      /*时钟选择寄存器,  0选择ClockX 1选择ClockSX.
  129.                       通道0,1,4,5用ClockA或ClockSA,通道2,3,6,7用ClockB或ClockSB   
  130.                       设置后，0、1用ClockA，4、5用ClockSA，2、3用ClockB，6、7用ClockSB   */
  131.     PWMSCLA=50;       //ClockSA=ClockA/(2*PWMSCLA)=20000k/(2*50)=200KHZ
  132.     PWMSCLB=5;        //ClockSB=ClockA/(2*PWMSCLB)=20000k/(2*5)=2MHZ
  133.     PWMPOL=0X00;      //输出极性选择 0起始为低电平 1起始为高电平
  134.       
  135.     PWMPER1=250;      //200KHZ/250=800HZ  
  136.     PWMDTY1=100;      /*左对齐，起始输出为高电平时，占空比=(PWMDTY3+1)/(PWMPER0+1) */  
  137.   
  138.     PWMPER3=250;      //2M/250=8KHZ                             
  139.     PWMDTY3=0;        /*左对齐，起始输出为高电平时，占空比=(PWMDTY3+1)/(PWMPER0+1) */   
  140.     PWMPER7=250;                                   
  141.     PWMDTY7=0;      

  143.     PWME_PWME1=0;     //启动PWM输出
  144.     PWME_PWME3=1;
  145.     PWME_PWME7=1;  
  146.     return;
  147. }

  149. void SetBusCLK_40M(void)
  150. {   
  151.     CLKSEL=0X00;                                //disengage PLL to system
  152.     PLLCTL_PLLON=1;                        //turn on PLL
  153.     SYNR =0xc0 | 0x04;                        
  154.     REFDV=0x80 | 0x01;
  155.     POSTDIV=0x00;       //pllclock=2*osc*(1+SYNR)/(1+REFDV)=80MHz;
  156.     _asm(nop);          //BUS CLOCK=40M
  157.     _asm(nop);
  158.     while(!(CRGFLG_LOCK==1));          //when pll is steady ,then use it;
  159.     CLKSEL_PLLSEL =1;                        //engage PLL to system;
  160. }
  161. void fDEV_Init()
  162. {        
  163.           SetBusCLK_40M();
  164.           fAD_Init();
  165.     fPWM_Init();  
  166.     return;
  167. }  

  169. /***************     定时器函数1ms     *****/   
  170. void fPIT_Init(void)   //定时中断初始化函数 1MS定时中断设置
  171. {
  172.     PITCFLMT_PITE=0; //定时中断通道关     
  173.     PITCE_PCE0=1;    //定时器通道 0使能  定时1ms用
  174.     PITMUX_PMUX0=1;  //定时器通道 0选择8位计数器1
  175.     PITMTLD1=40-1;   //8位计数器0初值设定。40分频，在 40MHzBusClock下，为 1MHz。即 1us.
  176.     PITLD0=1000-1;   //16位计数器初值设定。1000*1us=1ms
  177.     PITINTE_PINTE0=1;//开中断，定时器中断通道 0中断使能   
  178.     PITCFLMT_PITE=1; //定时器通道使能
  179.     return;
  180. }

  182. void fAD_CAIJI(unsigned int *AD_val)    //AD采集两个通道
  183. {
  184.           while(!ATD0STAT2_CCF0);//0通道转换完成前等待，采集角速度
  185.           *AD_val=ATD0DR0;
  186.           AD_val++;
  187.           while(!ATD0STAT2_CCF1);//1通道转换完成前等待，采集角度
  188.           *AD_val=ATD0DR1;           
  189.           return;
  190. }

  192. void fAD_QIUHE(void)
  193. {
  194.     word i;
  195.    
  196.    
  197.     for(i=0;i<AD_number;i++)
  198.       {
  199.         fAD_CAIJI(AD_CAIJI);                                            
  200.         AD_QIUHE[0]+=AD_CAIJI[0];     //获取AD0通道值的20次和
  201.         AD_QIUHE[1]+=AD_CAIJI[1];     //获取AD1通道值的20次和
  202.       }
  203.     return;   
  204. }   

  206. void fAD_QIUJUNZHI()                  //求平均值
  207. {
  208.     AD_JUNZHI[0]=(word)AD_QIUHE[0]/AD_number;
  209.     AD_JUNZHI[1]=(word)AD_QIUHE[1]/AD_number;
  210.     AD_QIUHE[0]=0;                     
  211.     AD_QIUHE[1]=0;
  212.     return;   
  213. }
  214.                                                      
  215. void fPWM_KONGZHI(void)               //电机控制
  216. {   
  217.     GYRO=AD_JUNZHI[0];                //求得角速度                                       
  218.     ANG =AD_JUNZHI[1];                //求得角度
  219.     /***********************正转****************/
  220.     //角度，前倾变大，后倾变小，平衡时候ANG_center
  221.     /*
  222.     //摔倒处理
  223.     if ((ANG > FDOWN)|| (ANG < BDOWN) )
  224.     {
  225.         PWM_DIR0=1;            
  226.         PWM_DIR1=1;
  227.         PWMDTY7=PWMDTY3=0;
  228.         PWMPOL=0X00;
  229.         return;
  230.     } */
  231.     if (ANG > ANG_center)            
  232.     {   //   fAD_QIUJUNZHI();            
  233.         DTY1=((ANG - ANG_center ) * PID1_P + (GYRO_center-GYRO) / PID1_D);
  234.         DTY2=((ANG - ANG_center ) * PID2_P + (GYRO_center-GYRO) / PID2_D);     
  235.         if(DTY1>=DTYMAX) DTY1=DTYMAX;
  236.         if(DTY2>=DTYMAX) DTY2=DTYMAX;  
  237.         PWM_DIR0=1;            
  238.         PWM_DIR1=1;
  239.         PWMDTY3=(byte)DTY1;
  240.         PWMDTY7=(byte)DTY2;
  241.         
  242.         PWMPOL=0X00;      //输出极性选择 0起始为低电平 1起始为高电平        
  243.     }   
  244.     //Delay_us(5);        
  245.     /**********************反转***************/
  246.     else if (ANG<ANG_center)              
  247.     {
  248.         DTY1=(( ANG_center - ANG ) * PID1_P + (GYRO - GYRO_center) / PID1_D );
  249.         DTY2=(( ANG_center - ANG ) * PID2_P + (GYRO - GYRO_center) / PID2_D );                        
  250.         if(DTY1>=DTYMAX) DTY1=DTYMAX;   
  251.         if(DTY2>=DTYMAX) DTY2=DTYMAX;         
  252.         PWM_DIR0=0;
  253.         PWM_DIR1=0;
  254.         PWMDTY3=(byte)DTY1;
  255.         PWMDTY7=(byte)DTY2;
  256.                PWMPOL=0Xff;               
  257.     }
  258.     return;   
  259. }

  261. void main(void)
  262. {
  263.     byte key=0;
  264.    
  265.     DDRA=0xFF;
  266.     PORTA=0x00;
  267.     DDRB=0XFF;
  268.     PORTB=0X00;  
  269.     DDRE=0;
  270.     fDEV_Init();
  271.     key=PORTE;
  272.     PWME_PWME1=1;     //启动PWM输出
  273.     //while(key==PORTE){//按下E口任意按键，退出自标定角度的初始值，蜂鸣器停止响
  274.         //fAD_QIUJUNZHI();      
  275.         //ANG_center=AD_JUNZHI[1]; //求得角度初始值
  276.         Delay_ms(5);
  277.     //}
  278.     PWME_PWME1=0;     //关闭PWM输出
  279.     fPIT_Init();
  280.     EnableInterrupts;                     
  281.     for(;;)
  282.     {
  283.    
  284.     }
  285. }   

  287. #pragma CODE_SEG __NEAR_SEG NON_BANKED
  288. //1ms中断
  289. void interrupt 66 PIT0_ISR(void)
  290. {
  291.     PITTF_PTF0=1;           //清中断标志位
  292.     PIT_CNT++;
  293.    
  294.     if(PIT_CNT==1)
  295.     {      
  296.        fAD_QIUHE() ;        //取20次AD输入的和        
  297.     }
  298.     else if(PIT_CNT==2)
  299.        fAD_QIUJUNZHI();     //20次AD输入的平均值
  300.    
  301.     else if(PIT_CNT==3)
  302.       //ANGCulate();        //PD调节
  303.        fPWM_KONGZHI();      //电机pwm输出
  304.     //else if(PIT_CNT==4)
  305.       // ANGControl();      //直立控制
  306. ……………………

  308. …………限于本文篇幅 余下代码请从51黑下载附件…………
复制代码

所有资料51hei提供下载:
龙邱自平衡车清华方案自由滑行版.rar (215.32 KB, 下载次数: 54)


分享到:  QQ好友和群QQ好友和群 QQ空间QQ空间 腾讯微博腾讯微博 腾讯朋友腾讯朋友
收藏收藏3 分享淘帖 顶顶1 踩踩
回复

使用道具 举报

沙发
ID:417080 发表于 2019-4-30 00:13 | 只看该作者
大佬，这是用的啥软件啊
回复

使用道具 举报

板凳
ID:737781 发表于 2020-4-27 09:29 | 只看该作者
是龙邱k60的吗
回复

使用道具 举报

地板
ID:712298 发表于 2020-5-4 15:15 | 只看该作者
sadbkj 发表于 2019-4-30 00:13
大佬，这是用的啥软件啊

这是Codewarrior 5.1，应该是基于XS128单片机的
回复

使用道具 举报

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

本版积分规则

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

Powered by 单片机教程网

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