Line Robot - Basic lessons - Exercises - Wall following with 2 sensors
If we want to follow wall better or do not have enough space to bend front sensors, we can use 2 sensors per side instead of 1.
Theory
Wall is thick line on the right of the picture, robot's ideal position is dashed line (robot's longitudinal axis in the ideal position). Therefore, the robot is too far from the wall and it is rotated to the wall. Red rectangles are distance sensors and red dashed lines are distances.
2 sensors allow us to estimate 2 errors:
- rotational error and
- error in distance to the wall.
We must address both issues. Difference between front and rear distances can be used to determine rotational error. Mean value of the both sensors is distance to the wall. In every moment we must calculate both errors and drive the robot in a way that tries to eliminate both of them.
Task: smooth wall following with 2 sensors.
Write a program that implements the theory above, therefore follows a wall with 2 sensors. This task is difficult. If You are not an experienced programmer, just take a look at the solution an try to understand the logic.
Solution
void RobotSoccer::loop() {
const uint16_t FOLLOW_WALL_MM = 90; // Ideal distance to wall.
const float STRENGTH_ROTATION = 3; // The bigger this factor, the more will the robot try to decrease rotational error.
const float STRENGTH_LATERAL = 3; // The bigger this factor, the more will the robot try to decrease lateral error (distance to the wall).
const bool followLeft = true; // If not, follow right wall.
// Rotational and lateral errors
float errorTooMuchCW = followLeft ? frontLeft() - rearLeft() : rearRight() - frontRight();
float errorTooMuchLeft = followLeft ? FOLLOW_WALL_MM - (frontLeft() + rearLeft()) / 2.0 : (frontRight() + rearRight()) / 2.0 - FOLLOW_WALL_MM;
// Combined error
float increaseLeftBy = -errorTooMuchCW * STRENGTH_ROTATION + errorTooMuchLeft * STRENGTH_LATERAL;
// Drive one side at top speed.
go(increaseLeftBy < 0 ? TOP_SPEED + increaseLeftBy : TOP_SPEED , increaseLeftBy < 0 ? TOP_SPEED : TOP_SPEED - increaseLeftBy);
}