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

#include /* common defines and macros */#include derivative.h /* derivative-specific definitions */int left1=0;int left2=0;int right1=0;int right2=0;int AR_LEFT=0;/left2-right2int AR_RIGHT=0;int CR=0;/左边相加减右边相加int preCR=0;int ppreCR=0;int mkp=0;int mki=0;int mkd=0;int ideal_speed=0; /设定速度int speed=0;int s_ideal06=75,80,42,42,42,42; /普通道、长直道、普通到弯、长直道到弯、弯内、偏离黑线int s_ideal16=70,75,42,42,42,42;int s_ideal26=62,70,42,40,41,40;int s_ideal36=54,66,42,40,41,40;int table_mkp06=30,30,30,30,30,30; /ni 16.31 ,shun 15.16int table_mkp16=25,25,25,25,25,25; int table_mkp26=5,4,4,20,20,20;int table_mkp36=4,4,4,10,8,9; /稳定速度int table_mki06=0,0,20,20,20,20;int table_mki16=0,0,20,20,20,20;int table_mki26=0,0,0,10,10,20;int table_mki36=0,0,0,0,0,0;int table_mkd06=0,0,0,0,0,0;int table_mkd16=0,0,0,0,0,0;int table_mkd26=0;int table_mkd36=0,0,0,0,0,0;int s_table6;int b_mkp6=0;int b_mki6=0;int b_mkd6=0;int table_rkp07=5,3,2,550,550,550,8;/普通道中间、长直道低速、长直道高速、普到弯、直到弯、弯、普通道两边int table_rkp17=7,5,4,450,450,400,9;int table_rkp27=6,3,2,150,150,150,9;int table_rkp37=5,3,2,150,150,150,9;int table_rkd07=0,0,0,400,400,400,100;int table_rkd17=0,0,0,500,500,500,100;int table_rkd27=0,0,0,200,300,400,100;int table_rkd37=0,0,0,200,300,400,100;int b_rkp7=0;int b_rkd7=0;int rkp=0;int rkd=0;int f=0;/ pwmDTY要加的值int pref=0;int Pulse_count=0; /脉冲数int ganhuang=0;unsigned int ting=0;int i=0; int Flag_Chute=0; /道路标志int GeneralCtn=0;int CurveCtn=0;int ChuteCtn=0;int WANCtn=0;int Flag_gaosu=0;unsigned char Flag_Pwm;/知道转弯道标志 int flag=0; /*/PLL超频到40MHZ*/ void PLL_Init(void) CLKSEL=0X00; PLLCTL_PLLON=1; REFDV=0X80|0X01; SYNR=0X40|0X04; POSTDIV=0X00; asm nop; asm nop; while(!(CRGFLG_LOCK=1); CLKSEL_PLLSEL=1;/延时函数 cnt*1ms;void delay(unsigned int cnt) unsigned int loop_i,loop_j; for(loop_i=0;loop_icnt;loop_i+) loop_j=0x1300; while(loop_j-); /*/计数程序/*/void PACBInit() /PT7 获得脉冲值 PACTL=0X40; /PT7 PIN,PACN32 16BIT,Rising edge,NOT INTERRUPT TCTL3=0x40; /c-输入捕捉7 上升沿有效, TIE_C7I=0; /通道7 禁止中断 TIOS_IOS7=0; /每一位对应通道的: 0 输入捕捉,1 输出比较 PACNT = 0; void RTI_init(void) /RTI 产生 10ms 的中断定时 asm sei; /关闭中断 RTICTL=0xC7; /中断周期设置10ms 中断一次(或者让RTICTL=0x59) CRGINT_RTIE=1; /实时中断有效，一旦RTIF=1 则发出中断请求 asm cli; /开放中断 /舵机初始化void PWM_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 /电机初始化void PWM_init_motor(void) /电机初始化 PWME_PWME0=0; PWME_PWME1=0; PWMPRCLK_PCKA=2; /Clock A=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; /Clock A=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 使能 void AD_Init(void) ATD0CTL1=0x20; /选择AD通道为外部触发,10位精度,采样前不放电 ATD0CTL2=0x40; /标志位自动清零，禁止外部触发, 禁止中断 ATD0CTL3=0xA0; /右对齐无符号,每次转换4个序列, No FIFO, Freeze模式下继续转 ATD0CTL4=0x09; /采样时间为4个AD时钟周期,PRS=9,ATDClock=40/(2*(9+1)2MHz ATD0CTL5=0x30; /特殊通道禁止,连续转换4个通道 ,多通道转换，起始通道为0转换 ATD0DIEN=0x00; /禁止数字输入/*/检测起跑线*/void Checkstart() asm sei; TIOS_IOS0=0; /输入捕捉 TSCR1=0X80; TSCR2=0X07; TCTL4=0X01;/上升沿捕捉 TIE=0X01; /允许硬件中断 asm cli; /拨码开关void boman() if(PORTA_PA0=1) b_rkp0=table_rkp00; b_rkp1=table_rkp01; b_rkp2=table_rkp02; b_rkp3=table_rkp03; b_rkp4=table_rkp04; b_rkp5=table_rkp05; b_rkp6=table_rkp06; b_rkd0=table_rkd00; b_rkd1=table_rkd01; b_rkd2=table_rkd02; b_rkd3=table_rkd03; b_rkd4=table_rkd04; b_rkd5=table_rkd05; b_rkd6=table_rkd06; b_mkp0=table_mkp00; b_mkp1=table_mkp01; b_mkp2=table_mkp02; b_mkp3=table_mkp03; b_mkp4=table_mkp04; b_mkp5=table_mkp05; b_mki0=table_mki00; b_mki1=table_mki01; b_mki2=table_mki02; b_mki3=table_mki03; b_mki4=table_mki04; b_mki5=table_mki05; b_mkd0=table_mkd00; b_mkd1=table_mkd01; b_mkd2=table_mkd02; b_mkd3=table_mkd03; b_mkd4=table_mkd04; b_mkd5=table_mkd05; s_table0=s_ideal00; s_table1=s_ideal01; s_table2=s_ideal02; s_table3=s_ideal03; s_table4=s_ideal04; s_table5=s_ideal05; if(PORTA_PA1=1) b_rkp0=table_rkp10; b_rkp1=table_rkp11; b_rkp2=table_rkp12; b_rkp3=table_rkp13; b_rkp4=table_rkp14; b_rkp5=table_rkp15; b_rkp6=table_rkp16; b_rkd0=table_rkd10; b_rkd1=table_rkd11; b_rkd2=table_rkd12; b_rkd3=table_rkd13; b_rkd4=table_rkd14; b_rkd5=table_rkd15; b_rkd6=table_rkd16; b_mkp0=table_mkp10; b_mkp1=table_mkp11; b_mkp2=table_mkp12; b_mkp3=table_mkp13; b_mkp4=table_mkp14; b_mkp5=table_mkp15; b_mki0=table_mki10; b_mki1=table_mki11; b_mki2=table_mki12; b_mki3=table_mki13; b_mki4=table_mki14; b_mki5=table_mki15; b_mkd0=table_mkd10; b_mkd1=table_mkd11; b_mkd2=table_mkd12; b_mkd3=table_mkd13; b_mkd4=table_mkd14; b_mkd5=table_mkd15; s_table0=s_ideal10; s_table1=s_ideal11; s_table2=s_ideal12; s_table3=s_ideal13; s_table4=s_ideal14; s_table5=s_ideal15; if(PORTA_PA2=1) b_rkp0=table_rkp20; b_rkp1=table_rkp21; b_rkp2=table_rkp22; b_rkp3=table_rkp23; b_rkp4=table_rkp24; b_rkp5=table_rkp25; b_rkp6=table_rkp26; b_rkd0=table_rkd20; b_rkd1=table_rkd21; b_rkd2=table_rkd22; b_rkd3=table_rkd23; b_rkd4=table_rkd24; b_rkd5=table_rkd25; b_rkd6=table_rkd26; b_mkp0=table_mkp20; b_mkp1=table_mkp21; b_mkp2=table_mkp22; b_mkp3=table_mkp23; b_mkp4=table_mkp24; b_mkp5=table_mkp25; b_mki0=table_mki20; b_mki1=table_mki21; b_mki2=table_mki22; b_mki3=table_mki23; b_mki4=table_mki24; b_mki5=table_mki25; b_mkd0=table_mkd20; b_mkd1=table_mkd21; b_mkd2=table_mkd22; b_mkd3=table_mkd23; b_mkd4=table_mkd24; b_mkd5=table_mkd25; s_table0=s_ideal20; s_table1=s_ideal21; s_table2=s_ideal22; s_table3=s_ideal23; s_table4=s_ideal24; s_table5=s_ideal25; if(PORTA_PA3=1) b_rkp0=table_rkp30; b_rkp1=table_rkp31; b_rkp2=table_rkp32; b_rkp3=table_rkp33; b_rkp4=table_rkp34; b_rkp5=table_rkp35; b_rkp6=table_rkp36; b_rkd0=table_rkd30; b_rkd1=table_rkd31; b_rkd2=table_rkd32; b_rkd3=table_rkd33; b_rkd4=table_rkd34; b_rkd5=table_rkd35; b_rkd6=table_rkd36; b_mkp0=table_mkp30; b_mkp1=table_mkp31; b_mkp2=table_mkp32; b_mkp3=table_mkp33; b_mkp4=table_mkp34; b_mkp5=table_mkp35; b_mki0=table_mki30; b_mki1=table_mki31; b_mki2=table_mki32; b_mki3=table_mki33; b_mki4=table_mki34; b_mki5=table_mki35; b_mkd0=table_mkd30; b_mkd1=table_mkd31; b_mkd2=table_mkd32; b_mkd3=table_mkd33; b_mkd4=table_mkd34; b_mkd5=table_mkd35; s_table0=s_ideal30; s_table1=s_ideal31; s_table2=s_ideal32; s_table3=s_ideal33; s_table4=s_ideal34; s_table5=s_ideal35; /*/赛道特征识别/*/void Roadjudge(void) if(Flag_Chute=0) /普通弯道情况 0 GeneralCtn=0; CurveCtn=0; if(CR=-23&CR=40) /此时对应一个车轮的内侧压线 ChuteCtn+; WANCtn=0; if(CR40) / if(CR90) /对应车轮的外侧压线 ChuteCtn=0; WANCtn+; else ChuteCtn=0; WANCtn=0; if(ChuteCtn10000) / 300 Flag_Chute=1; if(WANCtn40)CurveCtn+; / 60 if(CR1|CurveCtn-23&CR1300)Flag_Chute=0; /if(GeneralCtn130)Flag_Ct=1; /else Flag_Ct=0; /if(k_abs(Turn_C-Turn54) / / Flag_Zhj=1; / / else / / Flag_Zhj=0; / / if(Flag_Pwm=1&Flag_Ct=1)Flag_Chute=1; /*/舵机控制*/void rudder_ctrl(void) if(Flag_Chute=0) /普通道 Flag_Pwm=0; if(CR-23) rkp=b_rkp0; rkd=b_rkd0; else rkp=b_rkp6; rkd=b_rkp6; else if(Flag_Chute=1) /长直道 Flag_Pwm=1; if(Flag_gaosu=0) /disu rkp=b_rkp1; /5 rkd=b_rkd1; else /gaosu rkp=b_rkp2; /4 rkd=b_rkd2; else if(Flag_Chute=2) /大弯道 if(Flag_Pwm=0) /普通道到大弯道 rkp=b_rkp3; rkd=b_rkd3; else if(Flag_Pwm=1) /直道 到 大弯道 rkp=b_rkp4; rkd=b_rkd4; else rkp=b_rkp5; rkd=b_rkd5; f=3800+rkp*CR+rkd*(CR-2*preCR+ppreCR); /舵机的PID算式 ppreCR=preCR;/计算之后向前推进赋值 为下次计算做准备 preCR=CR; /*电机控制*/void motor_ctrl1(void) if(Flag_Chute=0) /普通道 Flag_Pwm=0; mkp=b_mkp0; mki=b_mki0; mkd=b_mkd0; ideal_speed=s_table0; else if(Flag_Chute=1) /长直道 Flag_Pwm=1; / Flag_PIDRev=0; mkp=b_mkp1; mki=b_mki1; mkd=b_mkd1; ideal_speed=s_table1; else if(Flag_Chute=2) /大弯道 if(Flag_Pwm=0) /普通道进大弯道 mkp=b_mkp2; mki=b_mki2; mkd=b_mkd2; ideal_speed=s_table2; else if(Flag_Pwm=1) /直道进大弯道 mkp=b_mkp3; mki=b_mki3; mkd=b_mkd3; ideal_speed=s_table3; else mkp=b_mkp4; mki=b_mki4; mkd=b_mkd4; ideal_speed=s_table4; Flag_Pwm=2; /电机控制 void motor_ctrl2(void) int error,m_perror,m_ierror,m_derror; int pre_error=0; int ppre_error=0; error=ideal_speed-Pulse_count; m_perror = error - pre_error; m_ierror=error; m_derror=error-2*pre_error+ppre_error; ppre_error=pre_error; pre_error=error; speed+=mkp*m_perror + mki*m_ierror + mkd*m_derror; /速度PID控制算式 if(speed=9000) speed=9000; if(speed=0) PWMDTY45=0; PWMDTY01=(int)speed/10; else PWMDTY45=(int)(-speed)/10; PWMDTY01=0; /主函数/void main(void) DisableInterrupts; PWMDTY23=3800; PLL_Init(); DDRA=0X00;/输入 boman(); PACBInit(); RTI_init(); PWM_rudder_init(); PWM_init_motor(); AD_Init(); DDRB=0XFF;/输出 PORTB=0X03;/1号与2号灯亮 delay(5000); /4000 3s左右 PORTB=0xFC;/3号与4号灯亮 Checkstart(); EnableInterrupts; /* put your own code here */ for(;) while(!ATD0STAT2_CCF0); / 等待转换结束while(ATDOSTAT2_CCF0=1) left1=ATD0DR0;/读取结果寄存器left1的值 while(!ATD0STAT2_CCF2); / 等待转换结束while(ATDOSTAT2_CCF1=1) left2=ATD0DR2;/读取结果寄存器的值 while(!ATD0STAT2_CCF1); / 等待转换结束while(ATDOSTAT2_CCF2=1) right1=ATD0DR1;/读取结果寄存器的值 while(!ATD0STAT2_CCF3); / 等待转换结束while(ATDOSTAT2_CCF3=1) right2=ATD0DR3;/读取结果寄存器的值 AR_LEFT=left1+left2; AR_RIGHT=right1+right2; CR=(AR_RIGHT-AR_LEFT)/10; if(Pulse_count65)Flag_gaosu=1; else Flag_gaosu=0; if(AR_RIGHT110|AR_LEFT3800) /if(AR_RIGHT110) f=4500; /f=3100; if(pref3800) /if(AR_LEFT4500)f=4500; if(f3100)f=3100; PWMDTY23=f; pref=PWMDTY23; motor_ctrl2(); /* loop forever */ #pragma CODE_SEG _NEAR_SEG NON_BANKED interrupt 7 void Int_TimerOverFlow(void) Pulse_count= PACNT; /脉冲计数赋值 PACNT = 0X0000; CRGFLG_RTIF=1; if(ting1100)ting+; else ting=1100;interrupt VectorNumber_Vtimch0 void stop(void) DisableInterrupts; TFLG1_C0F=1; /清除中断标志位 /PORTB=0X03;/1号与2号灯亮 / delay(20); / PORTB=0xFC;/3号与4号灯亮 / ganhuang+; /if(ganhuang%4=0) / ganhuang=0; /PORTB=0xFC;/3号与4号灯亮 /ganhuang=0; /TIE=TIE&0X0FE; flag=1; if(ting=1100) flag=2; PORTB=0xF6; for(i=0;i4500)f=4500; if(f3100)f=3100; PWMDTY23=f; pref=PWMDTY23; /mkp=20; /mki=0; /mkd=0; /ideal_speed=0; /motor_ctrl2(); PWMDTY01=0; PWMDTY45=0; delay(2); EnableInterrupts; / asm cli; #pragma CODE_SEG DEFAULT /结论3800中间；3100(-700)左打死，4500(700)右打死 /7.11当右边两个相加小于40时往左打死，此时左车轮大概偏黑线2cm，当左边两个相加小于100时右打死 /此时右轮偏2cm