for(jj=0;jj<2770;jj++);//32MHz--1ms
}
staticvoidPWM_Init(void)
{PWME=0x00;
//舵机
PWMCTL_CON01=1;//0和1联合成16位PWM
PWMCAE_CAE1=0;//选择输出模式为左对齐输出模式
PWMCNT01=0;//计数器清零
PWMPOL_PPOL1=1;//先输出高电平,计数到DTY时,反转电平
PWMPRCLK=0x05;//clockA不分频,即clockA=busclok
//PWMSCLA=1;//对clockSA进行32分频,PWMclock=clockA/16*2=1MHz
PWMPER01=20000;//周期为10ms;100Hz
PWMDTY01=1550;//舵机中心位置
PWMCLK_PCLK1=0;//选择clockSA作时钟源
//电机正转
PWMCTL_CON23=1;//2和3联合成16位PWM
PWMCAE_CAE3=0;//选择输出模式为左对齐输出模式
PWMCNT23=0;//计数器清零
PWMPOL_PPOL3=1;//先输出高电平,计数到DTY时,反转电平
PWMPRCLK=0x05;//clockB不分频,即clockB=busclok
//PWMSCLB=0x10;//对clockSB进行32分频,PWMclock=clockB/16*2=1MHz
//PWMPER23=55;//周期为55us;18181Hz
PWMSCLB=160;//对clockSB进行32分频,PWMclock=clockB/160*2=0.1MHz
PWMPER23=1000;//周期为100us;10KHz
PWMCLK_PCLK3=1;//选择clockSB作时钟源
//PWMDTY23=0;
//电机反转
PWMCTL_CON45=1;//4和5联合成16位PWM
PWMCAE_CAE5=0;//选择输出模式为左对齐输出模式
PWMCNT45=0;//计数器清零
PWMPOL_PPOL5=1;//先输出高电平,计数到DTY时,反转电平
PWMPRCLK=0x05;//clockB不分频,即clockB=busclok
PWMSCLA=5;//对clockSA进行16分频,PWMclock=clockA/16=0.1MHz
PWMPER45=1000;//周期为100us;10KHz
PWMCLK_PCLK5=1;//选择clockSA作时钟源
//PWMDTY45=0;
PWME=0xff;
}
voidSetBusCLK_32M(void)
{
//CLKSEL=0X00;//disengagePLLtosystem
//PLLCTL_PLLON=1;//turnonPLL
SYNR=3;//pllclock=2*osc*(1+SYNR)/(1+REFDV)=64MHz;
REFDV=1;
//POSTDIV=0x00;
//_asm(nop);//BUSCLOCK=32M
//_asm(nop);
while(!
(CRGFLG_LOCK==1));//whenpllissteady,thenuseit;
CLKSEL_PLLSEL=1;//engagePLLtosystem;
}
voidAD_Init(void)
{
ATD0CTL0=0X01;
ATD0CTL1=0x00;//7:
1-外部触发,65:
00-8位精度,4:
放电,3210:
ch
ATD0CTL2=0x60;//启动A/D转换,快速清零,无等待模式,禁止外部触发,中断禁止
ATD0CTL3=0xc0;//右对齐无符号,每次转换13个序列,NoFIFO,Freeze模式下继续转
ATD0CTL4=0x09;//765:
采样时间为4个AD时钟周期,ATDClock=[BusClock*0.5]/[PRS+1]
ATD0CTL5=0x30;//6:
0特殊通道禁止,5:
1连续转换,4:
1多通道轮流采样
ATD0DIEN=0x00;//禁止数字输入
}
voidIRQ_Init(void){
IRQCR_IRQE=1;//1IRQconfiguredtorespondonlytofallingedges
IRQCR_IRQEN=0;//0IRQinterruptdisable
}
voidInitSpeed(void){
PACNT=0;//脉冲累加器A寄存器清零
PACTL_PAEN=1;//使能脉冲累加器A
PACTL_PAMOD=0;
PACTL_PEDGE=1;//上升沿
}
voidRTI_Init(void){
CRGINT_RTIE=1;
CRGFLG_RTIF=1;
PLLCTL_PRE=1;
RTICTL=0x5F;//定时32.7ms30Hz
}
voidPIT_init(void)//定时中断初始化函数5MS定时中断设置
{
PITCFLMT_PITE=0;//定时中断通道0关
PITCE_PCE0=1;//定时器通道0使能
PITMTLD0=160-1;//8位定时器初值设定,160分频,在32MHzBusClock下,为0.2MHz。
即5us
PITLD0=PITTIME-1;//16位定时器初值设定。
PITTIME*0.005MS
PITINTE_PINTE0=1;//定时器中断通道0中断使能
PITCFLMT_PITE=1;//定时器通道0使能
}
#pragmaCODE_SEG__NEAR_SEGNON_BANKED//指示该程序在不分页区
voidinterrupt66PIT0(void)
{
count++;
if(count==6000)
{
PWME=0x03;
//count=0;
}
PITTF_PTF0=1;//清中断标志位
}
voidInit_Dev(void)
{
SetBusCLK_32M();
PWM_Init();
AD_Init();
//PIT_init();
//RTI_Init();
//IRQ_Init();
//InitSpeed();//初始化累加器
}
unsignedintangle_table[13];
unsignedintangle_table1[13]={
1400,1440,1480,1500,1520,1530,
1550,
1570,1590,1620,1640,1680,1700,
};
unsignedintangle_table2[13]={
1400,1415,1430,1445,1470,1520,
1550,
1580,1630,1655,1670,1685,1700,
};
unsignedintangle_table3[13]={
1400,1410,1420,1430,1450,1460,
1550,
1620,1630,1670,1680,1690,1700,
};
unsignedintangle_table4[13]={
1400,1410,1420,1430,1440,1470,
1550,
1630,1660,1670,1680,1690,1700,
};
/*****************************00状态弯道******************************************/
voidangle00()
{intdty01,dty23;
if(ch0>=ch1&&ch1>=ch1_mid){
PWMDTY01=1550;
//PWMDTY23=382;
PWMDTY23=500;
}elseif(ch0>ch1&&ch1>=10)
{
/*if(PWMDTY23==382){
PWMDTY23=1000;
PWMDTY45=382;
delay(6);
PWMDTY45=1000;
}*/
dty01=1550-(ch1_mid-ch1)*6;
dty23=808;
if(dty01<=1400){
dty01=1400;
dty23=381;
}
PWMDTY01=dty01;
//dty23=500+dif/10*20;
//if(dty23>700)
//dty23=700;
PWMDTY23=dty23;
}
if(ch0=ch0_mid){
PWMDTY01=1550;
//PWMDTY23=382;
PWMDTY23=500;
}
elseif(ch0=10)
{
/*if(PWMDTY23==382){
PWMDTY23=1000;
PWMDTY45=382;
delay(6);
PWMDTY45=1000;
}
*/
dty01=1550+(ch0_mid-ch0)*6;
dty23=808;
if(dty01>=1700){
dty01=1700;
dty23=381;
}
PWMDTY01=dty01;
//dty23=500+(-dif)/10*20;
//if(dty23>800)
//dty23=800;
PWMDTY23=dty23;
}
}
/*****************************01状态弯道******************************************/
/*voidangle01()
{
if(ch0>=ch1)
{
if(ch1=(ch1_mid-tidu))
{
PWMDTY01=angle_table[5];
PWMDTY23=speed_bend;
//PORTB=0x0f;
}
elseif(ch1<(ch1_mid-tidu)&&ch1>=(ch1_mid-(2*tidu)))
PWMDTY01=angle_table[4];
elseif(ch1<(ch1_mid-(2*tidu))&&ch1>=(ch1_mid-(3*tidu)))
PWMDTY01=angle_table[3];
elseif(ch1<(ch1_mid-(3*tidu))&&ch1>=(ch1_mid-(4*tidu)))
PWMDTY01=angle_table[2];
elseif(ch1<(ch1_mid-(4*tidu))&&ch1>=(ch1_mid-(5*tidu)))
PWMDTY01=angle_table[1];
elseif(ch1<(ch1_mid-(6*tidu))&&ch1>=(ch1_mid-(6*tidu)))
PWMDTY01=angle_table[0];
elseif(ch1>=ch1_mid)
PWMDTY01=angle_table1[6];
}
else
{
if(ch0=(ch0_mid-tidu))
{
PWMDTY01=angle_table[7];
PWMDTY23=speed_bend;
//PORTB=0xf0;
}
elseif(ch0<(ch0_mid-tidu)&&ch0>=(ch0_mid-(2*tidu)))
PWMDTY01=angle_table[8];
elseif(ch0<(ch0_mid-(2*tidu))&&ch0>=(ch0_mid-(3*tidu)))
PWMDTY01=angle_table[9];
elseif(ch0<(ch0_mid-(3*tidu))&&ch0>=(ch0_mid-(4*tidu)))
PWMDTY01=angle_table[10];
elseif(ch0<(ch0_mid-(4*tidu))&&ch0>=(ch0_mid-(5*tidu)))
PWMDTY01=angle_table[11];
elseif(ch0<(ch0_mid-(5*tidu))&&ch0>=(ch0_mid-(6*tidu)))
PWMDTY01=angle_table[12];
elseif(ch0>=ch0_mid)
{
PWMDTY01=angle_table[6];
PWMDTY23=speed_mid;
}
}
}*/
voidangle01()
{intdty01,dty23;
if(ch0>=ch1&&ch1>=ch1_mid){
PWMDTY01=1550;
//PWMDTY23=382;
PWMDTY23=150;
}elseif(ch0>ch1&&ch1>=10)
{
/*if(PWMDTY23==382){
PWMDTY23=1000;
PWMDTY45=382;
delay(6);
PWMDTY45=1000;
}*/
dty01=1550-(ch1_mid-ch1)*6;
dty23=618;
if(dty01<=1400){
dty01=1400;
dty23=381;
}
PWMDTY01=dty01;
//dty23=500+dif/10*20;
//if(dty23>700)
//dty23=700;
PWMDTY23=dty23;
}
if(ch0=ch0_mid){
PWMDTY01=1550;
//PWMDTY23=382;
PWMDTY23=150;
}
elseif(ch0=10)
{
/*if(PWMDTY23==382){
PWMDTY23=1000;
PWMDTY45=382;
delay(6);
PWMDTY45=1000;
}
*/
dty01=1550+(ch0_mid-ch0)*6;
if(dty01>=1700){
dty01=1700;
dty23=381;
}
PWMDTY01=dty01;
//dty23=500+(-dif)/10*20;
//if(dty23>800)
//dty23=800;
PWMDTY23=dty23;
}
}
/*****************************10状态弯道******************************************/
voidangle10()
{
if(ch0>=ch1)
{
if(ch1=(ch1_mid-tidu))
{
PWMDTY01=angle_table[5];
PWMDTY23=speed_bend;
//PORTB=0x0f;
}
elseif(ch1<(ch1_mid-tidu)&&ch1>=(ch1_mid-(2*tidu)))
PWMDTY01=angle_table[4];
elseif(ch1<(ch1_mid-(2*tidu))&&ch1>=(ch1_mid-(3*tidu)))
PWMDTY01=angle_table[3];
elseif(ch1<(ch1_mid-(3*tidu))&&ch1>=(ch1_mid-(4*tidu)))
PWMDTY01=angle_table[2];
elseif(ch1<(ch1_mid-(4*tidu))&&ch1>=(ch1_mid-(5*tidu)))
PWMDTY01=angle_table[1];
elseif(ch1<(ch1_mid-(5*tidu))&&ch1>=(ch1_mid-(6*tidu)))
PWMDTY01=angle_table[0];
elseif(ch1>=ch1_mid)
{
PWMDTY01=angle_table1[6];
PWMDTY23=speed_mid;
//PORTB=0xe7;
}
}
else
{
if(ch0=(ch0_mid-tidu))
{
PWMDTY01=angle_table[7];
PWMDTY23=speed_bend;
//PORTB=0xf0;
}
elseif(ch0<(ch0_mid-tidu)&&ch0>=(ch0_mid-(2*tidu)))
PWMDTY01=angle_table[8];
elseif(ch0<(ch0_mid-(2*tidu))&&ch0>=(ch0_mid-(3*tidu)))
PWMDTY01=angle_table[9];
elseif(ch0<(ch0_mid-(3*tidu))&&ch0>=(ch0_mid-(4*tidu)))
PWMDTY01=angle_table[10];
elseif(ch0<(ch0_mid-(4*tidu))&&ch0>=(ch0_mid-(5*tidu)))
PWMDTY01=angle_table[11];
elseif(ch0<(ch0_mid-(5*tidu))&&ch0>=(ch0_mid-(6*tidu)))
PWMDTY01=angle_table[12];
elseif(ch0>=ch0_mid)
{
PWMDTY01=angle_table1[6];
PWMDTY23=speed_mid;
//PORTB=0xe7;
}
}
}
/*****************************11状态弯道******************************************/
voidangle11()
{
if(ch0>=ch1)
{//i=0;
if(ch1=(ch1_mid-tidu))
{
PWMDTY01=angle_table[5];
PWMDTY23=speed_bend-150;
//PORTB=0x0f;
}
elseif(ch1<(ch1_mid-tidu)&&ch1>=(ch1_mid-(2*tidu))){
PWMDTY01=angle_table[4];
PWMDTY23=speed_bend-150;
}
elseif(ch1<(ch1_mid-(2*tidu))&&ch1>=(ch1_mid-(3*tidu))){
PWMDTY01=angle_table[3];
PWMDTY23=speed_bend-100;
}
elseif(ch1<(ch1_mid-(3*tidu))&&ch1>=(ch1_mid-(4*tidu))){
PWMDTY01=angle_table[2];
PWMDTY23=speed_bend-50;
}
elseif(ch1<(ch1_mid-(4*tidu))&&ch1>=(ch1_mid-(5*tidu))){
PWMDTY01=angle_table[1];
PWMDTY23=speed_bend-50;
}
elseif(ch1<(ch1_mid-(5*tidu))&&ch1>=(ch1_mid-(6*tidu))){
PWMDTY01=angle_table[0];
PWMDTY23=speed_mid-50;
}
elseif(ch1>=ch1_mid)
{
PWMDTY01=angle_table[6];
PWMDTY23=speed_mid;
//PORTB=0xe7;
}
}
else
{//i=0;
if(ch0=(ch0_mid-tidu))
{
PWMDTY01=angle_table[7];
PWMDTY23=speed_bend-150;
}
elseif(ch0<(ch0_mid-tidu)&&ch0>=(ch0_mid-(2*tidu))){
PWMDTY01=angle_table[8];
PWMDTY23=speed_bend-100;
}
elseif(ch0<(ch0_mid-(2*tidu))&&ch0>=(ch0_mid-(3*tidu))){
PWMDTY01=angle_table[9];
PWMDTY23=speed_bend