当机器人进入墙角时，可能会碰到这种情况：首先左边有障碍物，于是它倒退、右转，再向前行走，这时右边也有障碍物，于是再后退、左转、前进，左边又有障碍物，再次右边有障碍物……此时，机器人就会一直困在墙角里出不来。
技巧是记下障碍物交替的总次数，还有必须记下每个传感器前一次的状态，并和当前的状态对比。如果状态相反，就在交替总数上加“1”。如果这个交替总数超过了程序预先给定的阀值，那么就该做一个“U”形转弯，并且把计数器复位。
机器人导航避开死角范例程序如下。
这个程序使机器人在第四次或第五次交替探测到“死区”后，完成一个“U”形的拐弯，其次数取决于哪一个传感器先感应到障碍物。
byte w Left Old; // Previous loop whisker values
byte w Right Old;
byte counter; // For counting alternate corners
int SNUM[3] ;
int INA = 4; //电机 A 正反转控制端
int PWMA = 5; //电机 A 调速端
int INB = 7; //电机 B 正反转控制端
int PWMB = 6; //电机 B 调速端
void motospd(int sp1,int sp2) //电机速度控制函数，括号内分别为左右电机速度值，
{ //范围-255～+255，正值为正转，负值为反转。
if(sp1〉0)
digital Write(INA, HIGH);
else
digital Write(INA, LOW);
if(sp2〉0)
digital Write(INB, HIGH);
else
digital Write(INB, LOW);
analog Write(PWMA,abs (sp1));
analog Write(PWMB,abs (sp2));
}
void setup() // Built-in initialization block
{
pin Mode(8, INPUT); //配置左传感器 IO 口为输入
pin Mode(14, INPUT); //配置中传感器 IO 口为输入
pin Mode(15, INPUT); //配置右传感器 IO 口为输入
pin Mode(INA,OUTPUT);
pin Mode(INB,OUTPUT); //配置电机驱动 IO 口为输出
tone(4, 3000, 1000); // Play tone for 1 second
delay(1000); // Delay to finish tone
w Left Old = 0; // Init. previous whisker states
w Right Old = 1;
counter = 0; // Initialize counter to 0
}
void loop() // Main loop auto-repeats
{
// Corner Escape
byte w Left = digital Read(15); // Copy right result to w Left
byte w Right = digital Read(8); // Copy left result to w Right
if(w Left != w Right) // One whisker pressed?
{ // Alternate from last time?
if ((w Left != w Left Old) && (w Right != w Right Old))
{
counter++; // Increase count by one
w Left Old = w Left; // Record current for next rep
w Right Old = w Right;
if(counter == 4) // Stuck in a corner?
{
w Left = 0; // Set up for U-turn
w Right = 0;
counter = 0; // Clear alternate corner count
}
}
else // Not alternate from last time
{
counter = 0; // Clear alternate corner count
}
}
// Whisker Navigation
if((w Left == 0) && (w Right == 0)) // If both whiskers contact
{
motospd(-100,-100); //后退
delay(50);
motospd(50,100); //左转
}
else if(w Left == 0) // If only left whisker contact
{
motospd(-100,-100); //后退
delay(50);
motospd(100,50); //右转
}
else if(w Right == 0) // If only right whisker contact
{
motospd(-100,-100); //后退
delay(50);
motospd(50,100); //左转
}
else // Otherwise, no whisker contact
{
motospd(100,100); //直行
}
delay(20);
}
免责声明：以上内容源自网络，版权归原作者所有，如有侵犯您的原创版权请告知，我们将尽快删除相关内容。
