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

/*
**********************************************************
飞思卡尔智能车程序直立行走例程
北京龙丘智能科技
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
------------------------------------
程序使用说明：
该工程里写出电磁组直立行走的参考方案，通过程序可以实现小车自由滑行。
==========================================================
陀螺仪模块电路说明：
角度计算采用清华硬件电路，详见40.pdf(电磁组直立行车参考设计方案20.pdf)
第54页，图3-23.一个运算放大器实现角度计算
==========================================================
接线说明
清华版陀螺仪放置的时候，记得ENC-03在下，加速度在上
放置方向跟电机的接线也有关系的。
-----------------------------------------
单片机 | 龙邱陀螺仪模块 |
--------+----------------+---------------
AD0 | gyro角速度 |
AD1 | ang角度 |
=========================================
单片机 | 电机驱动模块 | 车模电机
--------+----------------+---------------
PP7 | IN4 OUT1 | 右边电机正
PB1 | IN3 OUT2 | 右边电机负
PB0 | IN2 OUT3 | 左边电机正
PP3 | IN1 OUT4 | 左边电机负
单片机5V| Vmcu |
===========================================================
调整步骤：
1.下载程序到单片机，
2.进入在线调试状态，
3.把车竖直立起来达到平衡状态，在线查看寄存器数值
根据平衡状态下的ANG，确定ANG_center数值
根据平衡状态下的GYRO，确定GYRO_center数值
4.中心数值确定后，再调整PD参数
P系数相当于倒立摆的回复力。这个参数应该大于重力加速度的等效数值车模才能够保持直立。
随着该参数逐步增大，车模开始能够保持直立。进一步增大该参数会引起车模震荡。
D系数相当于倒立摆的阻尼力。该参数可以有效抑制车模的的震荡。当该参数过大的时候，车模开始抖动。
D系数,先令D=0，然后逐步增加P，直到车模发生震荡，再增加D消除震荡，直到车模开始抖动；
然后增加P，消除抖动；
5.反复几次后就可以找到适当的P,D。
6.小车基本平衡后用户可以根据需要添加速度控制和寻线算法，比赛的重点就在这里，各尽其能！
==========================================================
***********************************************************
*/
#include <hidef.h>
#include "derivative.h"
#define PWM_DIR0 PORTB_PB0 //电机方向控制
#define PWM_DIR1 PORTB_PB1 //电机方向控制
word AD_CAIJI[2]={0,0}; //采集的AD数值临时存放数组
word AD_QIUHE[2]={0,0}; //求和临时存放数组
word AD_JUNZHI[2]={0,0}; //平均值
volatile byte PIT_CNT=0; //中断计数
volatile int ANG=0; //角度，前倾变大，后倾变小，平衡时候ANG_center =
volatile int GYRO=0; //角速度，前倾变小，后倾变大，静止时候为GYRO_center=？
volatile int PID1_P=15; //P系数,pwm3
volatile int PID1_D=6; //D系数,pwm3
volatile int PID2_P=15; //P系数,pwm7
volatile int PID2_D=6; //D系数,pwm7
volatile int ANG_center =319; //AD的初始值，每个模块数值都不一样，跟换模块需要重新确定该值
volatile int GYRO_center=367; //角速度初始值，电机不动时候静止状态下采集的数值
volatile int AD_number=20; //求和数量
volatile word DTY1=0; //临时变量
volatile word DTY2=0; //临时变量
volatile word DTYMAX=245; //PWM占空比最大值
#define FDOWN 253 //前向倒下时的角度数值
#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;// 禁止外部触发,标志位快速清零,中断禁止
ATD0CTL3=0b10100000;/* 7;右对齐无符号;
6~3:转换序列长度为4;
No FIFO模式,Freeze模式下继续转换? */
ATD0CTL4=0b00000111;// 4AD采样周期,ATDClock=[BusClock*0.5]/[PRS+1] ; PRS=15, divider=32 ?
ATD0CTL5=0b00110000;// 特殊通道禁止,多通道采样,扫描模式连续采样,开始为 AN0
ATD0DIEN=0b00000000;// 禁止数字输入
return;
}
//-----PWM初始化程序------//
void fPWM_Init(void)
{
PWME=0x00; /* 禁止PWM输出*/
PWMCTL=0x00; //通道不级联
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;
}
/*************** 定时器函数1ms *****/
void fPIT_Init(void) //定时中断初始化函数 1MS定时中断设置
{
PITCFLMT_PITE=0; //定时中断通道关
PITCE_PCE0=1; //定时器通道 0使能定时1ms用
PITMUX_PMUX0=1; //定时器通道 0选择8位计数器1
PITMTLD1=40-1; //8位计数器0初值设定。40分频，在 40MHzBusClock下，为 1MHz。即 1us.
PITLD0=1000-1; //16位计数器初值设定。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调节
fPWM_KONGZHI(); //电机pwm输出
//else if(PIT_CNT==4)
// ANGControl(); //直立控制
……………………
…………限于本文篇幅余下代码请从51黑下载附件…………

复制代码