#include "derivative.h" /* derivative-specific definitions */
#define PITTIME0 2500 //5ms定时
#define Acc_zeroset 340//角度零偏 //335 331 340
#define Gyor_zeroset 340//角加速度零偏
#define MOTORDEATH1 40//电机前倾死区电压 //150
#define MOTORDEATH2 40 //电机后倾死区电压 //310
#define P_ANGLE1 90.0 //角度前倾比例 90
#define P_ANGLE2 90.0 //角度后倾比例 90
#define D_ANGLE1 2.0//角度积分
int CAR_SPEED_SETfDelta=255; //预想速度 180
#define SPEED_CONTROL_P 0.12//速度比例 0.12
#define SPEED_CONTROL_I 0.0005//速度积分 0.0005
float DIR_CONTROL_P= 385; // 260
#define DD 3200 //1900
float Acc_Z_7260=0.0,GYRO_VAL_en03=0.0; //原始数据
float ACC=0.0,GYRO=0.0;//角度和角速度
float g_nLeftMotorPulseSigma=0, g_nRightMotorPulseSigma=0, g_fCarSpeed=0; // 速度读取，小车速度
float g_fRightVoltageSigma =0, g_fLeftVoltageSigma =0,e=0,g=0,dl=0;
int zhankong1=0,zhankong2=0;
float a=0,b=0,c=0,d=0,aa=0,bb=0,acc_err=0,last_acc=0;
int flag=0,flag1=0;//标志位
int MOTOR1=0; //PWM1计算结果
float MOTOR2=0,MOTOR2_END=0.0; //速度控制积分，速度控制反馈
float D_err=0,last_err=0,I_err=0;
int zhengfu=0,sudu=0,liangpian=0,yy=0;
void SetBusCLK_80M(void)
{
CLKSEL=0X00; //disengage PLL to system
PLLCTL_PLLON=1; //turn on PLL
SYNR =0xc0 | 0x09;
REFDV=0x80 | 0x01;
POSTDIV=0x00; //pllclock=2*osc*(1+SYNR)/(1+REFDV)=160MHz;
_asm(nop); //BUS CLOCK=80M
_asm(nop);
while(!(CRGFLG_LOCK==1)); //when pll is steady ,then use it;
CLKSEL_PLLSEL =1; //engage PLL to system;
}
void PIT_Init(void)//定时中断初始化函数
{
//通道0初始化，20ms定时中断设置
//PITCFLMT_PITE=0;//定时中的通道，四个通道均关闭
PITCE_PCE0=1;//定时器通道0使能
PITMUX_PMUX0=0;//0:8位定时器0与16位定时器共同作用，需设定PITMTLD0初值
//1:8位定时器1与16位定时器共同作用，需设定PITMTLD1初值
PITMTLD0=160-1;//8位定时器0初值设定，80MHzBusClock下为2us，设定一次即可
// PITMTLD1=160-1;//8位定时器1初值设定，80MHzBusClock下为2us，设定一次即可
PITLD0=PITTIME0-1;//16位定时器初值设定,time=PITTIME*0.02MS
PITINTE_PINTE0=1;//1定时器中断通道0中断使能
PITCFLMT_PITE=1;//定时器使能，初始化一次即可
}
void ADC_Init(void) //10精度AD采集
{
ATD0CTL0=0x07;
ATD0CTL1=0xa0; //7:1-外部触发,65:00-8位精度,01-10精度，10-精度，,4:放电,3210:ch
ATD0CTL2=0x40; //禁止外部触发, 中断禁止
ATD0CTL3=0xB0; //a右对齐无符号,每次转换6个序列, No FIFO, Freeze模式下继续转
ATD0CTL4=0xff; //765
:采样时间为4个AD时钟周期,ATDClock=[BusClock*0.5]/[PRS+1]
ATD0CTL5=0x30; //6:0特殊通道禁止,5:1连续转换 ,4:1多通道轮流采样
ATD0DIEN=0x00; //禁止数字输?/禁止数字输入
}
float ReadATD(byte ch)
{
float ad=0.0;
while(!ATD0STAT0_SCF);
switch(ch)
{
default:
case 0:
ad= ATD0DR0;
break;
case 1:
ad= ATD0DR1;
break;
case 2:
ad= ATD0DR2;
break;
case 3:
ad= ATD0DR3;
break;
case 4:
ad= ATD0DR4;
break;
case 5:
ad= ATD0DR5;
break;
case 6:
ad= ATD0DR6;
break;
case 7:
ad= ATD0DR7;
break;
}
return ad;
}
void Init_pwm(void)
{
PWME=0x00; //PWME7,PWME6,....=0;
PWMPOL=0xff; //PPOL0,PPOL1..... 0/1 INPUT
PWMCLK=0xff; //PCLK0,PCLK1.....SELECT SA OR SB
PWMCAE=0x00; //CAE7,CAE6....左对齐
PWMCTL=240; //级联
PWMDTY67=0; //PWDTY7为占空比寄存器
PWMPER67=1000; //PWMPER7为通道周期寄存器 计数值为100
PWMDTY45=0; //PWDTY5为占空比寄存器
PWMPER45=1000;
PWMDTY23=0; //PWDTY3为占空比寄存器
PWMPER23=1000;
PWMDTY01=0; //PWDTY1为占空比寄存器
PWMPER01=1000;
PWMSCLA=5; //CLOCK A 10分频 ？？50
PWMSCLB=5; //CLOCK B 10分频
PWMPRCLK=0x33; //预分频时钟选择寄存器 总线时钟8分频 10MH
PWMSDN=128; //PWMIF=1;
//PWMIE=0;
//PWMRSTRT=0;
//PEMLVL=0;
//PWM7IN=0;
//PWM7INL=0;
//PWM7ENA=0;
PWME=255;
}
void bomakaiguan(void)
{
DDRS=0XFF;
PTS=0XFF;
DDRS=0X00;
if(PTS_PTS3==0)
{
zhengfu=1;
}
if(PTS_PTS3==1)
{
zhengfu=-1;
}
liangpian=zhengfu*(PTS_PTS0+PTS_PTS1*2+PTS_PTS2*4);
sudu=(PTS_PTS4+PTS_PTS5*2+PTS_PTS6*4);
switch(sudu)
{
case 0:CAR_SPEED_SETfDelta=195;break;//上午第三次
case 1:CAR_SPEED_SETfDelta=200;break;//上午第二次
case 2:CAR_SPEED_SETfDelta=220;break;//52米不用清零
case 3:CAR_SPEED_SETfDelta=240;break;//
case 4:CAR_SPEED_SETfDelta=250;break;//上午第一次冲成绩
case 5:CAR_SPEED_SETfDelta=255;break;//试跑道 不调零点
case 6:CAR_SPEED_SETfDelta=265;break;
case 7:CAR_SPEED_SETfDelta=230;break;
default:CAR_SPEED_SETfDelta=220;break;
}
}
void Get_Accz_Gyro(void)
{
Acc_Z_7260=ReadATD(0);//加速度传感器的x轴
GYRO_VAL_en03=ReadATD(1);
}
void INIT_jishu(void)
{
DDRB=0;
DDRA=0;
DDRK=0xFF;
PORTB=0;
PORTA=0;
}
void MEASURE(void)
{
g_nLeftMotorPulseSigma=PORTB;
g_nRightMotorPulseSigma=P
ORTA;
PORTK =0xFF; //计数清零
PORTK=0;
}
void SpeedControl(void)
{
float fDelta;
float fP, fI;
MEASURE();
g_fCarSpeed = (g_nLeftMotorPulseSigma + g_nRightMotorPulseSigma) / 2; //平均速度由左右两电机测速计算所得
g_nLeftMotorPulseSigma = g_nRightMotorPulseSigma = 0; //测速计数模块清零
fDelta = CAR_SPEED_SETfDelta - g_fCarSpeed; //与预想给定速度相减
fP = fDelta * SPEED_CONTROL_P; //实际速度与预想速度相减后的值 乘以比例系数
fI = fDelta * SPEED_CONTROL_I; //实际速度与预想速度相减后的值 乘以积分系数
MOTOR2 += fI;
MOTOR2_END=fP+MOTOR2; // 速度控制结果PWM
}
void DirectionControlOutput(void)
{
float fLeftRightAdd, fLeftRightSub, fValue;
g_fRightVoltageSigma = ReadATD(2);
g_fLeftVoltageSigma = ReadATD(3);
fLeftRightAdd =g_fLeftVoltageSigma +g_fRightVoltageSigma ; //计算ADD与SUB
fLeftRightSub = g_fLeftVoltageSigma - g_fRightVoltageSigma ;
e=fLeftRightSub/fLeftRightAdd;
D_err=e-last_err;
fValue = e * DIR_CONTROL_P+ D_err*DD; //注意此处没有进行 陀螺仪 的零点偏移计算！！！
g=fValue;
last_err=e;
}
void AngelControl(void)
{
ACC= Acc_Z_7260-Acc_zeroset-MOTOR2_END+liangpian; //求零偏值
GYRO= GYRO_VAL_en03-Gyor_zeroset;
if(ACC==0)
{
PWMDTY67=0;
PWMDTY45=0;
PWMDTY23=0;
PWMDTY01=0;
}
if(ACC>0) //前倾
{
MOTOR1=(int)(MOTORDEATH1+P_ANGLE1*ACC-D_ANGLE1*GYRO);
if(ACC>25)
MOTOR1=0;
PWMDTY67=0;
zhankong1=(int)(MOTOR1-g);// 左轮
if(zhankong1>1000)
zhankong1=1000;
if(zhankong1<0)
zhankong1=0;
PWMDTY45=zhankong1;// 左轮
PWMDTY23=0;
zhankong2=(int)(MOTOR1+g);// 左轮
if(zhankong2>1000)
zhankong2=1000;
if(zhankong2<0)
zhankong2=0;
PWMDTY01=zhankong2;
}
if(ACC<0) //后倾
{
MOTOR1=(int)(MOTORDEATH2-P_ANGLE2*ACC+D_ANGLE1*GYRO);
PWMDTY67=MOTOR1;
PWMDTY45=0;
PWMDTY23=MOTOR1;
PWMDTY01=0;
}
}
void INIT(void)
{
SetBusCLK_80M();
PIT_Init() ;//定时中断初始化函数
ADC_Init() ;//10精度AD采集
Init_pwm() ;//PWM初始化
INIT_jishu() ;//计数初始化
}
void main(void)
{
INIT(); //初始化程序
EnableInterrupts;
for(;;)
{
bomakaiguan();
Get_Accz_Gyro();
AngelControl();
}
}
#pragma CODE_SEG __NEAR_SEG NON_BANKED//指示该程序在不分页区
void interrupt 66 PIT0(void)
{
flag++;
if(flag==20)
{
SpeedControl();
flag=0;
}
DirectionControlOutput();
if(PTS_PTS7==1)
{
flag1++;
if(flag1==4000)
{
MOTOR2=0;
flag1=0;
}
}
PITTF_PTF0=1;//清中断标志位
}下载本文
