智能车电磁组完整程序.docx

智能车电磁组完整程序.docx

《智能车电磁组完整程序.docx》由会员分享，可在线阅读，更多相关《智能车电磁组完整程序.docx（28页珍藏版）》请在冰豆网上搜索。

智能车电磁组完整程序

#include/*commondefinesandmacros*/

#include"derivative.h"/*derivative-specificdefinitions*/

intleft1=0;

intleft2=0;

intright1=0;

intright2=0;

intAR_LEFT=0;//left2-right2

intAR_RIGHT=0;

intCR=0;//左边相加减右边相加

intpreCR=0;

intppreCR=0;

intmkp=0;

intmki=0;

intmkd=0;

intideal_speed=0;//设定速度

intspeed=0;

ints_ideal0[6]={75,80,42,42,42,42};//普通道、长直道、普通到弯、长直道到弯、弯内、偏离黑线

ints_ideal1[6]={70,75,42,42,42,42};

ints_ideal2[6]={62,70,42,40,41,40};

ints_ideal3[6]={54,66,42,40,41,40};

inttable_mkp0[6]={30,30,30,30,30,30};//ni16.31,shun15.16

inttable_mkp1[6]={25,25,25,25,25,25};

inttable_mkp2[6]={5,4,4,20,20,20};

inttable_mkp3[6]={4,4,4,10,8,9};//稳定速度

inttable_mki0[6]={0,0,20,20,20,20};

inttable_mki1[6]={0,0,20,20,20,20};

inttable_mki2[6]={0,0,0,10,10,20};

inttable_mki3[6]={0,0,0,0,0,0};

inttable_mkd0[6]={0,0,0,0,0,0};

inttable_mkd1[6]={0,0,0,0,0,0};

inttable_mkd2[6]={0};

inttable_mkd3[6]={0,0,0,0,0,0};

ints_table[6];

intb_mkp[6]=0;

intb_mki[6]=0;

intb_mkd[6]=0;

inttable_rkp0[7]={5,3,2,550,550,550,8};//普通道中间、长直道低速、长直道高速、普到弯、直到弯、弯、普通道两边

inttable_rkp1[7]={7,5,4,450,450,400,9};

inttable_rkp2[7]={6,3,2,150,150,150,9};

inttable_rkp3[7]={5,3,2,150,150,150,9};

inttable_rkd0[7]={0,0,0,400,400,400,100};

inttable_rkd1[7]={0,0,0,500,500,500,100};

inttable_rkd2[7]={0,0,0,200,300,400,100};

inttable_rkd3[7]={0,0,0,200,300,400,100};

intb_rkp[7]=0;

intb_rkd[7]=0;

intrkp=0;

intrkd=0;

intf=0;//pwmDTY要加的值

intpref=0;

intPulse_count=0;//脉冲数

intganhuang=0;

unsignedintting=0;

inti=0;

intFlag_Chute=0;//道路标志

intGeneralCtn=0;

intCurveCtn=0;

intChuteCtn=0;

intWANCtn=0;

intFlag_gaosu=0;

unsignedcharFlag_Pwm;//知道转弯道标志

intflag=0;

/***********************

//PLL超频到40MHZ

****************/

voidPLL_Init（void）{

CLKSEL=0X00;

PLLCTL_PLLON=1;

REFDV=0X80|0X01;

SYNR=0X40|0X04;

POSTDIV=0X00;

asmnop;

asmnop;

while（!

（CRGFLG_LOCK==1））;

CLKSEL_PLLSEL=1;

}

//延时函数cnt*1ms;

voiddelay（unsignedintcnt）{

unsignedintloop_i,loop_j;

for（loop_i=0;loop_i

loop_j=0x1300;

while（loop_j--）;

}

}

/*******************************

///////////////\\计数程序//\\\\\\\\\\\\\\\

*********************************/

voidPACBInit（）//PT7获得脉冲值

{

PACTL=0X40;//PT7PIN,PACN3216BIT,Risingedge,NOTINTERRUPT

TCTL3=0x40;//c-输入捕捉7上升沿有效,

TIE_C7I=0;//通道7禁止中断

TIOS_IOS7=0;//每一位对应通道的:

0输入捕捉,1输出比较

PACNT=0;

}

voidRTI_init（void）//RTI产生10ms的中断定时

{

asmsei;//关闭中断

RTICTL=0xC7;//中断周期设置10ms中断一次（或者让RTICTL=0x59<为10.24ms定时>）

CRGINT_RTIE=1;//实时中断有效，一旦RTIF=1则发出中断请求

asmcli;//开放中断

}

//舵机初始化

voidPWM_rudder_init（void）{

PWME_PWME3=0;

PWME_PWME2=0;

PWMPRCLK_PCKB=2;//CLOCKB=BUS/4=10MHz

PWMSCLB=2;//CLOCCSB=10/（2*2）=2.5MHz

PWMCTL_CON23=1;//组合PWM23

PWMCLK_PCLK3=1;//PWM3使用SB

PWMPER23=50000;//写PWM23的周期寄存器，周期是20ms

PWMPOL_PPOL3=1;//极性为正

PWMCAE_CAE3=0;//左对齐

PWME_PWME3=1;//使能PWM23

}

//电机初始化

voidPWM_init_motor（void）{//电机初始化

PWME_PWME0=0;

PWME_PWME1=0;

PWMPRCLK_PCKA=2;//ClockA=40M/4=10M

PWMPOL_PPOL1=1;//通道1正极性输出

PWMCLK_PCLK1=0;//通道1选择A时钟

PWMCAE_CAE1=0;//左对齐

PWMCTL_CON01=1;

PWMPER01=1000;//输出频率=10M/1000=10Khz

PWMDTY01=0;//通道1占空比为100/250

PWME_PWME1=1;//通道1使能

PWME_PWME4=0;

PWME_PWME5=0;

PWMPRCLK_PCKA=1;//ClockA=40M/2=20M

PWMPOL_PPOL5=1;//通道5正极性输出

PWMCLK_PCLK5=1;//通道5选择SA时钟

PWMSCLA=1;//ClockSB=20M/（2*1）=10M

PWMCAE_CAE5=0;//左对齐

PWMCTL_CON45=1;

PWMPER45=1000;//输出频率=10M/1000=10Khz

PWMDTY45=0;//初始通占空比0

PWME_PWME5=1;//通道5使能

}

voidAD_Init（void）

{

ATD0CTL1=0x20;//选择AD通道为外部触发,10位精度,采样前不放电

ATD0CTL2=0x40;//标志位自动清零，禁止外部触发,禁止中断

ATD0CTL3=0xA0;//右对齐无符号,每次转换4个序列,NoFIFO,Freeze模式下继续转

ATD0CTL4=0x09;//采样时间为4个AD时钟周期,PRS=9,ATDClock=40/（2*（9+1））2MHz

ATD0CTL5=0x30;//特殊通道禁止,连续转换4个通道,多通道转换，起始通道为0转换

ATD0DIEN=0x00;//禁止数字输入

}

/************************

/////////检测起跑线\\\\\\\\\\\

*****************/

voidCheckstart（）{

asmsei;

TIOS_IOS0=0;//输入捕捉

TSCR1=0X80;

TSCR2=0X07;

TCTL4=0X01;//上升沿捕捉

TIE=0X01;//允许硬件中断

asmcli;

}

//拨码开关

voidboman（）{

if（PORTA_PA0==1）{

b_rkp[0]=table_rkp0[0];

b_rkp[1]=table_rkp0[1];

b_rkp[2]=table_rkp0[2];

b_rkp[3]=table_rkp0[3];

b_rkp[4]=table_rkp0[4];

b_rkp[5]=table_rkp0[5];

b_rkp[6]=table_rkp0[6];

b_rkd[0]=table_rkd0[0];

b_rkd[1]=table_rkd0[1];

b_rkd[2]=table_rkd0[2];

b_rkd[3]=table_rkd0[3];

b_rkd[4]=table_rkd0[4];

b_rkd[5]=table_rkd0[5];

b_rkd[6]=table_rkd0[6];

b_mkp[0]=table_mkp0[0];

b_mkp[1]=table_mkp0[1];

b_mkp[2]=table_mkp0[2];

b_mkp[3]=table_mkp0[3];

b_mkp[4]=table_mkp0[4];

b_mkp[5]=table_mkp0[5];

b_mki[0]=table_mki0[0];

b_mki[1]=table_mki0[1];

b_mki[2]=table_mki0[2];

b_mki[3]=table_mki0[3];

b_mki[4]=table_mki0[4];

b_mki[5]=table_mki0[5];

b_mkd[0]=table_mkd0[0];

b_mkd[1]=table_mkd0[1];

b_mkd[2]=table_mkd0[2];

b_mkd[3]=table_mkd0[3];

b_mkd[4]=table_mkd0[4];

b_mkd[5]=table_mkd0[5];

s_table[0]=s_ideal0[0];

s_table[1]=s_ideal0[1];

s_table[2]=s_ideal0[2];

s_table[3]=s_ideal0[3];

s_table[4]=s_ideal0[4];

s_table[5]=s_ideal0[5];

}

if（PORTA_PA1==1）{

b_rkp[0]=table_rkp1[0];

b_rkp[1]=table_rkp1[1];

b_rkp[2]=table_rkp1[2];

b_rkp[3]=table_rkp1[3];

b_rkp[4]=table_rkp1[4];

b_rkp[5]=table_rkp1[5];

b_rkp[6]=table_rkp1[6];

b_rkd[0]=table_rkd1[0];

b_rkd[1]=table_rkd1[1];

b_rkd[2]=table_rkd1[2];

b_rkd[3]=table_rkd1[3];

b_rkd[4]=table_rkd1[4];

b_rkd[5]=table_rkd1[5];

b_rkd[6]=table_rkd1[6];

b_mkp[0]=table_mkp1[0];

b_mkp[1]=table_mkp1[1];

b_mkp[2]=table_mkp1[2];

b_mkp[3]=table_mkp1[3];

b_mkp[4]=table_mkp1[4];

b_mkp[5]=table_mkp1[5];

b_mki[0]=table_mki1[0];

b_mki[1]=table_mki1[1];

b_mki[2]=table_mki1[2];

b_mki[3]=table_mki1[3];

b_mki[4]=table_mki1[4];

b_mki[5]=table_mki1[5];

b_mkd[0]=table_mkd1[0];

b_mkd[1]=table_mkd1[1];

b_mkd[2]=table_mkd1[2];

b_mkd[3]=table_mkd1[3];

b_mkd[4]=table_mkd1[4];

b_mkd[5]=table_mkd1[5];

s_table[0]=s_ideal1[0];

s_table[1]=s_ideal1[1];

s_table[2]=s_ideal1[2];

s_table[3]=s_ideal1[3];

s_table[4]=s_ideal1[4];

s_table[5]=s_ideal1[5];

}

if（PORTA_PA2==1）{

b_rkp[0]=table_rkp2[0];

b_rkp[1]=table_rkp2[1];

b_rkp[2]=table_rkp2[2];

b_rkp[3]=table_rkp2[3];

b_rkp[4]=table_rkp2[4];

b_rkp[5]=table_rkp2[5];

b_rkp[6]=table_rkp2[6];

b_rkd[0]=table_rkd2[0];

b_rkd[1]=table_rkd2[1];

b_rkd[2]=table_rkd2[2];

b_rkd[3]=table_rkd2[3];

b_rkd[4]=table_rkd2[4];

b_rkd[5]=table_rkd2[5];

b_rkd[6]=table_rkd2[6];

b_mkp[0]=table_mkp2[0];

b_mkp[1]=table_mkp2[1];

b_mkp[2]=table_mkp2[2];

b_mkp[3]=table_mkp2[3];

b_mkp[4]=table_mkp2[4];

b_mkp[5]=table_mkp2[5];

b_mki[0]=table_mki2[0];

b_mki[1]=table_mki2[1];

b_mki[2]=table_mki2[2];

b_mki[3]=table_mki2[3];

b_mki[4]=table_mki2[4];

b_mki[5]=table_mki2[5];

b_mkd[0]=table_mkd2[0];

b_mkd[1]=table_mkd2[1];

b_mkd[2]=table_mkd2[2];

b_mkd[3]=table_mkd2[3];

b_mkd[4]=table_mkd2[4];

b_mkd[5]=table_mkd2[5];

s_table[0]=s_ideal2[0];

s_table[1]=s_ideal2[1];

s_table[2]=s_ideal2[2];

s_table[3]=s_ideal2[3];

s_table[4]=s_ideal2[4];

s_table[5]=s_ideal2[5];

}

if（PORTA_PA3==1）{

b_rkp[0]=table_rkp3[0];

b_rkp[1]=table_rkp3[1];

b_rkp[2]=table_rkp3[2];

b_rkp[3]=table_rkp3[3];

b_rkp[4]=table_rkp3[4];

b_rkp[5]=table_rkp3[5];

b_rkp[6]=table_rkp3[6];

b_rkd[0]=table_rkd3[0];

b_rkd[1]=table_rkd3[1];

b_rkd[2]=table_rkd3[2];

b_rkd[3]=table_rkd3[3];

b_rkd[4]=table_rkd3[4];

b_rkd[5]=table_rkd3[5];

b_rkd[6]=table_rkd3[6];

b_mkp[0]=table_mkp3[0];

b_mkp[1]=table_mkp3[1];

b_mkp[2]=table_mkp3[2];

b_mkp[3]=table_mkp3[3];

b_mkp[4]=table_mkp3[4];

b_mkp[5]=table_mkp3[5];

b_mki[0]=table_mki3[0];

b_mki[1]=table_mki3[1];

b_mki[2]=table_mki3[2];

b_mki[3]=table_mki3[3];

b_mki[4]=table_mki3[4];

b_mki[5]=table_mki3[5];

b_mkd[0]=table_mkd3[0];

b_mkd[1]=table_mkd3[1];

b_mkd[2]=table_mkd3[2];

b_mkd[3]=table_mkd3[3];

b_mkd[4]=table_mkd3[4];

b_mkd[5]=table_mkd3[5];

s_table[0]=s_ideal3[0];

s_table[1]=s_ideal3[1];

s_table[2]=s_ideal3[2];

s_table[3]=s_ideal3[3];

s_table[4]=s_ideal3[4];

s_table[5]=s_ideal3[5];

}

}

/**************************

////////赛道特征识别///////

****************************/

voidRoadjudge（void）{

if（Flag_Chute==0）//普通弯道情况0

{GeneralCtn=0;

CurveCtn=0;

if（CR>=-23&&CR<=40）//此时对应一个车轮的内侧压线@@@@@@@@@@@@@@@@@@@@@@@@@@@@

{

ChuteCtn++;

WANCtn=0;

}

if（CR<-23||CR>40）{//@@@@@@@@@@@@@@@@@@@@@@@@@@

if（CR<-85||CR>90）{//对应车轮的外侧压线@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@

ChuteCtn=0;

WANCtn++;

}

else{

ChuteCtn=0;

WANCtn=0;

}

}

if（ChuteCtn>10000）//300

{

Flag_Chute=1;

}

if（WANCtn<-10000）{

Flag_Chute=2;

}

}

if（Flag_Chute==1）//长直道情况1

{

ChuteCtn=0;

GeneralCtn=0;

WANCtn=0;

if（CR>40）CurveCtn++;//60@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@

if（CR<-23）CurveCtn--;//-60@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@

if（CurveCtn>1||CurveCtn<-1）

{

if（Flag_gaosu==1）//高速时转入大弯道情况

{

Flag_Chute=2;

}

if（Flag_gaosu==0）

Flag_Chute=0;//低速时转入普通弯道情况

}

}

if（Flag_Chute==2）//大弯道情况2

{

ChuteCtn=0;

CurveCtn=0;

WANCtn=0;//@@@@@@@@@@@@@@@@@@@@@@@

if（CR>-23&&CR<35）GeneralCtn++;

elseGeneralCtn=0;

if（GeneralCtn>1300）Flag_Chute=0;

//if（GeneralCtn>130）Flag_Ct=1;

//elseFlag_Ct=0;

//if（k_abs（Turn_C-Turn<54）

//{

//Flag_Zhj=1;

//}

//else

//{

//Flag_Zhj=0;

//}

//if（Flag_Pwm==1&&Flag_Ct==1）Flag_Chute=1;

}

}

/******************************

\\\\\\\\\\\\\\\\\\\\//舵机控制\\\\\\\\\\\\\\\\\

*********************************/

voidrudder_ctrl（void）{

if（Flag_Chute==0）//普通道

{

Flag_Pwm=0;

if（CR<40&&CR>-23）{

rkp=b_rkp[0];

rkd=b_rkd[0];

}

else{

rkp=b_rkp[6];

rkd=b_rkp[6];

}

}

elseif（Flag_Chute==1）//长直道

{

Flag_Pwm=1;

if（Fl