robot程序.docx
- 文档编号:27053613
- 上传时间:2023-06-26
- 格式:DOCX
- 页数:18
- 大小:18.93KB
robot程序.docx
《robot程序.docx》由会员分享,可在线阅读,更多相关《robot程序.docx(18页珍藏版)》请在冰豆网上搜索。
robot程序
//***************************FileName:
Robbot.c***********************//
//***************************ICCAVR6.30编译**************************//
#include
#include
//****************************宏定义********************************//
#defineAa0.5//PID参数
#defineBa-0.5//PID参数
#defineCa0//PID参数
#defineAb0.09//PID参数
#defineBb-0.1//PID参数
#defineCb0//PID参数
#defineU112//PID参数
#defineMaxSpeed0x40//最大速度
#defineMidSpeed0x18//中速,用于转弯
#defineSize6//任务数组大小
//*****************************全局变量定义**************************//
charsflag=0x00;//记录上一次校偏状态
charcrossflag=0;//过线标志,用于判断是否过线
charforflag=0;//记录上一次机器人行进状态
charforlight;//记录上一次A口光电传感器的状态
floatEkA;//本次左边电机速度误差
floatEkA_1=0;//上次左边电机速度误差
floatEkA_2=0;//上上次左边电机速度误差
floatEkB;//本次左边电机速度误差
floatEkB_1=0;//上次左边电机速度误差
floatEkB_2=0;//上上次左边电机速度误差
charflage=0;
chara=0;//溢出次数,控制PID窗口时间
charc=0;//控制寻线频率
intdesireV=10;
charb=0;
chartime=0;//机器人行走步数
charfob=0;//=1后退,=0前进
charTask[Size]={0x37,0x27,0x1B,0x29,0x23,0x1B};//任务数据数组
//******************************定时器1初始化*************************//
voidTimer1Init(inttemptimsk,inttemptccrA,inttemptccrB)
{unsignedcharsreg;
TIMSK=temptimsk;
sreg=SREG;//保存全局中断标志
_CLI();//屏蔽所有中断
TCCR1A=temptccrA;
TCCR1B=temptccrB;
SREG=sreg;//恢复全局中断标志
}
//******************************写OCR1A寄存器**************************//
voidSetOutputComReg1A(inttempocr)
{unsignedcharsreg;
sreg=SREG;
_CLI();
OCR1A=tempocr;
SREG=sreg;
}
//******************************写OCR1B寄存器**************************//
voidSetOutputComReg1B(inttempocr)
{unsignedcharsreg;
sreg=SREG;
_CLI();
OCR1B=tempocr;
SREG=sreg;
}
//******************************读OCR1A寄存器**************************//
intGetOutputComReg1A()
{inttemp;
temp=OCR1A;
return(temp);
}
//******************************读OCR1B寄存器**************************//
intGetOutputComReg1B()
{inttemp;
temp=OCR1B;
return(temp);
}
//******************************长延时函数***************************//
voidDELAY(intdelaytime)
{inti,j;
for(i=0;i<=delaytime;i++)
{for(j=0;j<=0xFFFE;j++);}
}
//******************************短延时函数***************************//
voiddelay(inti)
{intj;
for(j=0;j<=i;j++);
}
//******************************PID调节:
左电机***********************//
voidPIDA()
{chary;//本次采样速度值
floatu;//电压差值
intz;//本次输出增量
inttemp1;//临时记录值
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调节:
右电机***********************//
voidPIDB()
{chary;
floatu;//电压差值
intz;
inttemp1;
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***************************//
charTalone()
{if((forlight&0x09)==0)//3单独亮
return(0);
else
return
(1);//返回为校偏状态值
}
//***************************寻线处理函数2***************************//
charFUalone()//4单独亮
{if((forlight&0x05)==0)
return(0);
else
return
(2);
}
//***************************寻线处理函数3***************************//
charFIalone()//5单独亮
{if((forlight&0x22)==0)
return(0);
else
return
(1);
}
//***************************寻线处理函数4***************************//
charSalone()//6单独亮
{if((forlight&0x12)==0)
return(0);
else
return
(2);
}
//***************************寻线处理函数5****************************//
charInLine1(void)//线上偏离判断
{chardiscrepancy;//行进状态
charl2;//传感器状态
l2=0xFF^PINA;
l2=0x3C&l2;
l2=l2>>2;
switch(l2)
{case0x00:
discrepancy=0;break;//0000b未偏
case0x01:
discrepancy=0;break;
case0x02:
discrepancy=0;break;
case0x04:
discrepancy=2;break;//0010b左偏
case0x05:
discrepancy=2;break;
case0x06:
discrepancy=2;break;
case0x08:
discrepancy=1;break;//0001b右偏
case0x09:
discrepancy=1;break;
case0x0A:
discrepancy=1;break;}
return(discrepancy);
}
//***************************寻线处理函数6****************************//
charInLine2(void)//线上偏离判断
{chardiscrepancy;
charl2;
l2=0xFF^PINA;
l2=0x3C&l2;//3,4,5,6灯
l2=l2>>2;
switch(l2)
{case0x00:
discrepancy=0;break;//0000b//未偏
case0x01:
discrepancy=1;break;//0001b右偏
case0x02:
discrepancy=2;break;//0010b左偏
case0x04:
discrepancy=0;break;
case0x05:
discrepancy=1;break;
case0x06:
discrepancy=2;break;
case0x08:
discrepancy=0;break;
case0x09:
discrepancy=1;break;
case0x0A:
discrepancy=2;break;}
return(discrepancy);
}
//***************************寻线处理函数7****************************//
charOutLine(void)//线外偏离判断
{chardiscrepancy;
charl2;
l2=PINA^0xFF;
l2=0x3C&l2;//00111100b
l2=l2>>2;
switch(l2)
{case0x00:
discrepancy=forflag;break;//0000b
case0x01:
discrepancy=Talone();break;//0001b
case0x02:
discrepancy=FUalone();break;//0010b
case0x04:
discrepancy=FIalone();break;//0100b
case0x05:
discrepancy=0;break;//0101b
case0x06:
discrepancy=0;break;//0110b
case0x08:
discrepancy=Salone();break;//1000b
case0x09:
discrepancy=0;break;//1001b
case0x0A:
discrepancy=0;break;//1010b
case0x0F:
discrepancy=0;break;//1111b
}
return(discrepancy);
}
//***************************校偏函数********************************//
voidRevise(chardiscrepancy)
{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;}
}
//***************************寻线函数********************************//
voidSearchLine()
{charonline;//测点状态寄存器
charl78;//7,8号测点状态寄存器
chary=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());}}//线外走偏
}
//***************************停止函数********************************//
voidstop()
{PORTD=PORTD|0xCF;
}
//***************************转向函数********************************//
charaction(chardirection)//=1为右转,==0为左转,=2为停止
{charlight;
chari=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);
}
//*******************************前进函数****************************//
voidForward(charstep,chardirection,charfob)
{charflagi=0;//单边过线标志,整即过线后清0
charflagj;//过线辅助判断标志
charmiddle1=0x00;//过线检测状态寄存器
charmiddle2=0x00;//前次过线检测状态寄存器
charpostionflag=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;
}
//***************************主函数**********************************//
voidmain()
{charmovesteps;//传递前进或后退步数
charmovedirection;//运动方向
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)br
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- robot 程序