飞思卡尔电磁组程序

//控制策略car.cpp:DefinestheinitializationroutinesfortheDLL.//#includemath.h#includestdafx.h#include控制策略car.h//#includeJudgement.H#ifdef_DEBUG#definenewDEBUG_NEW#undefTHIS_FILEstaticcharTHIS_FILE[]=__FILE__;#endifexternC_declspec(dllexport)voidJudge(unsignedshortSpeed,unsignedchar*SensorData,unsignedshortSensorCount,unsignedchar*CCDData,intCCDWidth,intCCDHeight,unsignedshort*MotorPWM,unsignedshort*SteerPWM);externC_declspec(dllexport)voidReset();voidInit_ECT(void);voidInit_PWMout(void);voidInit_SCI(void);voidTransmit_Sci(unsignedint);unsignedcharReceive_Sci(void);voidSignalProcess();voidPWMout(int,int,int);voidMemoryroad(void);voidRoadplay(void);voidPIDinit(void);voiddelay(intcnt);voidSteerContral(void);voidCircles(void);intRunroad_time=0/*第一圈时间*/,Roadtime,Runroad_time2=0/*第二圈时间*/;intInbentcount=0,Outbentcount=0;//弯道数目intSpeedout,Steerout,Speedfout,allspeed;intAD,cnt,flag_delay=0;intf0=0,f1=0,f2=0,f3=0,f4=0,f5=0,f6=0,f7=0,f8=0,f9=0,f10=0;unsignedcharch_0,ch_1,ch_2,ch_3,ch_4,ch_5,ch_6,ch_7,ch_8,ch_9,ch_10,ch_11,ch_12;staticintoldDirection,Direction,Velocity,VelocityFz;//方向速度变量16位intflag0,flag1,flag2,flag3,flag4,flag5,flag6,flag7,flag8,flag9,flag10,Flagstart;unsignedcharJump=0,Jump0=0,Jump1=0,Jump2=0,Jump3=0,Jump4=0,Jump5=0,Jump6=0,Jump7=0,Jump8=0,Jump9=0;unsignedcharPulseCnt,PORTB;unsignedintsudu,Turn=11,bjqushu,oldbjqushu,qushu;unsignedintjstime,SYSCLOCK=0,sctime=0;intStartline,Circle=0,line=0,line0=0,line2=0,line22=0,line3=0,line33=0,aa0=0,aa1=0,aa2=0;intZhuanjiao[]={-250,-235,-215,-195,-175,-150,-120,-90,-60,-30,0,30,60,90,120,150,175,195,215,235,250};intHiShuruSudu[]={15,20,20,20,22,22,25,25,30,30,40,30,30,25,25,22,22,20,20,20,15};intShuruSudu[]={15,20,20,20,22,22,25,25,25,30,30,30,25,25,25,22,22,20,20,20,15};intss=0,insstime[50],outsstime[50],oldsstime=0,ssflag=0,oldssflag=0,sstime_flag2=0,sstime_flag1=0;intoldss;/***************************PID*************************///速度PIDtypedefstruct{intNextPoint;intThisPoint;//设定目标DesiredvalueintKp;//比例常数ProportionalConstintKi;//积分常数IntegralConstintKd;//微分常数DerivativeConstintLastError;//Error[-1]intPrevError;//Error[-2]intSumError;//SumsofErrors}speedPID;speedPIDsPID={0,0,1500,600,60,0,0,0};/**pp=&sPID;*///角度PIDtypedefstruct{intNextPoint;intThisPoint;//设定目标DesiredvalueintKp;//比例常数ProportionalConstintKi;//积分常数IntegralConstintKd;//微分常数DerivativeConstintLastError;//Error[-1]intPrevError;//Error[-2]intSumError;//SumsofErrors}steerPID;steerPIDDjPID={0,0,1,0,0,0,0,0};/*,*pp=&sPID;*/intspeedPIDCalc(speedPID*pp);//速度、舵机PID初始化:voidPIDinit(void)/*sPID.ThisPoint=0;//设定目标DesiredvaluesPID.Kp=1500;//比例常数ProportionalConstsPID.Ki=600;//积分常数IntegralConstsPID.Kd=60;//微分常数DerivativeConstsPID.LastError=0;//Error[-1]sPID.PrevError=0;//Error[-2]sPID.SumError=0;//SumsofErrorsDjPID.ThisPoint=0;//设定目标DesiredvalueDjPID.Kp=1;//比例常数ProportionalConstDjPID.Ki=0;//积分常数IntegralConstDjPID.Kd=0;//微分常数DerivativeConstDjPID.LastError=0;//Error[-1]DjPID.PrevError=0;//Error[-2]DjPID.SumError=0;//SumsofErrors/*************************PID初始化**********************/unsignedshortPWM;//Flagstart=0;externC_declspec(dllexport)voidJudge(unsignedshortSpeed,unsignedchar*SensorData,unsignedshortSensorCount,unsignedchar*CCDData,intCCDWidth,intCCDHeight,unsignedshort*MotorPWM,unsignedshort*SteerPWM){SensorCount=11;sudu=Speed;ch_0=SensorData[0];ch_1=SensorData[1];ch_2=SensorData[2];ch_3=SensorData[3];ch_4=SensorData[4];ch_5=SensorData[5];ch_6=SensorData[6];ch_7=SensorData[7];ch_8=SensorData[8];ch_9=SensorData[9];ch_10=SensorData[10];//ch_11=SensorData[11];//ch_12=SensorData[12];if(ch_0255){flag0=1;f0=1;}if(ch_0=255){flag0=0;f0=0;}if(ch_1255){flag1=300;f1=1;}if(ch_1=255){flag1=0;f1=0;}if(ch_2255){flag2=5;f2=1;}if(ch_2=255){flag2=0;f2=0;}if(ch_3255){flag3=700;f3=1;}if(ch_3=255){flag3=0;f3=0;}if(ch_4255){flag4=9;f4=1;}if(ch_4=255){flag4=0;f4=0;}if(ch_5255){flag5=1100;f5=1;}if(ch_5=255){flag5=0;f5=0;}if(ch_6255){flag6=13;f6=1;}if(ch_6=255){flag6=0;f6=0;}if(ch_7255){flag7=2550;f7=1;}if(ch_7=255){flag7=0;f7=0;}if(ch_8255){flag8=17;f8=1;}if(ch_8=255){flag8=0;f8=0;}if(ch_9255){flag9=1900;f9=1;}if(ch_9=255){flag9=0;f9=0;}if(ch_10255){flag10=21;f10=1;}if(ch_10=255){flag10=0;f10=0;}AD=flag0+flag1+flag2+flag3+flag4+flag5+flag6+flag7+flag8+flag9+flag10;switch(AD){case1:Turn=0;//130130030557057007099110911001113131513150015171719171900192121break;case301:Turn=1;break;case300:Turn=2;break;case305:Turn=3;break;case5:Turn=4;break;case705:Turn=5;break;case700:Turn=6;break;case709:Turn=7;break;case9:Turn=8;break;case1109:Turn=9;break;case1100:Turn=10;break;case1113:Turn=11;break;case13:Turn=12;break;case1513:Turn=13;break;case1500:Turn=14;break;case1517:Turn=15;break;case17:Turn=16;break;case1917:Turn=17;break;case1900:Turn=18;break;case1921:Turn=19;break;case21:Turn=20;break;}/*if(flag_delay0){delay(10);flag_delay=0;}*/Circles();//*SteerPWM=65535*(20-Turn)/20;SteerContral();*SteerPWM=Steerout;if(Circle=5){*MotorPWM=32767;}else{*MotorPWM=54000-1000*abs(Turn-10);}}/*************************************************