避障机器人设计与调试

避障机器人设计与调试
一、实训目的
1 了解机器人大赛中避障的规则，进一步理解电机和红外测距传感器的原理；
2 掌握避障机器人的设计方法。

二、实训设备
1 硬件：HOST机一台、基于机器人项目驱动的嵌入式教学实训平台一套；
2 软件：WIN2000 或xp 操作系统、Siliconlab IDE开发环境、调试器。

三、实训原理
实现避障的功能从原理上是通过分析红外测距传感器的测量值判断障碍物的位置，然后驱动电机避开障碍物。

通过连接三个红外测距传感器，机器人可以探测到120 度的视角范围的障碍物。

四、实训步骤
1、正确连接PC 机、调试器和基于机器人项目驱动的嵌入式教学实训平台；
2、打开电源, 打开Siliconlab IDE；
3、打开避障的例程，正确调试并运行该程序。

4、烧录完成后断电拔掉调试器，把组装好的机器人放入模拟的参赛场地，再次打开电
源，观察机器人避障的情况。

void main()
{
unsigned int ad_test;
unsigned int i = 0;
SystemInit();
while(1)
{
DodgeObstruction();
}
}
void DodgeObstruction()
{
unsigned char ad_distance_left = 0;
unsigned char ad_distance_midl = 0;
unsigned char ad_distance_rigt = 0;
while(1)
{
ad_distance_left = GetIR_Distance(2);
ad_distance_midl = GetIR_Distance(3);
ad_distance_rigt = GetIR_Distance(4);
if(ad_distance_left>40 && ad_distance_midl>40 && ad_distance_rigt>40)
{
DC_Motor(1,0,60);
DC_Motor(3,0,60);
DC_Motor(2,0,60);
DC_Motor(4,0,60);
}
if(ad_distance_rigt<40)
{
DC_Motor(1,0,60);
DC_Motor(3,0,60);
DC_Motor(2,2,10);
DC_Motor(4,2,10);
}
if(ad_distance_midl<40)
{
DC_Motor(1,2,70);
DC_Motor(3,2,70);
DC_Motor(2,2,70);
DC_Motor(4,2,70);
}
if(ad_distance_left<40)
{
DC_Motor(1,2,10);
DC_Motor(3,2,10);
DC_Motor(2,0,60);
DC_Motor(4,0,60);
}
}
}
void
DC_Motor(unsigned char motor_num,unsigned char direction, unsigned char motor_speed) {
unsigned char SFRPAGE_save = SFRPAGE;
SFRPAGE = CONFIG_PAGE;
if(!motor_speed)
{
switch(motor_num)
{
case 1:
PCA0CPH0 = 255;
break;
case 2:
PCA0CPH1 = 255;
break;
case 3:
PCA0CPH2 = 255;
break;
case 4:
PCA0CPH3 = 255;
break;
case 5:
PCA0CPH4 = 255;
break;
case 6:
PCA0CPH5 = 255;
break;
default: break;
}
}
else
switch(motor_num)
{
case 1:
PCA0CPH0 = 255 - (motor_speed+116);
break;
case 2:
PCA0CPH1 = 255 - (motor_speed+116);
break;
case 3:
PCA0CPH2 = 255 - (motor_speed+116);
break;
case 4:
PCA0CPH3 = 255 - (motor_speed+116);
Break;
case 5:
PCA0CPH4 = 255 - (motor_speed+116);
break;
case 6:
PCA0CPH5 = 255 - (motor_speed+116);
break;
default: break;
}
switch(direction)
{
case 0:
if(motor_num==1) P3 &= ~0x20; //P1.3 = 0,即DIR0置0
if(motor_num==2) P1 &= ~0x10; //P1.4 = 0,即DIR1置0
if(motor_num==3) P1 &= ~0x20; //P1.5 = 0,即DIR2置0
if(motor_num==4) P1 &= ~0x40; //P1.6 = 0,即DIR3置0
if(motor_num==5) P3 &= ~0x40; //P3.6 = 0,即DIR4置0
break;
case 1:
if(motor_num==1) PCA0CPH0 = 255;
if(motor_num==2) PCA0CPH1 = 255;
if(motor_num==3) PCA0CPH2 = 255;
if(motor_num==4) PCA0CPH3 = 255;
if(motor_num==5) PCA0CPH4 = 255;
if(motor_num==6) PCA0CPH5 = 255;
break;
case 2:
if(motor_num==1) P3 |= 0x20; //P3.5 = 1,即DIR0置1；
if(motor_num==2) P1 |= 0x10; //P1.4 = 1,即DIR1置1；
if(motor_num==3) P1 |= 0x20; //P1.5 = 1,即DIR2置1；
if(motor_num==4) P1 |= 0x40; //P1.6 = 1,即DIR3置1；
if(motor_num==5) P3 |= 0x40; //P3.6 = 1,即DIR4置1；
break;
default: break;
}
SFRPAGE = SFRPAGE_save;
}。