We are now in a position to make a few major changes and arrive upon the final version of Controller, which is fully capable of autonomously following a multisegment path. Let me start by presenting the code in full, with the changes highlighted, and then I will explain the pertinent parts of the code.
We begin with the Controller class, the heart of the code, which is responsible for making the robot follow the path:
import lejos.hardware.Sound;
import lejos.hardware.port.Port;
import lejos.utility.Delay;
public class Controller
{
private final static int DELAY = 50;
private final static int SKIP_FORWARD = 20;
private final static double DELAYS_PER_DEG = 100.0 / 180;
private final static int SMALL_ROT = 10;
private final static int MED_ROT = 90;
private final static int MAX_ROT = 135;
private ColorSensor sensor;
private DifferentialDrive drive;
public Controller(Port sensor_port, Port left_port, Port right_port)
{
log("Initializing Controller");
sensor = new ColorSensor(sensor_port);
drive = new DifferentialDrive(left_port, right_port);
}
public void run()
{
log("Running controller");
do
{
move();
}
while (wide_seek());
end();
}
private void move()
{
log("Forward");
do
{
naive_move();
}
while (narrow_seek());
log("Stop");
}
private void naive_move()
{
drive.forward();
while (sensor.onPath())
{
delay();
}
drive.stop();
}
private boolean narrow_seek()
{
log("Seeking Path in Narrow Arc.");
return sweepClockwise(SMALL_ROT) || sweepCounterClockwise(2 * SMALL_ROT) || sweepClockwise(SMALL_ROT);
}
private void skip_forward(int duration)
{
drive.forward();
for (int i = 0; i < duration; i++)
{
delay();
}
drive.stop();
}
private boolean wide_seek()
{
skip_forward(SKIP_FORWARD);
return sweepClockwise(MED_ROT) ||
sweepCounterClockwise(MED_ROT + MAX_ROT) ||
sweepClockwise(2 * MAX_ROT) ||
sweepCounterClockwise(MAX_ROT);
}
private boolean sweepClockwise(int rot_limit)
{
log("Sweeping clockwise. Limit: " + rot_limit + " degrees.");
drive.rotateClockwise();
return sweep(rot_limit);
}
private boolean sweepCounterClockwise(int rot_limit)
{
log("Sweeping counter-clockwise. Limit: " + rot_limit + " degrees");
drive.rotateCounterClockwise();
return sweep(rot_limit);
}
private boolean sweep(int rot_limit)
{
for (int i = 0; i < (rot_limit * DELAYS_PER_DEG); i++)
{
delay();
if (sensor.onPath())
{
log("Path Detected");
drive.stop();
return true;
}
}
log("Path not detected. Rotation limit exceeded.");
drive.stop();
return false;
}
private void end()
{
Sound.beepSequence();
log("Program ends");
}
private void delay()
{
Delay.msDelay(DELAY);
}
private static void log(String msg)
{
System.out.println("log> " + msg);
}
}The first change is the reintroduction of a move() method that is not naive but is capable of autocorrection by using narrow_seek():
private void move()
{
log("Forward");
do
{
naive_move();
}
while (narrow_seek());
log("Stop");
}The key feature of the method is the do-while loop that is characterized by the fact that it always iterates at least once before the condition is checked at the bottom. This means that when move() is called, the naive_move() method is called at least once (causing the robot to start moving).
When naive_move() returns either because the robot has drifted off the path or because the segment has ended, the while condition runs narrow_seek(). This results in a three-sweep search for the path in a 20 degree arc. If the path is reacquired, naive_move() is called again; otherwise, the loop terminates and the move() method ends. Thus, move() is designed for motion along a straight-line segment with the ability to correct for drift.
When move() finishes, it means narrow_seek() failed to find the path so either the next segment is at an angle of more than 10 degrees from the current segment or the path has come to an end. To test for the former, we implemented the
wide_seek() method, which is like narrow_seek() but searches for the path in a wider arc. The code is as follows:
private boolean wide_seek()
{
skip_forward(SKIP_FORWARD);
return sweepClockwise(MED_ROT) ||
sweepCounterClockwise(MED_ROT + MAX_ROT) ||
sweepClockwise(2 * MAX_ROT) ||
sweepCounterClockwise(MAX_ROT);
}The first thing this method does is to move the robot forward by a small amount so that the center of the robot is on top of the end of the current segment. This is achieved by calling the skip_forward() method. This was implemented to deal with the issue of the robot not pivoting in the correct place. By skipping forward, we ensure that if the next segment is discovered while sweeping, the robot will be aligned correctly along the new segment the moment the detection occurs and the sweep stops.
The next statement of this method is analogous to narrow_seek() returning the result of a complex OR operation, consisting of several clockwise and anticlockwise sweeps. We defined two limits for sweeps used here: MED_ROT (90 degrees) and MAX_ROT (135 degrees). The idea is that it is more likely that the next segment of the path is within 90 degrees of the current segment than it is within the next 45 degrees after that. So, we come up with an algorithm that checks in an arc of 180 degrees around the current orientation of the robot before moving on to the larger 270 degree arc. The two arcs are shown in the following figure:
The downside of this is that some parts of the arc are covered multiple times, so it is a balancing act. Different configurations of the path may require a different approach, but these are easy to implement in the program.
The way the OR operation is set up, the robot first sweeps for 90 degrees in the clockwise direction. It then sweeps in the opposite direction first through 90 degrees to get back to the starting orientation, and then it continues on for the maximum 135 degrees. If it still fails to find the path, it does a huge 270 degree rotation to check the clockwise direction to its maximum 135 degree limit. Failing even this, the robot rotates 135 degrees anticlockwise to return to the starting orientation and returns false to indicate that no path was found. The arcs through which the robot sweeps as it searches for the next segment are shown in the following figure:
Note that this setup means that the robot never looks for the next segment in the 90 degree arc directly behind it. This is to ensure that it doesn't pick up the current segment in the opposite direction because the rotations aren't precise and there is a possibility that the robot can overshoot these limits. These aren't hard numbers; they were arrived upon by trial and error. The code was set up with these numbers defined as static variables rather than strewn throughout the code to allow easy access and quick changes.
This leaves us with the skip_forward() method:
private void skip_forward(int duration)
{
drive.forward();
for (int i = 0; i < duration; i++)
{
delay();
}
drive.stop();
}It is rather straightforward. We supply as an argument the amount of delays (basically a measure of time in this code) we want the robot to move forward. We then start the forward motion and use a for loop to delay/pause execution for that long, and after that we stop the motion. Empirically, I discovered that the correct number of delays to bring the center of the robot on top of the end of the current segment is 20, and I stored it in the SKIP_FORWARD variable that is used in the call to skip_forward() inside wide_seek().
The final method that is changed is the run() method itself. In run(), we want to use move() to move the robot along straight-line segments interspersed with wide_seek() to find the next segment when the current one ends. To that end, we set it up analogously to how move() works:
public void run()
{
log("Running controller");
do
{
move();
}
while (wide_seek());
end();
}The do-while loop iterates the move() method and after the completion of each segment, looks for the next segment using wide_seek(). When the end of the path is reached, wide_seek() will return false, terminating the loop and bringing the program to an end.
To test this final version of the code, construct a number of different paths consisting of straight-line segments at an angle to each other, where none of them are crossing and none of them turn with angles less than 45 degrees (as it's too sharp).
Execute the program on these paths to ensure that the values hardcoded in for the speed, rotation limits, delay times, and skip forwards are valid.