Wall Follower Robots

@thrandell

Real world jobs have meant I have had to pack away the robot projects since the start of the Easter break until I get a break some time in the future.

Last time I thought about "wall following" was using sonar when following this thread.
https://forum.dronebotworkshop.com/user-robot-projects/henry-ix-a-fully-autonomous-robot-platform/

Although some aspects of robotics is common to all robot projects the few on this forum with an interest in robotics are working on using different types of robot bases and sensor types and ultimate ambitions for an outcome.

I found the Arduino sketch that I used on this wall follower.  It’s an early effort so don’t judge me too harshly 😉

The robot used an Arduino clone board and LSM303 compass that I picked up at Pololu Robotics & Electronics.  The compass has been discontinued but I did find their library code if you want to follow along. https://github.com/pololu/lsm303-arduino    It looks like my code lost some indentation after the post, sorry about that.

Tom

/* Sharp IR Range Finder Implementation of Tim Bower's Virtual Triange Wall Follower.
* IRFollowWall
* Calibration has shown that the IR sensors can measure from 3.9 inches to 31.5 inches.
* This is a test using an A* Prime board with two Sharp IR Sensors and a servo to point the leftmost sensor perpendicular to the robot's pose.
* IRSensor1 - Pin: A2, 5V 
* IRSensor2 - Pin: A3, 5V
* The two IR sensors are mounted on a servo with a 45 degree angle between them.
* The coordinate frame is measured in inches.
* The (0,0) of the robot's frame is somewhere behind the two sensors.
* 
* NOTE: a positive AngleToWall => turn left
* a negative AngleToWall => turn right This seems like the opposite of a polar coordinate system, ie. substract to turn left, add to turn right
* 
* setM1Speed(int speed) 
* setM2Speed(int speed)
* setSpeeds(int m1Speed, int m2Speed)
* Speed should be between -400 and 400
* 
* Large parts of this sketch were browored from a Pololu compass demo.
*/

#include <DRV8835MotorShield.h>
#include <Wire.h>
#include <LSM303.h>
#include <math.h>

#define CALIBRATION_SAMPLES 100 // Number of compass readings to take when calibrating

// Allowed deviation (in degrees) relative to target angle that must be achieved before driving straight
#define DEVIATION_THRESHOLD 5 // less then 5 and it hunts, greater then 5 and it gets sloppy.

DRV8835MotorShield motors;
LSM303 compass;

extern float sensor_arr[3] = {0.0, 0.0, 0.0};
int IRSensor1 = 2; // connected to the A2 pin
int IRSensor2 = 3; // connected to the A3 pin
float degreesInRadian = 57.2958;
int Wall_Lead = 60; // tunable constant (in inches), increase if turning too aggressively, decrease if too sluggish 
int Dwall = 12; // tunable constant, distance from the wall (in inches) that robot should track

uint16_t speedStraightLeft = 100; // Maximum motor speed when going straight; variable speed when turning
uint16_t speedStraightRight = 100;
uint16_t turnBaseSpeed = 60; // Base speed when turning (added to variable speed)
// uint16_t driveTime = 2000; // Time to drive straight, in milliseconds

void setup() { 
Serial.begin(9600);

// Initialize the Wire library and join the I2C bus as a master
Wire.begin();
compass.init();
compass.enableDefault();
// is there a configureforcompassheading() method?
delay(1000);

// uncomment one or both of the following lines if your motors' directions need to be flipped
motors.flipM1(true);
//motors.flipM2(true);

motors.setM1Speed(turnBaseSpeed * 3);
motors.setM2Speed(-turnBaseSpeed * 3);

LSM303::vector<int16_t> running_min = {32767, 32767, 32767}, running_max = {-32768, -32768, -32768};
unsigned char index;
for(index = 0; index < CALIBRATION_SAMPLES; index++)
{
// take a reading of the magnetic vector and store it in running_min/max
// making the assumption that the robot is always running on the level ground
compass.read();
running_min.x = min(running_min.x, compass.m.x);
running_min.y = min(running_min.y, compass.m.y);
running_min.z = min(running_min.z, compass.m.z);

running_max.x = max(running_max.x, compass.m.x);
running_max.y = max(running_max.y, compass.m.y);
running_max.z = max(running_max.z, compass.m.z);

delay(50);
}

motors.setM1Speed(0);
motors.setM2Speed(0);
delay(100); // to let the robot come to a stop

// Store calibrated values in m_max and m_min
compass.m_min.x = running_min.x;
compass.m_min.y = running_min.y;
compass.m_min.z = running_min.z;

compass.m_max.x = running_max.x;
compass.m_max.y = running_max.y;
compass.m_max.z = running_max.z;

// This was a small improvement over the following settings.
// These are static (no motor runnning) settings
//compass.m_min = (LSM303::vector<int16_t>){-2505, -1931, -1809};
//compass.m_max = (LSM303::vector<int16_t>){+3215, +1604, +1950};

}

void loop() {

float relative_heading, angleToTurn;
int speed;
static float target_heading = 360; // a static variable will preserve its value even if is out of its scope.
// 360 because a setting of 0 had some problems.
// why is this static if it is only used in loop?
// Heading is given in degrees away from the magnetic vector, increasing clockwise.
compass.read();
sensor_arr[0] = compass.heading();

// This gives us the relative heading with respect to the target angle
relative_heading = relativeHeading(sensor_arr[0], target_heading);

Serial.print("Heading "); Serial.print(sensor_arr[0]); Serial.print(" Target heading "); Serial.print(target_heading); 
Serial.print(" Relative heading "); Serial.println(relative_heading);
Serial.println();

// If the robot has turned to the direction it wants to be pointing, go straight
if(abs(relative_heading) < DEVIATION_THRESHOLD)
{
motors.setSpeeds(speedStraightLeft, speedStraightRight);

// delay(driveTime);

// Turn off motors and wait a short time to stop smoothly.
// motors.setSpeeds(0, 0);
// delay(100);

// Set target heading to move toward or away from wall
angleToTurn = getAngleToTurn(Wall_Lead, Dwall);
compass.read();
sensor_arr[0] = compass.heading();
target_heading = sensor_arr[0] + angleToTurn;
if(target_heading >= 360)
target_heading -= 360;
Serial.print("Heading "); Serial.print(sensor_arr[0]); Serial.print(" Angle to Turn "); Serial.print(angleToTurn); 
Serial.print(" Target "); Serial.println(target_heading); Serial.println();

}
else {

// To avoid overshooting, the closer the robot gets to the target
// heading, the slower it should turn. Set the motor speeds to a
// minimum base amount plus an additional variable amount based
// on the heading difference.

speed = speedStraightLeft*relative_heading/180;

if (speed < 0)
speed -= turnBaseSpeed;
else
speed += turnBaseSpeed;

motors.setSpeeds(speed, -speed);
}

} // End loop

// Yields the angle difference in degrees between two headings
float relativeHeading(float heading_from, float heading_to)
{
float relative_heading = heading_to - heading_from;

// constrain to -180 to 180 degree range
if (relative_heading > 180)
relative_heading -= 360;
if (relative_heading < -180)
relative_heading += 360;

return relative_heading;
}

float getAngleToTurn(int Lead, int distToWall) {

float IR1reading;
float IR2reading;
float theta;
float sin_forty_five = .7071067; // The sin(45) constant value
float cos_forty_five = .7071067; // The cos(45) constant value
// cos(45) = sin(45) = 1/square root of 2 = 1 / 1.41421356237 = 0.707106781187 
// Tim's formula
// (x0, y0) = (0, convertToInches(IR1reading,1)) 
// (x1, y1) = (convertToInches(IR2reading,2) * cos(45), convertToInches(IR2reading,2) * sin(45))
// Angle to turn = atan2((y1 - Dwall), (x1 + Wall_Lead - y0))

IR1reading = getReading(IRSensor1,1); 
IR2reading = getReading(IRSensor2,2); 
Serial.print("IR Sensor 1 "); Serial.println(IR1reading); 
Serial.print("IR Sensor 2 "); Serial.println(IR2reading);
theta = atan2( IR2reading * sin_forty_five - distToWall, IR2reading * cos_forty_five + Lead - IR1reading ); // in radians
return (theta * degreesInRadian) * -1;
}

float getReading(int IRSensor, int sensorNum) { // Running Average

float reading;
float k = 30.0; // this software is so slow that it hunts when k is larger. At 100 is just turns in a circle.
float sample; 

reading = convertToInches(analogRead(IRSensor), sensorNum);
sample = (1/k * reading) + ((1 - 1/k) * sensor_arr[sensorNum]); // the array index and IR sensor number just happen to match.
sensor_arr[sensorNum] = sample;

return sample;
}

float convertToInches(int x, int sensorNum) { // Apply the calibration functions to the raw reading.
float y; // Distance in inches. With IR2 the reading increases when
// the object is closer then 3.9 inches. 5 inches might be the minimum.

if (sensorNum == 1) {
if (x < 46) x = 46;
y = 1321.5 * pow(x, -0.9433);
// Serial.println("using sensor 1"); // for testing only.
}
else { // must be IR sensor number 2
if (x < 81) x = 81;
y = 3836.6 * pow(x, -1.1337);
} 
//if (y > 31.5) // anything greater is out of scope.
// return 32;
//else if (y < 3.9) // anything less is also out of scope.
// return 3;
//else
return y;
}

Posted by: @thrandell

↑

I was organizing all of my robot stuff and I found the remains of an old wall follower that I made five years ago.  I was thinking that maybe other readers of the forums might also have some wall follower experiences to share so I started this topic…

Anyway, the control software I used was something that Tim Bower came up with.  He is an associate professor at Kansas State in the US.  He calls it the Virtual Triangle Wall Follower.  You can read about his idea here: timbow11.dreamhosters.com/robot_prog/MobileBot/Algorithms/WallFollow.html

The robot I built used a compass and two Sharp IR range finders to implement his algorithm.  I calibrated the two sensors to follow the wall at 10 inches and I thought it was interesting what the robot would perceive as 10 inches when the wall surface and lighting changed during its travels.  Here’s the video: https://www.youtube.com/@tomrandell683/shorts

It looks like it got stuck on the door mat. 🙁

Tom

Most doormats have a raised edge or lip, even if it's just 5–10 mm. Small robots with limited ground clearance or soft rubber wheels often hit the mat like a speed bump and get stuck. If the chassis baseplate or caster wheel snagged the edge, it would stop forward motion.Doormats are often fibrous or textured, which can jam or resist the drive wheels.The surface may also reduce traction, especially if the robot has smooth plastic wheels.