龍邱自平衡車清華方案自由滑行版(直立行走)源碼

ID:287806 · 發(fā)表于 2018-4-21 18:11

實物圖：

平衡車單片機源程序如下:

/*
**********************************************************
飛思卡爾智能車程序直立行走例程
北京龍丘智能科技
Designed by longqiu
E-mail:chiusir@yahoo.cn
軟件版本:V1.2
------------------------------------
Code Warrior 5.1
Target : MC9S12XS128
Crystal: 16.000Mhz
busclock:40.000MHz
pllclock:80.000MHz
------------------------------------
程序使用說明：
該工程里寫出電磁組直立行走的參考方案，通過程序可以實現(xiàn)小車自由滑行。
==========================================================
陀螺儀模塊電路說明：
角度計算采用清華硬件電路，詳見40.pdf(電磁組直立行車參考設計方案20.pdf)
第54頁，圖3-23.一個運算放大器實現(xiàn)角度計算
==========================================================
接線說明
清華版陀螺儀放置的時候，記得ENC-03在下，加速度在上
放置方向跟電機的接線也有關系的。
-----------------------------------------
單片機 | 龍邱陀螺儀模塊 |
--------+----------------+---------------
AD0 | gyro角速度 |
AD1 | ang角度 |
=========================================
單片機 | 電機驅(qū)動模塊 | 車模電機
--------+----------------+---------------
PP7 | IN4 OUT1 | 右邊電機正
PB1 | IN3 OUT2 | 右邊電機負
PB0 | IN2 OUT3 | 左邊電機正
PP3 | IN1 OUT4 | 左邊電機負
單片機5V| Vmcu |
===========================================================
調(diào)整步驟：
1.下載程序到單片機，
2.進入在線調(diào)試狀態(tài)，
3.把車豎直立起來達到平衡狀態(tài)，在線查看寄存器數(shù)值
根據(jù)平衡狀態(tài)下的ANG，確定ANG_center數(shù)值
根據(jù)平衡狀態(tài)下的GYRO，確定GYRO_center數(shù)值
4.中心數(shù)值確定后，再調(diào)整PD參數(shù)
P系數(shù)相當于倒立擺的回復力。這個參數(shù)應該大于重力加速度的等效數(shù)值車模才能夠保持直立。
隨著該參數(shù)逐步增大，車模開始能夠保持直立。進一步增大該參數(shù)會引起車模震蕩。
D系數(shù)相當于倒立擺的阻尼力。該參數(shù)可以有效抑制車模的的震蕩。當該參數(shù)過大的時候，車模開始抖動。
D系數(shù),先令D=0，然后逐步增加P，直到車模發(fā)生震蕩，再增加D消除震蕩，直到車模開始抖動；
然后增加P，消除抖動；
5.反復幾次后就可以找到適當?shù)腜,D。
6.小車基本平衡后用戶可以根據(jù)需要添加速度控制和尋線算法，比賽的重點就在這里，各盡其能！
==========================================================
***********************************************************
*/
#include <hidef.h>
#include "derivative.h"
#define PWM_DIR0 PORTB_PB0 //電機方向控制
#define PWM_DIR1 PORTB_PB1 //電機方向控制
word AD_CAIJI[2]={0,0}; //采集的AD數(shù)值臨時存放數(shù)組
word AD_QIUHE[2]={0,0}; //求和臨時存放數(shù)組
word AD_JUNZHI[2]={0,0}; //平均值
volatile byte PIT_CNT=0; //中斷計數(shù)
volatile int ANG=0; //角度，前傾變大，后傾變小，平衡時候ANG_center =
volatile int GYRO=0; //角速度，前傾變小，后傾變大，靜止時候為GYRO_center=？
volatile int PID1_P=15; //P系數(shù),pwm3
volatile int PID1_D=6; //D系數(shù),pwm3
volatile int PID2_P=15; //P系數(shù),pwm7
volatile int PID2_D=6; //D系數(shù),pwm7
volatile int ANG_center =319; //AD的初始值，每個模塊數(shù)值都不一樣，跟換模塊需要重新確定該值
volatile int GYRO_center=367; //角速度初始值，電機不動時候靜止狀態(tài)下采集的數(shù)值
volatile int AD_number=20; //求和數(shù)量
volatile word DTY1=0; //臨時變量
volatile word DTY2=0; //臨時變量
volatile word DTYMAX=245; //PWM占空比最大值
#define FDOWN 253 //前向倒下時的角度數(shù)值
#define BDOWN 190 //后向倒下
void Delay_us(word x)
{
byte i = 0,j = 0;
while(x--)
{
for(i=0;i<20;i++)
for(j=0;j<2;j++);
}
return;
}
void Delay_ms(word x)
{
byte i = 0,j = 0;
while(x--)
{
for(i=0;i<200;i++)
for(j=0;j<200;j++);
}
return;
}
/****************************初始化設備**************************************/
void fAD_Init(void)
{
ATD0CTL1=0b00100000;// 10 位精度
ATD0CTL2=0b01000000;// 禁止外部觸發(fā),標志位快速清零,中斷禁止
ATD0CTL3=0b10100000;/* 7;右對齊無符號;
6~3:轉換序列長度為4;
No FIFO模式,Freeze模式下繼續(xù)轉換? */
ATD0CTL4=0b00000111;// 4AD采樣周期,ATDClock=[BusClock*0.5]/[PRS+1] ; PRS=15, divider=32 ?
ATD0CTL5=0b00110000;// 特殊通道禁止,多通道采樣,掃描模式連續(xù)采樣,開始為 AN0
ATD0DIEN=0b00000000;// 禁止數(shù)字輸入
return;
}
//-----PWM初始化程序------//
void fPWM_Init(void)
{
PWME=0x00; /* 禁止PWM輸出*/
PWMCTL=0x00; //通道不級聯(lián)
PWMCAE=0X00; //0左對齊 1中心對齊
PWMPRCLK=0X11; //2分頻
PWMCLK=0XFF; /*時鐘選擇寄存器, 0選擇ClockX 1選擇ClockSX.
通道0,1,4,5用ClockA或ClockSA,通道2,3,6,7用ClockB或ClockSB
設置后，0、1用ClockA，4、5用ClockSA，2、3用ClockB，6、7用ClockSB */
PWMSCLA=50; //ClockSA=ClockA/(2*PWMSCLA)=20000k/(2*50)=200KHZ
PWMSCLB=5; //ClockSB=ClockA/(2*PWMSCLB)=20000k/(2*5)=2MHZ
PWMPOL=0X00; //輸出極性選擇 0起始為低電平 1起始為高電平
PWMPER1=250; //200KHZ/250=800HZ
PWMDTY1=100; /*左對齊，起始輸出為高電平時，占空比=(PWMDTY3+1)/(PWMPER0+1) */
PWMPER3=250; //2M/250=8KHZ
PWMDTY3=0; /*左對齊，起始輸出為高電平時，占空比=(PWMDTY3+1)/(PWMPER0+1) */
PWMPER7=250;
PWMDTY7=0;
PWME_PWME1=0; //啟動PWM輸出
PWME_PWME3=1;
PWME_PWME7=1;
return;
}
void SetBusCLK_40M(void)
{
CLKSEL=0X00; //disengage PLL to system
PLLCTL_PLLON=1; //turn on PLL
SYNR =0xc0 | 0x04;
REFDV=0x80 | 0x01;
POSTDIV=0x00; //pllclock=2*osc*(1+SYNR)/(1+REFDV)=80MHz;
_asm(nop); //BUS CLOCK=40M
_asm(nop);
while(!(CRGFLG_LOCK==1)); //when pll is steady ,then use it;
CLKSEL_PLLSEL =1; //engage PLL to system;
}
void fDEV_Init()
{
SetBusCLK_40M();
fAD_Init();
fPWM_Init();
return;
}
/*************** 定時器函數(shù)1ms *****/
void fPIT_Init(void) //定時中斷初始化函數(shù) 1MS定時中斷設置
{
PITCFLMT_PITE=0; //定時中斷通道關
PITCE_PCE0=1; //定時器通道 0使能定時1ms用
PITMUX_PMUX0=1; //定時器通道 0選擇8位計數(shù)器1
PITMTLD1=40-1; //8位計數(shù)器0初值設定。40分頻，在 40MHzBusClock下，為 1MHz。即 1us.
PITLD0=1000-1; //16位計數(shù)器初值設定。1000*1us=1ms
PITINTE_PINTE0=1;//開中斷，定時器中斷通道 0中斷使能
PITCFLMT_PITE=1; //定時器通道使能
return;
}
void fAD_CAIJI(unsigned int *AD_val) //AD采集兩個通道
{
while(!ATD0STAT2_CCF0);//0通道轉換完成前等待，采集角速度
*AD_val=ATD0DR0;
AD_val++;
while(!ATD0STAT2_CCF1);//1通道轉換完成前等待，采集角度
*AD_val=ATD0DR1;
return;
}
void fAD_QIUHE(void)
{
word i;
for(i=0;i<AD_number;i++)
{
fAD_CAIJI(AD_CAIJI);
AD_QIUHE[0]+=AD_CAIJI[0]; //獲取AD0通道值的20次和
AD_QIUHE[1]+=AD_CAIJI[1]; //獲取AD1通道值的20次和
}
return;
}
void fAD_QIUJUNZHI() //求平均值
{
AD_JUNZHI[0]=(word)AD_QIUHE[0]/AD_number;
AD_JUNZHI[1]=(word)AD_QIUHE[1]/AD_number;
AD_QIUHE[0]=0;
AD_QIUHE[1]=0;
return;
}
void fPWM_KONGZHI(void) //電機控制
{
GYRO=AD_JUNZHI[0]; //求得角速度
ANG =AD_JUNZHI[1]; //求得角度
/***********************正轉****************/
//角度，前傾變大，后傾變小，平衡時候ANG_center
/*
//摔倒處理
if ((ANG > FDOWN)|| (ANG < BDOWN) )
{
PWM_DIR0=1;
PWM_DIR1=1;
PWMDTY7=PWMDTY3=0;
PWMPOL=0X00;
return;
} */
if (ANG > ANG_center)
{ // fAD_QIUJUNZHI();
DTY1=((ANG - ANG_center ) * PID1_P + (GYRO_center-GYRO) / PID1_D);
DTY2=((ANG - ANG_center ) * PID2_P + (GYRO_center-GYRO) / PID2_D);
if(DTY1>=DTYMAX) DTY1=DTYMAX;
if(DTY2>=DTYMAX) DTY2=DTYMAX;
PWM_DIR0=1;
PWM_DIR1=1;
PWMDTY3=(byte)DTY1;
PWMDTY7=(byte)DTY2;
PWMPOL=0X00; //輸出極性選擇 0起始為低電平 1起始為高電平
}
//Delay_us(5);
/**********************反轉***************/
else if (ANG<ANG_center)
{
DTY1=(( ANG_center - ANG ) * PID1_P + (GYRO - GYRO_center) / PID1_D );
DTY2=(( ANG_center - ANG ) * PID2_P + (GYRO - GYRO_center) / PID2_D );
if(DTY1>=DTYMAX) DTY1=DTYMAX;
if(DTY2>=DTYMAX) DTY2=DTYMAX;
PWM_DIR0=0;
PWM_DIR1=0;
PWMDTY3=(byte)DTY1;
PWMDTY7=(byte)DTY2;
PWMPOL=0Xff;
}
return;
}
void main(void)
{
byte key=0;
DDRA=0xFF;
PORTA=0x00;
DDRB=0XFF;
PORTB=0X00;
DDRE=0;
fDEV_Init();
key=PORTE;
PWME_PWME1=1; //啟動PWM輸出
//while(key==PORTE){//按下E口任意按鍵，退出自標定角度的初始值，蜂鳴器停止響
//fAD_QIUJUNZHI();
//ANG_center=AD_JUNZHI[1]; //求得角度初始值
Delay_ms(5);
//}
PWME_PWME1=0; //關閉PWM輸出
fPIT_Init();
EnableInterrupts;
for(;;)
{
}
}
#pragma CODE_SEG __NEAR_SEG NON_BANKED
//1ms中斷
void interrupt 66 PIT0_ISR(void)
{
PITTF_PTF0=1; //清中斷標志位
PIT_CNT++;
if(PIT_CNT==1)
{
fAD_QIUHE() ; //取20次AD輸入的和
}
else if(PIT_CNT==2)
fAD_QIUJUNZHI(); //20次AD輸入的平均值
else if(PIT_CNT==3)
//ANGCulate(); //PD調(diào)節(jié)
fPWM_KONGZHI(); //電機pwm輸出
//else if(PIT_CNT==4)
// ANGControl(); //直立控制
……………………
…………限于本文篇幅余下代碼請從51黑下載附件…………

復制代碼

所有資料51hei提供下載:

龍邱自平衡車清華方案自由滑行版.rar (215.32 KB, 下載次數(shù): 54)

ID:712298 · 發(fā)表于 2020-5-4 15:15

sadbkj 發(fā)表于 2019-4-30 00:13
大佬，這是用的啥軟件啊

這是Codewarrior 5.1，應該是基于XS128單片機的

ID:737781 · 發(fā)表于 2020-4-27 09:29

是龍邱k60的嗎

ID:417080 · 發(fā)表于 2019-4-30 00:13

大佬，這是用的啥軟件啊

帳號		自動登錄	找回密碼
密碼			立即注冊