飞思卡尔刹车过弯效果程序

飞思卡尔刹车过弯效果程序#include <hidef.h> /* common defines and macros */ #include <MC9S12XS128.h> /* derivative information */#pragma LINK_INFO DERIVATIVE "mc9s12xs128"///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////*------------------sen_AD.h -----------------------------*/////////////////////////////////////////////////////////////#define MIDDLE 9083 //10100 //舵机中间值#define Mid_Revise 0 //舵机中间值校正#define Right_Edge -1140 //舵机右极限#define Left_Edge 1140 //舵机左极限#define SPEED_KP 12 //定义比例系数#define SPEED_KI 3 //定义积分系数#define SPEED_KD 2 //定义微分系数#define VV_DEADLINE 0x01 //速度PID，设置死区范围//#define VV_BANGBANG 20 //速度PID，BANGBANG控制#define V_BANGBANG -13 //速度PID，BANGBANG控制word VV_MAX;word V_MAX=99;word VV_MIN=0; //最小速度byte Num;//byte blank_flag;const byte speed[10][9]={{60,60,60,60,60,60,60,60,60}, /*0*/{42,42,39,37,37,37,37,37,35}, /*1*/{44,44,39,37,37,37,37,37,35}, /*2*/{45,45,45,45,40,37,35,35,35}, /*3*/{50,50,50,47,45,40,35,30,25}, /*4*/ //speed select mode{55,55,50,45,40,35,35,30,25}, /*5*/{60,60,55,57,45,42,35,34,20}, /*6*/{65,65,60,55,40,37,35,34,25}, /*7*/{70,70,65,60,55,45,38,34,25}, /*8*/{75,75,70,65,58,50,45,35,25}, /*9*/};///////////////////////////////////////////////////////////*------------------RUDDER and SENSOR-------------------*////////////////////////////////////////////////////////////*----------------传感器记忆位量共同体-------------------*/union{word Word;struct{word BIT0 :1; /*0*/word BIT1 :1; /*1*/word BIT2 :1; /*2*/word BIT3 :1; /*3*/word BIT4 :1; /*4*/word BIT5 :1; /*5*/word BIT6 :1; /*6*/word BIT7 :1; /*7*/word BIT8 :1; /*8*///word BIT9 :1; /*9*///word BIT10 :1; /*10*/}Bits;}Sensor;#define Psensor Sensor.Word#define N 9#define Count 9/////////////加权平均算法得出智能车偏离程度////////////#define ANp 10 //初始比例参数#define ANd 17 //微分参数//#define ANi 0#define Al 6byte SetPulse;static char Sen_Weight[N]={8,6,4,2,0,-2,-4,-6,-8};//0 1 2 3 4 5 6 7 8static word ad_adjust[N]={680,690,690,670,688,690,652,675,650};//模型车参数word ad[9]; //AD转换存储值struct RUD_PID //RUDDER 参数结构体{word Ap;byte Ad;//byte Ai;byte Alpha;char angerror; //转角偏差量char angderror; //相邻两次转角偏差求差char angprerror; //前一次转角偏差量char Ap_error; //当前转角偏差赋值判断量//int Ki_out; //积分输出量int Kd_out; //微分输出量int Kp_out; //比例输出量int servout; //转角输出量}RUD_PID;char Sen_W; //传感器赛道权重char Sen_Weighting; //黑线中心char Sen_FlagCount; //传感器标志位求和总量////////////////////////////////////////////////////*-------------------MOTOR-----------------------*////////////////////////////////////////////////////word Pulse; //脉冲累加/*-------------电机参数结构体-------------------*//*------------------------------------------------*/struct PID //定义结构体{int vi_Ref; //速度PID，速度设定值int vi_FeedBack; //速度PID，速度反馈值int vi_PreError; //速度PID，速度误差,,vi_Ref - vi_FeedBackint vi_PreDerror; //速度PID，前一次，速度误差之差，d_error-PreDerror; int v_Kp; //比例系数，Kp = Kpint v_Ki; //积分系数，Ki = Kp * ( T / Ti )int v_Kd; //微分系数，Kd = KP * Td * Tint vl_PreU; //PID输出值}PID;/*--------------拨码开关变量共同体声明--------------*/union{ //拨码开关速度选择byte Byte;struct{byte BIT0 :1; /*0*/byte BIT1 :1; /*1*/byte BIT2 :1; /*2*/byte BIT3 :1; /*3*/byte BIT4 :1; /*4*/byte BIT5 :1; /*5*/byte BIT6 :1; /*6*/byte BIT7 :1; /*7*/}Bits;}In_speed;/////////////////////////////////////////////////////////////////////////// /*-----------------------------------------------------------------------*/////////////////////////////////////////////////////////////////////////// static void SetBusCLK_24M(void){CLKSEL=0X00; // disengage PLL to systemPLLCTL_PLLON=1; // turn on PLLSYNR=0x00 | 0x02; // VCOFRQ[7:6];SYNDIV[5:0]// fVCO= 2*fOSC*(SYNDIV + 1)/(REFDIV + 1)// fPLL= fVCO/(2 ×POSTDIV)// fBUS= fPLL/2// VCOCLK Frequency Ranges VCOFRQ[7:6]// 32MHz <= fVCO <= 48MHz 00// 48MHz < fVCO <= 80MHz 01// Reserved 10// 80MHz < fVCO <= 120MHz 11REFDV=0x80 | 0x01; // REFFRQ[7:6];REFDIV[5:0]// fREF=fOSC/(REFDIV + 1)// REFCLK Frequency Ranges REFFRQ[7:6]// 1MHz <= fREF <= 2MHz 00// 2MHz < fREF <= 6MHz 01// 6MHz < fREF <= 12MHz 10// fREF > 12MHz 11// pllclock=2*osc*(1+SYNR)/(1+REFDV)=32MHz;POSTDIV=0x00; // 4:0, fPLL= fVCO/(2xPOSTDIV)// If POSTDIV = $00 then fPLL is identical to fVCO (divide by one)._asm(nop); // BUS CLOCK=16M_asm(nop);while(!(CRGFLG_LOCK==1)); //when pll is steady ,then use it;CLKSEL_PLLSEL =1; //engage PLL to system}//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////void port_init(void){//DDRA|=0x00;//DDRT_DDRT0=0; //PT0口作为速度调试输入DDRB=0xf1;PORTB=0xff;}//////////////////////////////////////////////////////////////////////////////////AD采样初始化//////////////////////////////////////////////////////////////////////////////////////void AD0_Init(void){ATD0CTL1=0x20; //7:1-外部触发,65:00-8位精度,01-10位精度,10-12位精度,11-保留,AD转换最大数值为1024；4:放电,3210:chATD0CTL2=0x40; //禁止外部触发, 中断禁止//ATD0CTL2=0x42; //禁止外部触发, while(!ATDSTAT0_SCF)//中断使能，启动AD转换ATD0CTL3=0x80; //右对齐无符号,每次转换4个序列, No FIFO, Freeze模式下继续转ATD0CTL4=0x03; //765:采样时间4个AD时钟周期,每10us转换一次//ATDClock=[BusClock*0.5]/[PRS+1]=3MHzATD0CTL5=0x30; //6:0特殊通道禁止,5:1连续转换,4:1多通道轮流采样//ATD0CTL5_MULT=1;ATD0DIEN=0x00; //禁止数字输入}/////////////////////////////////////////////////////////*----------------------RUDDER ini---------------------*/////////////////////////////////////////////////////////void PWM01_Init(void) //舵机输出端口PWM7设置{PWME_PWME1 = 0; //PWM1输出关闭PWMCTL_CON01=1; //0/1级联PWMPRCLK = 0; //选择SCLAPWMSCLA = 2; //SCLB时钟24M/(2*PWMSCLB)PWMCLK_PCLK1 = 1; //PTP1选择ClockSA=6MhzPWMPOL_PPOL1=1; //极性选择:高电平输出PWMCNT01 = 0; //PWMDTY01 = MIDDLE; //9083;占空比=[(PEMDTY01+1)/(PEMPER01+1)]*100%=16.2%PWMPER01 = 56000; //舵机输出频率=[6M/(PWMPER01+1)]=107HzPWME_PWME1 = 1; //PWME1引脚PTP1为输出}////////////////////////////////////////*-------电机输入端口选择PWM3/7-----*///////////////////////////////////////void PWM23_Init(void){PWME_PWME3 = 0; //关闭PWM输出通道3PWMCTL_CON23=1;PWMPRCLK = 0; //选择CKLB//fre=busclkPWMSCLB = 12; //busclk/(2*pwmsclB)=1Mhz//busclk/(2*pwmsclB)=2Mhz PWMCLK_PCLK3=1;PWMPOL_PPOL3=1; //极性为高电平输出PWMCNT23 = 0;PWMDTY23 = 0; //输出占空比=[(PWMDTY23+1)/(PWMPER23+1)]*100%=10.7% PWMPER23 = 100; //outfre=24M/(2400+1)=10KHzPWME_PWME3 = 1; //PWM3输出}void PWM67_Init(void){PWME_PWME7 = 0;PWMCTL_CON67=1;PWMPRCLK = 0; //选择CKLB//fre=busclk=24MHzPWMSCLB = 12; //busclk/(2*pwmsclB)=2MhzPWMCLK_PCLK7=1;PWMPOL_PPOL7=1;PWMCNT67 = 0;PWMDTY67 = 0;PWMPER67 = 100;PWME_PWME7 = 1; //打开PWM输出通道7}/*--------------------------------------------------------////////////////PIT模数递减初始化///////////////////////---------------------------------------------------------*/void initPIT(void)//定时中断初始化函数50MS定时中断设置{PITCFLMT_PITE=0; //定时中断通道0关PITCE_PCE0=1; //定时器通道0使能PITMTLD0=240-1; //8位定时器初值设定。