完善资料让更多小伙伴认识你,还能领取20积分哦, 立即完善>
智能车源代码光电组(有注解) #include #include #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=2*osc*(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/(16*SCI0BDL) //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 * 2*i; //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;
|
|
相关推荐
|
|
这是直立还是四轮车??
|
|
|
|
|
|
真给力!!!!!!!!!!
|
|
|
|
|
|
只有小组成员才能发言,加入小组>>
1856个成员聚集在这个小组
加入小组我的项目我做主,使用GN+Ninja来完成构建系统(VSCode开发RT106X)
36153 浏览 0 评论
NXP IMX8应用处理器快速入门必备:技巧、使用、设计指南
4256 浏览 0 评论
5871 浏览 1 评论
6629 浏览 0 评论
NXP i.MX6UL开发板(linux系统烧录+规格+硬件+模块移植)使用手册
4075 浏览 0 评论
512浏览 2评论
求助,S32G上Core M启动后如何让Core A在Flash指定位置加载uboot?
495浏览 2评论
ESP32-WROVER-IE + LAN8720以太网,GPIO0电压只有1.6v,无法正常进入spi flash boot模式如何解决?
491浏览 2评论
求分享适用于PN7160 Android的NFC工厂测试应用程序
574浏览 2评论
619浏览 2评论
小黑屋| 手机版| Archiver| 电子发烧友 ( 湘ICP备2023018690号 )
GMT+8, 2024-9-11 16:47 , Processed in 1.619200 second(s), Total 88, Slave 69 queries .
Powered by 电子发烧友网
© 2015 bbs.elecfans.com
关注我们的微信
下载发烧友APP
电子发烧友观察
版权所有 © 湖南华秋数字科技有限公司
电子发烧友 (电路图) 湘公网安备 43011202000918 号 电信与信息服务业务经营许可证:合字B2-20210191