[资料]

飞思卡尔智能车光电组基本资料

2015-4-14 08:59:39 8682

0 智能车源代码光电组(有注解) #include /* common definesand macros / #include / derivativeinformation / #include"math.h" #include"PWM.h" #include"ATD.h" #include"LQprintp.h" #pragma LINK_INFODERIVATIVE "mc9s12dg256b" unsigned int i =0; unsigned int j =0; unsigned int t =0; byte ad_value[13]; uchar data[13]; int sum = 0; uchar start_flag =0; uchar num = 0; uchar lw=0; unsigned int per =65530; int SPWM = 0; int L_SPWM = 0; unsigned int SPmax= 1000; int MPWM = 0; ucharcurrent_corrd = 0; static unsignedint mem_num = 0; //********************PID*************************** static unsignedint Kp=25; static unsignedint Kp2=60; static unsignedint Ki=9; static unsignedint Kd=30; static unsignedint rKp=100; static unsignedint rKp2=60; static unsignedint rKi=0; static unsignedint rKd=60; unsigned short E =5; unsigned char q =1; int Mp = 0; int Mi = 0; int Md = 0; int Mp2 = 0; int P_Speed = 0; int L_u[3]; //****************************************************** //*******************舵机PID变量****************** static unsignedint s_sKp=35; //直道PID的P值 static unsignedint s_sKp2=0; //直道PID的二阶P值 static unsignedint s_sKi=0; //直道PID的I值 static unsignedint s_sKd=10; //直道PID的D值 //用于防止PID溢出** unsigned shorts_sE = 5; unsigned char s_sq= 1; //********************* //分别存放P I D 值* int s_sMp =0; int s_sMi = 0; int s_sMd = 0; int s_sMp2 = 0; //********************* int sL_u[3]; //存放前3次理论速度与实际速度的差值 intlast_corrd[3][10] ={{0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0,0, 0, 0}, {0, 0, 0, 0, 0, 0, 0,0, 0, 0}}; //***************************************************** //*****************PID调试中断********************** unsigned char cp =0; unsigned char ci =0; unsigned char cd =0; unsigned intsearch_PACN10; unsigned int np =0; unsigned intsp[500]; //****************************************************** //*****************存储前20点的数据***************** int L_num[10] ={0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; int sum_corrd = 0; int wb = 0; //记录当前状态黑为0，白为1； //****************************************************** //****************红外滤波************************* int corrd[10] ={0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; int T_corrd = 0; int is_white = 0; int numb = 0; //****************************************************** //*******************数据统计*********************** int corrd_sate[23]= {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,0, 0, 0, 0}; int SPWM_sate[15]= {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; int MPWM_sate[11]= {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; ucharcorrd_time[512]; ucharSPWM_time[512]; uchARMPWM_time[512]; ucharPACN_time[512]; //******************************************************** voidsetbusclock(void) { CLKSEL=0X00; //disengagePLL to system PLLCTL_PLLON=1; //turn on PLL SYNR=1; REFDV=1; //pllclock=2osc(1+SYNR)/(1+REFDV)=32MHz; _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 Dly_ms(intms) { int ii,jj; if (ms<1) ms=1; for(ii=0;ii for(jj=0;jj<2670;jj++); //busclk:16MHz--1ms } static voidSCI_Init(void) { SCI0CR2=0x2c; //enable Receive FullInterrupt,RX enable,Tx enable SCI0BDH=0x00; //出口波特率为9600 SCI0BDL=0x68; //SCI0BDL=busclk/(16SCI0BDL) //busclk 8MHz, 9600bps,SCI0BDL=0x34 //busclk 16MHz,9600bps,SCI0BDL=0x68 //busclk 24MHz,9600bps,SCI0BDL=0x9C } //busclk 32MHz,9600bps,SCI0BDL=0xD0 static voidIOC_Init(void) { PERT = 0XFF; //PPST=0XFF; DDRT=0XFe; PBCTL=0X50;//PT0 PIN,PACN10 16BIT,FALLingedge,NOT INTERRUPT //PACN0=200; //PACN1=0xFF; TCTL4=0x01;//40表示ICx禁止, 1表示上升沿, 2表示下降沿, 3表示任何沿 //TIE =0x00;//每一位对应相应通道中断允许,0表示禁止中断 TIOS =0xfe;//每一位对应通道的: 0输入捕捉,1输出比较 } unsigned intget_Speed() { int Speed; Speed = PACN10; PACN10= 0; return Speed; } void s_PID_MPWM() { L_u[2] = L_u[1]; L_u[1] = L_u[0]; L_u[0] = P_Speed - search_PACN10; Mp = Kp L_u[0]; Mi = Mi + Ki * L_u[0]; Md = Kd * (L_u[0] - 2 * L_u[1] + L_u[2]); Mp2 = Kp2 * (L_u[0] - L_u[1]); if(((Ki * L_u[0]) > E)\|\|((Ki * L_u[0])< -E)) q = 0; else q = 1; MPWM = MPWM + Mp + q * Mi + Md + Mp2; if(MPWM > 1000) MPWM = 1000; if(MPWM < -1000) MPWM = -1000; Set_MPWM(MPWM); } void r_PID_MPWM() { L_u[2] = L_u[1]; L_u[1] = L_u[0]; L_u[0] = P_Speed - search_PACN10; Mp = rKp * L_u[0]; Mi = Mi + rKi * L_u[0]; Md = rKd * (L_u[0] - 2 * L_u[1] + L_u[2]); Mp2 = rKp2 * (L_u[0] - L_u[1]); if(((rKi * L_u[0]) > E)\|\|((rKi * L_u[0])< -E)) q = 0; else q = 1; MPWM = MPWM + Mp + q * Mi + Md + Mp2; if(MPWM > 1000) MPWM = 1000; if(MPWM < -1000) MPWM = -1000; Set_MPWM(MPWM); } //*****************舵机PID控制函数********************* void s_PID_SPWM() { sL_u[2] = sL_u[1]; sL_u[1] = sL_u[0]; sL_u[0] = ((current_corrd+last_corrd[0][1]) -(last_corrd[0][2]+last_corrd[0][3]))/2; //*****计算PID值********* if(sL_u[0]<20 && sL_u[0]>-20) { s_sMp = s_sKp * sL_u[0]; s_sMi = s_sMi + s_sKi * sL_u[0]; s_sMd = s_sKd * (sL_u[0] - 2 * sL_u[1] +sL_u[2]); s_sMp2 = s_sKp2 * (sL_u[0] - sL_u[1]); //*************************** //*******I项溢出防止***** if(((s_sKi * sL_u[0]) > s_sE)\|\|((s_sKi* sL_u[0]) < -s_sE)) s_sq = 0; else s_sq = 1; //***************************** SPWM = SPWM + s_sMp + s_sq * s_sMi + s_sMd+ s_sMp2; //*******PWM溢出防止***** if(SPWM > 70) SPWM = 70; if(SPWM < -70) SPWM = -70; //************************* } else sL_u[0] = sL_u[1]; Set_SPWM(SPWM); } //******************************************************** voidshow_SPWM_data() { for(j = 0; j < 15; j++) { printp("%10d", SPWM_sate[j]); } } voidshow_MPWM_data() { for(j = 0; j < 11; j++) { printp("%10d", MPWM_sate[j]); } } voidshow_corrd_data() { for(j = 0; j < 23; j++) { printp("%6d", corrd_sate[j]); } } voidshow_corrd_time() { for(j = 0; j < 512; j++) { printp("%c", corrd_time[j]); } } voidshow_SPWM_time() { for(j = 0; j < 512; j++) { printp("%c", SPWM_time[j]); } } voidshow_MPWM_time() { for(j = 0; j < 512; j++) { printp("%c", MPWM_time[j]); } } voidshow_PACN_time() { for(j = 0; j < 512; j++) { printp("%c", PACN_time[j]); } } void main(void) { /* put your own code here / setbusclock(); SCI_Init(); AD_Init(); IOC_Init(); Ini_PWM(); EnableInterrupts; for(;;) { Dly_ms(10); //**********红外滤波*********************** for(j=0; j<10; j++) { adc_get(ad_value); for(i = 1; i<12; i++) { if(ad_value>=160) { data = 1; num = num + 1; } else data = 0; sum = sum + data 2i; //printp("%10d",data); } if(num == 0) { corrd[j] = -1; } else { corrd[j] = sum/num; } sum = 0; num = 0; } is_white = 0; for(i=0;i<10;i++) { if(corrd < 0) { is_white++; } for(j=i;j<10;j++) { T_corrd = corrd[j]; corrd[j] = corrd; corrd = T_corrd; } } if(is_white > 6) { numb = 0; } sum = 0; j = 0; for(i=3; i<8; i++) { if(corrd > -1) { sum += corrd; j++; } } if(j!=0) { current_corrd = sum/j; numb = j; } L_num[9] = L_num[8]; L_num[8] = L_num[7]; L_num[7] = L_num[6]; L_num[6] = L_num[5]; L_num[5] = L_num[4]; L_num[4] = L_num[3]; L_num[3] = L_num[2]; L_num[2] = L_num[1]; L_num[1] = L_num[0]; L_num[0] = numb; //**************************************************** wb =L_num[1]\|L_num[2]\|L_num[3]\|L_num[4]\|L_num[5]\|L_num[6]\|L_num[7]\|L_num[8]\|L_num[9]; if(numb == 0) { DDRB = 0x80; if(search_PACN10 > 10 &&wb>0) { P_Speed = 20; if(lw == 0) { SPWM = -70; } else { SPWM = 72; } //if(sum_corrd<220&&sum_corrd>-220) Set_MPWM(0); // else // Set_MPWM(700); } else { P_Speed = 20; if(lw == 0) { SPWM = -70; } else { SPWM = 72; } Set_MPWM(1000); } //Set_MPWM(0); } else { DDRB = 0x00; // printp("%d ", current_corrd); // if(PTJ & 0x80) Set_MPWM(0); //else if(wb == 0) { Set_MPWM(1000); } else { switch(current_corrd) { case 2: SPWM = -70;P_Speed = 32;r_PID_MPWM();lw =0;break; case 3: SPWM = -60;P_Speed = 32;r_PID_MPWM();lw =0;break; case 4: SPWM = -50;P_Speed = 32;r_PID_MPWM();lw =0;break; case 5: SPWM = -40;P_Speed = 32;r_PID_MPWM();lw =0;break; case 6: SPWM = -30;P_Speed = 32;r_PID_MPWM();lw =0;break; case 7: SPWM = -25;P_Speed = 32;r_PID_MPWM();lw =0;break; case 8: SPWM = -20;P_Speed = 32;r_PID_MPWM();lw =0;break; case 9: SPWM = -15;P_Speed = 32;s_PID_MPWM();lw =0;break; case 10: SPWM = -10; P_Speed =32;s_PID_MPWM();lw = 0;break; case 11: SPWM = -5; P_Speed =32;s_PID_MPWM();lw = 0;break; case 12: SPWM = 0; P_Speed = 32;s_PID_MPWM();break; case 13: SPWM = 5; P_Speed = 32;s_PID_MPWM();lw = 1;break; case 14: SPWM = 10; P_Speed = 32;s_PID_MPWM();lw = 1;break; case 15: SPWM = 15; P_Speed =32;s_PID_MPWM();lw = 1;break; case 16: SPWM = 20; P_Speed =32;r_PID_MPWM();lw = 1;break; case 17: SPWM = 25; P_Speed =32;r_PID_MPWM();lw = 1;break; case 18: SPWM = 30; P_Speed =32;r_PID_MPWM();lw = 1;break; case 19: SPWM = 40; P_Speed =32;r_PID_MPWM();lw = 1;break; case 20: SPWM = 50; P_Speed =32;r_PID_MPWM();lw = 1;break; case 21: SPWM = 60; P_Speed =32;r_PID_MPWM();lw = 1;break; case 22: SPWM = 70; P_Speed =32;r_PID_MPWM();lw = 1;break; default: break; } //Set_MPWM(500); } } last_corrd[0][3] = last_corrd[0][2]; last_corrd[0][2] = last_corrd[0][1]; last_corrd[0][1] = last_corrd[0][0]; last_corrd[0][0] = current_corrd; corrd_sate[current_corrd]++; SPWM_sate[SPWM/10+7]++; MPWM_sate[MPWM/10]++; corrd_time[t]=current_corrd; SPWM_time[t]=SPWM+70; MPWM_time[t]=(MPWM+1000)/10; s_PID_SPWM(); //Set_SPWM(SPWM); //Set_MPWM(500); search_PACN10 = PACN10; //printp("%10d", search_PACN10); PACN_time[t]=search_PACN10; PACN10 = 0; sum_corrd = 0; sum = 0; start_flag = 0; num = 0; 6
举报淘帖0 只看该作者相关推荐 • 学做飞思卡尔智能车 4732 • 【秀秀资源】单片机，飞思卡尔智能车电磁组 2731 • 飞思卡尔智能车控制PWM的引脚在哪 4906 • 【AWorks280试用申请】飞思卡尔智能车 2147 • 谁有飞思卡尔光电组信号采集有关资料 1607 • 求助！！！！飞思卡尔智能车比赛指导 2768 • 飞思卡尔智能车赛道曲率计算 6857 • 飞思卡尔智能车大赛各个高校所设计的方案 4984 • 飞思卡尔HC12资料 3368 • 飞思卡尔智能车比赛基础---XS128资料 14773 7条评论发表评论只看该作者