|
点击下载：寻迹机器人源码及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 ((forlight&0x09)==0) //3单独亮
return(0);
else
return(1); //返回为校偏状态值
}
//***************************寻线处理函数2***************************//
char FUalone() //4单独亮
{if ((forlight&0x05)==0)
return(0);
else
return(2);
}
//***************************寻线处理函数3***************************//
char FIalone() //5单独亮
{if ((forlight&0x22)==0)
return(0);
else
return(1);
}
//***************************寻线处理函数4***************************//
char Salone() //6单独亮
{if ((forlight&0x12)==0)
return(0);
else
return(2);
}
//***************************寻线处理函数5****************************//
char InLine1(void) //线上偏离判断
{char discrepancy; //行进状态
char l2; //传感器状态
l2=0xFF^PINA;
l2=0x3C&l2;
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=0x3C&l2;//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=0x3C&l2; //00111100b
l2=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=online&0xC0;
l78=l78>>6;
if(l78==0x03) //7，8同时亮无偏
Revise(0);
if((online&0x0D)==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=0x03&online;
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; //将设定速度置为0
SetOutputComReg1A(0x03FF);
SetOutputComReg1B(0x03FF);}
else
{if (direction==1)
{PORTB = 0x08; //右转
do{
light = PINA^0xFF;
if ((light&0x44)!=0x00) //当检测到3号灯亮转向完成
break;}while(1);}
if (direction==0)
{PORTB = 0x04; //左转
DELAY(6);
do{
light = PINA^0xFF;
if ((light&0x88)!=0x00) //4号灯亮转向完成
break;}while(1);}
DELAY(2); //转向退出延时，防止错误读值
return(1);}
return(0);
}
//*******************************前进函数****************************//
void Forward(char step,char direction,char fob)
{char flagi=0; //单边过线标志，整即过线后清0
char 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 = middle1&0xC0; //提取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)&&((middle2&0x02)==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)&&((middle2&0x01)==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)&&((middle2&0x01)==0x00)) //左边到线，右边未到线
flagi=1;
if((middle1==0x02)&&((middle2&0x02)==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:6
void TIM1_OVF(void) //计时器时间到,进入中断
{char light;
CLI();
a++;
c++;
if(a==40) //速度采样时间到
{a=0;
PIDA();
PIDB();}
if(c=100) //寻线采样时间到
{c=0;
SearchLine();}
SEI();
}
|
