標(biāo)題: 龍邱自平衡車清華方案自由滑行版(直立行走)源碼 [打印本頁(yè)]
作者: 如同1219    時(shí)間: 2018-4-21 18:11
標(biāo)題: 龍邱自平衡車清華方案自由滑行版(直立行走)源碼
實(shí)物圖：

平衡車單片機(jī)源程序如下:
  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. 程序使用說(shuō)明：  
  16. 該工程里寫出電磁組直立行走的參考方案，通過(guò)程序可以實(shí)現(xiàn)小車自由滑行。
  17. ==========================================================   
  18. 陀螺儀模塊電路說(shuō)明：   
  19. 角度計(jì)算采用清華硬件電路，詳見40.pdf(電磁組直立行車參考設(shè)計(jì)方案20.pdf)
  20. 第54頁(yè)，圖3-23.一個(gè)運(yùn)算放大器實(shí)現(xiàn)角度計(jì)算   
  21. ==========================================================
  22. 接線說(shuō)明
  23. 清華版陀螺儀放置的時(shí)候，記得ENC-03在下，加速度在上
  24. 放置方向跟電機(jī)的接線也有關(guān)系的。  
  25. -----------------------------------------
  26. 單片機(jī)  | 龍邱陀螺儀模塊 |
  27. --------+----------------+---------------
  28. AD0     |  gyro角速度    |
  29. AD1     |  ang角度       |
  30. =========================================
  31. 單片機(jī)  |  電機(jī)驅(qū)動(dòng)模塊  |  車模電機(jī)
  32. --------+----------------+---------------
  33. PP7     |  IN4     OUT1  |  右邊電機(jī)正
  34. PB1     |  IN3     OUT2  |  右邊電機(jī)負(fù)
  35. PB0     |  IN2     OUT3  |  左邊電機(jī)正
  36. PP3     |  IN1     OUT4  |  左邊電機(jī)負(fù)   
  37. 單片機(jī)5V|  Vmcu          |
  38. ===========================================================

  40. 調(diào)整步驟：
  41. 1.下載程序到單片機(jī)，
  42. 2.進(jìn)入在線調(diào)試狀態(tài)，
  43. 3.把車豎直立起來(lái)達(dá)到平衡狀態(tài)，在線查看寄存器數(shù)值
  44.    根據(jù)平衡狀態(tài)下的ANG，確定ANG_center數(shù)值
  45.    根據(jù)平衡狀態(tài)下的GYRO，確定GYRO_center數(shù)值
  46. 4.中心數(shù)值確定后，再調(diào)整PD參數(shù)
  47.     P系數(shù)相當(dāng)于倒立擺的回復(fù)力。這個(gè)參數(shù)應(yīng)該大于重力加速度的等效數(shù)值車模才能夠保持直立。
  48.     隨著該參數(shù)逐步增大，車模開始能夠保持直立。進(jìn)一步增大該參數(shù)會(huì)引起車模震蕩。
  49.     D系數(shù)相當(dāng)于倒立擺的阻尼力。該參數(shù)可以有效抑制車模的的震蕩。當(dāng)該參數(shù)過(guò)大的時(shí)候，車模開始抖動(dòng)。

  51.     D系數(shù),先令D=0，然后逐步增加P，直到車模發(fā)生震蕩，再增加D消除震蕩，直到車模開始抖動(dòng)；
  52.     然后增加P，消除抖動(dòng)；
  53. 5.反復(fù)幾次后就可以找到適當(dāng)?shù)腜,D。
  54. 6.小車基本平衡后用戶可以根據(jù)需要添加速度控制和尋線算法，比賽的重點(diǎn)就在這里，各盡其能！

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

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

  63. #define PWM_DIR0  PORTB_PB0   //電機(jī)方向控制
  64. #define PWM_DIR1  PORTB_PB1   //電機(jī)方向控制

  66. word AD_CAIJI[2]={0,0};         //采集的AD數(shù)值臨時(shí)存放數(shù)組
  67. word AD_QIUHE[2]={0,0};         //求和臨時(shí)存放數(shù)組
  68. word AD_JUNZHI[2]={0,0};        //平均值
  69. volatile byte PIT_CNT=0;        //中斷計(jì)數(shù)
  70. volatile int  ANG=0;            //角度，前傾變大，后傾變小，平衡時(shí)候ANG_center =
  71. volatile int  GYRO=0;           //角速度，前傾變小，后傾變大，靜止時(shí)候?yàn)镚YRO_center=？

  73. volatile int  PID1_P=15;         //P系數(shù),pwm3   
  74. volatile int  PID1_D=6;          //D系數(shù),pwm3
  75. volatile int  PID2_P=15;         //P系數(shù),pwm7
  76. volatile int  PID2_D=6;          //D系數(shù),pwm7
  77. volatile int  ANG_center =319;  //AD的初始值，每個(gè)模塊數(shù)值都不一樣，跟換模塊需要重新確定該值
  78. volatile int  GYRO_center=367;  //角速度初始值，電機(jī)不動(dòng)時(shí)候靜止?fàn)顟B(tài)下采集的數(shù)值
  79. volatile int  AD_number=20;     //求和數(shù)量
  80. volatile word DTY1=0;            //臨時(shí)變量
  81. volatile word DTY2=0;            //臨時(shí)變量
  82. volatile word DTYMAX=245;       //PWM占空比最大值

  84. #define FDOWN 253               //前向倒下時(shí)的角度數(shù)值
  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. /****************************初始化設(shè)備**************************************/
  109. void fAD_Init(void)
  110. {
  111.     ATD0CTL1=0b00100000;//   10 位精度
  112.     ATD0CTL2=0b01000000;//   禁止外部觸發(fā),標(biāo)志位快速清零,中斷禁止
  113.     ATD0CTL3=0b10100000;/*   7;右對(duì)齊無(wú)符號(hào);
  114.                              6~3:轉(zhuǎn)換序列長(zhǎng)度為4;
  115.                              No FIFO模式,Freeze模式下繼續(xù)轉(zhuǎn)換?  */
  116.     ATD0CTL4=0b00000111;//   4AD采樣周期,ATDClock=[BusClock*0.5]/[PRS+1]  ; PRS=15, divider=32 ?
  117.     ATD0CTL5=0b00110000;//   特殊通道禁止,多通道采樣,掃描模式連續(xù)采樣,開始為 AN0
  118.     ATD0DIEN=0b00000000;//   禁止數(shù)字輸入   
  119.     return;
  120. }
  121. //-----PWM初始化程序------//
  122. void fPWM_Init(void)
  123. {
  124.     PWME=0x00;        /* 禁止PWM輸出*/
  125.     PWMCTL=0x00;      //通道不級(jí)聯(lián)
  126.     PWMCAE=0X00;      //0左對(duì)齊 1中心對(duì)齊
  127.     PWMPRCLK=0X11;    //2分頻
  128.     PWMCLK=0XFF;      /*時(shí)鐘選擇寄存器,  0選擇ClockX 1選擇ClockSX.
  129.                       通道0,1,4,5用ClockA或ClockSA,通道2,3,6,7用ClockB或ClockSB   
  130.                       設(shè)置后，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;      /*左對(duì)齊，起始輸出為高電平時(shí)，占空比=(PWMDTY3+1)/(PWMPER0+1) */  
  137.   
  138.     PWMPER3=250;      //2M/250=8KHZ                             
  139.     PWMDTY3=0;        /*左對(duì)齊，起始輸出為高電平時(shí)，占空比=(PWMDTY3+1)/(PWMPER0+1) */   
  140.     PWMPER7=250;                                   
  141.     PWMDTY7=0;      

  143.     PWME_PWME1=0;     //啟動(dòng)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. /***************     定時(shí)器函數(shù)1ms     *****/   
  170. void fPIT_Init(void)   //定時(shí)中斷初始化函數(shù) 1MS定時(shí)中斷設(shè)置
  171. {
  172.     PITCFLMT_PITE=0; //定時(shí)中斷通道關(guān)     
  173.     PITCE_PCE0=1;    //定時(shí)器通道 0使能  定時(shí)1ms用
  174.     PITMUX_PMUX0=1;  //定時(shí)器通道 0選擇8位計(jì)數(shù)器1
  175.     PITMTLD1=40-1;   //8位計(jì)數(shù)器0初值設(shè)定。40分頻，在 40MHzBusClock下，為 1MHz。即 1us.
  176.     PITLD0=1000-1;   //16位計(jì)數(shù)器初值設(shè)定。1000*1us=1ms
  177.     PITINTE_PINTE0=1;//開中斷，定時(shí)器中斷通道 0中斷使能   
  178.     PITCFLMT_PITE=1; //定時(shí)器通道使能
  179.     return;
  180. }

  182. void fAD_CAIJI(unsigned int *AD_val)    //AD采集兩個(gè)通道
  183. {
  184.           while(!ATD0STAT2_CCF0);//0通道轉(zhuǎn)換完成前等待，采集角速度
  185.           *AD_val=ATD0DR0;
  186.           AD_val++;
  187.           while(!ATD0STAT2_CCF1);//1通道轉(zhuǎn)換完成前等待，采集角度
  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)               //電機(jī)控制
  216. {   
  217.     GYRO=AD_JUNZHI[0];                //求得角速度                                       
  218.     ANG =AD_JUNZHI[1];                //求得角度
  219.     /***********************正轉(zhuǎn)****************/
  220.     //角度，前傾變大，后傾變小，平衡時(shí)候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.     /**********************反轉(zhuǎn)***************/
  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;     //啟動(dòng)PWM輸出
  273.     //while(key==PORTE){//按下E口任意按鍵，退出自標(biāo)定角度的初始值，蜂鳴器停止響
  274.         //fAD_QIUJUNZHI();      
  275.         //ANG_center=AD_JUNZHI[1]; //求得角度初始值
  276.         Delay_ms(5);
  277.     //}
  278.     PWME_PWME1=0;     //關(guān)閉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;           //清中斷標(biāo)志位
  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調(diào)節(jié)
  303.        fPWM_KONGZHI();      //電機(jī)pwm輸出
  304.     //else if(PIT_CNT==4)
  305.       // ANGControl();      //直立控制
  306. ……………………

  308. …………限于本文篇幅 余下代碼請(qǐng)從51黑下載附件…………
復(fù)制代碼

所有資料51hei提供下載:
龍邱自平衡車清華方案自由滑行版.rar (215.32 KB, 下載次數(shù): 54)


作者: sadbkj    時(shí)間: 2019-4-30 00:13
大佬，這是用的啥軟件啊
作者: 沒有用戶名字    時(shí)間: 2020-4-27 09:29
是龍邱k60的嗎
作者: 玄米    時(shí)間: 2020-5-4 15:15
sadbkj 發(fā)表于 2019-4-30 00:13
大佬，這是用的啥軟件啊

這是Codewarrior 5.1，應(yīng)該是基于XS128單片機(jī)的



歡迎光臨 (http://www.torrancerestoration.com/bbs/) Powered by Discuz! X3.1