寻迹机器人源码及PCB图全套资料-面包板社区

//***************************FileName:Robbot.c***********************////***************************ICCAVR6.30编译**************************//#include io8535v.h>#include macros.h>//****************************宏定义********************************//#define Aa 0.5 //PID参数#define Ba -0.5 //PID参数#define Ca 0 //PID参数#define Ab 0.09 //PID参数#define Bb -0.1 //PID参数#define Cb 0 //PID参数#define U1 12 //PID参数#define MaxSpeed 0x40 //最大速度#define MidSpeed 0x18 //中速,用于转弯#define Size 6 //任务数组大小//*****************************全局变量定义**************************//char sflag=0x00; //记录上一次校偏状态char crossflag = 0; //过线标志，用于判断是否过线char forflag=0; //记录上一次机器人行进状态char forlight; //记录上一次A口光电传感器的状态float EkA; //本次左边电机速度误差float EkA_1=0; //上次左边电机速度误差float EkA_2=0; //上上次左边电机速度误差float EkB; //本次左边电机速度误差float EkB_1=0; //上次左边电机速度误差float EkB_2=0; //上上次左边电机速度误差char flage=0;char a=0; //溢出次数，控制PID窗口时间char c=0; //控制寻线频率int desireV=10;char b=0;char time=0; //机器人行走步数char fob=0; //=1后退，=0前进char Task[Size]={0x37,0x27,0x1B,0x29,0x23,0x1B};//任务数据数组//******************************定时器1初始化*************************//void Timer1Init(int temptimsk,int temptccrA,int temptccrB){unsigned char sreg;TIMSK = temptimsk; sreg = SREG; //保存全局中断标志 _CLI(); //屏蔽所有中断 TCCR1A = temptccrA; TCCR1B = temptccrB; SREG = sreg; //恢复全局中断标志}//******************************写OCR1A寄存器**************************//void SetOutputComReg1A(int tempocr){unsigned char sreg;sreg = SREG; _CLI(); OCR1A = tempocr; SREG = sreg;}//******************************写OCR1B寄存器**************************//void SetOutputComReg1B(int tempocr){unsigned char sreg;sreg = SREG;_CLI();OCR1B = tempocr;SREG = sreg;}//******************************读OCR1A寄存器**************************//int GetOutputComReg1A(){int temp;temp = OCR1A;return(temp);}//******************************读OCR1B寄存器**************************//int GetOutputComReg1B(){int temp;temp = OCR1B;return(temp);}//******************************长延时函数***************************//void DELAY(int delaytime){int i,j;for(i=0;i=delaytime;i++) {for (j=0;j=0xFFFE;j++) ;} }//******************************短延时函数***************************//void delay(int i){int j;for(j=0;j=i;j++) ;}//******************************PID调节:左电机***********************//void PIDA() {char y; //本次采样速度值float u; //电压差值int z; //本次输出增量int temp1; //临时记录值y = TCNT1;EkA=y-desireV;if (EkA==(0-desireV)) //当Ek大于某一值时直接加最大 {temp1=0x0000; SetOutputComReg1A(temp1);}else {EkA=Aa*EkA; EkA_1=Ba*EkA_1; EkA_2=Ca*EkA_2; u=EkA+EkA_1+EkA_2; z=u/U1*0x03FF; temp1=GetOutputComReg1A(); temp1=temp1+z; SetOutputComReg1A(temp1); EkA_2=EkA_1; EkA_1=EkA;}TCNT1=0x00;}//******************************PID调节:右电机***********************//void PIDB(){char y;float u; //电压差值int z;int temp1;y = TCNT2;EkB=y-desireV;if(EkB==(0-desireV)) //当Ek大于某一值时直接加最大// {temp1=0x0000; SetOutputComReg1B(temp1);}else {EkB =Ab*EkB; EkB_1=Bb*EkB_1; EkB_2=Cb*EkB_2; u=EkB+EkB_1+EkB_2; z=u/U1*0x03FF; temp1=GetOutputComReg1B(); temp1=temp1+z; SetOutputComReg1B(temp1); EkB_2=EkB_1; EkB_1=EkB;}TCNT2=0x00;}//***************************寻线处理函数1***************************//char Talone() {if ((forlight0x09)==0) //3单独亮 return(0);else return(1); //返回为校偏状态值 }//***************************寻线处理函数2***************************//char FUalone() //4单独亮{if ((forlight0x05)==0) return(0);else return(2);}//***************************寻线处理函数3***************************//char FIalone() //5单独亮{if ((forlight0x22)==0) return(0);else return(1);}//***************************寻线处理函数4***************************//char Salone() //6单独亮{if ((forlight0x12)==0) return(0);else return(2);}//***************************寻线处理函数5****************************//char InLine1(void) //线上偏离判断{char discrepancy; //行进状态char l2; //传感器状态l2=0xFF^PINA;l2=0x3Cl2;l2=l2>>2;switch(l2) {case 0x00: discrepancy=0; break; //0000b未偏 case 0x01: discrepancy=0; break; case 0x02: discrepancy=0; break; case 0x04: discrepancy=2; break; //0010b左偏 case 0x05: discrepancy=2; break; case 0x06: discrepancy=2; break; case 0x08: discrepancy=1; break; //0001b右偏 case 0x09: discrepancy=1; break; case 0x0A: discrepancy=1; break;}return(discrepancy); }//***************************寻线处理函数6****************************//char InLine2(void) //线上偏离判断{char discrepancy;char l2;l2=0xFF^PINA;l2=0x3Cl2;//3,4,5,6灯l2=l2>>2;switch(l2) {case 0x00: discrepancy=0; break; //0000b//未偏 case 0x01: discrepancy=1; break; //0001b右偏 case 0x02: discrepancy=2; break; //0010b左偏 case 0x04: discrepancy=0; break; case 0x05: discrepancy=1; break; case 0x06: discrepancy=2; break; case 0x08: discrepancy=0; break; case 0x09: discrepancy=1; break; case 0x0A: discrepancy=2; break;}return(discrepancy); }//***************************寻线处理函数7****************************//char OutLine(void) //线外偏离判断{char discrepancy;char l2;l2=PINA^0xFF; l2=0x3Cl2; //00111100bl2=l2>>2;switch(l2) {case 0x00: discrepancy=forflag; break; //0000b case 0x01: discrepancy=Talone(); break; //0001b case 0x02: discrepancy=FUalone(); break; //0010b case 0x04: discrepancy=FIalone(); break; //0100b case 0x05: discrepancy=0; break; //0101b case 0x06: discrepancy=0; break; //0110b case 0x08: discrepancy=Salone(); break; //1000b case 0x09: discrepancy=0; break; //1001b case 0x0A: discrepancy=0; break; //1010b case 0x0F: discrepancy=0; break; //1111b } return(discrepancy);} //***************************校偏函数********************************//void Revise(char discrepancy){if(discrepancy==0) {forflag=0; if(fob==1){PORTB=0x00; return;} PORTB=0x0C; return;}if(discrepancy==1) {PORTB=0x08; //A为左边电机 delay(0x0100); PORTB=0x0C; forflag=1; return;} if(discrepancy==2) //B为右边电机 {forflag=2; PORTB=0x04; delay(0x0100); PORTB=0x0C; return;} if(discrepancy==3) {PORTB=0x08; //A为左边电机 forflag=1; return;} if(discrepancy==4) {forflag=2; PORTB=0x04; return;}}//***************************寻线函数********************************//void SearchLine() {char online; //测点状态寄存器char l78; //7，8号测点状态寄存器char y=0;online=PINA^0xFF;forlight=online;l78=online0xC0; l78=l78>>6;if(l78==0x03) //7，8同时亮无偏 Revise(0);if((online0x0D)==0x0D) //1，3，4都亮，直走 {Revise(0); //校偏函数 y=1;} if(y==0) {if(l78==0x01) Revise(3); //3为大右偏 if(l78==0x02) Revise(4); //4为大左偏 if(l78==0x00) {online=0x03online; if(online==0x01) //只有1号灯亮 Revise(InLine1()); //线上走偏 if(online==0x02) //只有2号灯亮 Revise(InLine2()); //线上走偏 if (online==0x00) //1，2都不亮 Revise(OutLine());}}//线外走偏}//***************************停止函数********************************//void stop(){PORTD=PORTD|0xCF;}//***************************转向函数********************************//char action(char direction) // =1为右转，==0为左转，=2为停止{char light;char i=0;if(direction==2) {desireV = 0x00; //将设定速度置为0SetOutputComReg1A(0x03FF); SetOutputComReg1B(0x03FF);}else {if (direction==1) {PORTB = 0x08; //右转 do{ light = PINA^0xFF; if ((light0x44)!=0x00) //当检测到3号灯亮转向完成 break;}while(1);} if (direction==0) {PORTB = 0x04; //左转 DELAY(6); do{ light = PINA^0xFF; if ((light0x88)!=0x00) //4号灯亮转向完成 break;}while(1);} DELAY(2); //转向退出延时，防止错误读值 return(1);}return(0);} //*******************************前进函数****************************//void Forward(char step,char direction,char fob){char flagi=0; //单边过线标志，整即过线后清0char flagj; //过线辅助判断标志char middle1=0x00; //过线检测状态寄存器char middle2=0x00; //前次过线检测状态寄存器char postionflag=0; //位置标志desireV = MaxSpeed;if (fob==1) //1为后退PORTB=0x00;if (fob==0) //0为前进PORTB=0x0C;do{ flagj = 0;middle1 = PINB^0xFF; //读取PB口，并取反 middle1 = middle10xC0; //提取PB6,PB7 middle1 = middle1>>6; if((middle1==0x03)(middle2!=0x03)) //两边同时亮且前一次不是同时亮则为过线 {postionflag++; //位置标志加1 if(postionflag==(step-1)) desireV=MidSpeed; //将设定速度降低,便于转弯 if(postionflag==step) {if(action(direction)==1) break;} DELAY(30); //延时确保不重复检测 flagi = 0;} if(flagi!=0) //两边未同时过线 {if(flagi==1) //左边先到 {if((middle1==0x02)((middle20x02)==0x00)) //检测右边上升沿 {postionflag++; if(postionflag==(step-1)) desireV=MidSpeed; if(postionflag==step) {if(action(direction)==1) break;} DELAY(30); flagi = 0; flagj = 1;}} if(flagi==2) //右边先到 {if((middle1==0x01)((middle20x01)==0x00)) //检测左边上升沿 {postionflag++; if(postionflag==(step-1)) desireV=MidSpeed; if(postionflag==step) {if(action(direction)==1) break;} DELAY(30); flagi = 0; flagj = 1;}}} if(flagj==0) {if((middle1==0x01)((middle20x01)==0x00)) //左边到线，右边未到线 flagi=1; if((middle1==0x02)((middle20x02)==0x00)) //右边到线，左边未到线 flagi=2;}}while(1);return;}//***************************主函数**********************************//void main(){char movesteps; //传递前进或后退步数 char movedirection; //运动方向 DDRA=0x00; //PA口输入 DDRB=0xFF; //PB口输出 DDRC=0xFF; //PC口输出 DDRD=0xFF; //PD口输出 PORTA=0xFF; PORTB=0x08; //PB2=0，PB3=1// PORTC=0xFF; PORTD=0x88; SetOutputComReg1A(0x03FF); SetOutputComReg1B(0x03FF); Timer1Init( 0x82,0xF3,0x01); TCNT0=0x00; TCCR0=0x07; //T0上升沿驱动 ASSR=0x08; //外部TOSC1触发 TCCR2=0x01; SEI(); do{ if ((Task[time]0x01)==1) {if((Task[time]0x02)==0x02) //判断指定运动方向，1为前进 {movesteps=(Task[time]0xF0)>>4; //取高四位，运动格数 movedirection=(Task[time]0x0C)>>2; //提取低四位中的高两位，转向 Forward(movesteps,movedirection,0);} if((Task[time]0x02)==0x00) //判断指定运动方向，0为后退 {movesteps=(Task[time]0xF0)>>4; //取高四位，运动格数 movedirection=(Task[time]0x0C)>>2; //提取低四位中的高两位，转向 Forward( movesteps,movedirection,1 );} } time++; if(time==Size) break;}while(1);}//***************************中断服务程序****************************//#pragma interrupt_handler TIM1_OVF:6void TIM1_OVF(void) //计时器时间到,进入中断{char light;CLI();a++;c++;if(a==40) //速度采样时间到 {a=0; PIDA(); PIDB();}if(c=100) //寻线采样时间到 {c=0; SearchLine();} SEI();}……