智能车电磁组完整程序.docx
《智能车电磁组完整程序.docx》由会员分享，可在线阅读，更多相关《智能车电磁组完整程序.docx（24页珍藏版）》请在冰豆网上搜索。
智能车电磁组完整程序
#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_iloop_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（Flag_gaosu
升级会员 