Maze Solver - LeJOS

In this maze solver the robot follows a left hand rule as given in https://embedjournal.com/shortest-path-line-follower-robot-logic-revealed/. The program works in two phases. In the first phase, the robot samples the entire path, stores the x and y co-ordinates of the path and creates a map of maze in the memory. Given two consecutive points, the program also calculates and stores the direction of the robot at each point.  Then the program calculates the shortest path by cancelling out the U turn samples and the samples in opposite directions. In the second phase, the robot actually follows the shortest path. During path following, the robot samples the path again and whenever the direction of two consecutive points falls apart from the expected direction (which we got in the first phase) the robot aligns its direction accordingly.

Let me go through the program logic for shortest-path-finding and shortest-path-following in detail. However, before going through it, it would be better to read the article about Limited Maze Solver in order to understand the high level code structure that I have followed. Let us go through the robot path below and mark the path with directions and corresponding sample numbers in those directions. 

Let us assume that the robot path has the following directions and samples along each of them, when the robot traverses it in the first phase: (In real life, the number of samples may differ when the robot traverses, but the high level logic remains the same)

[North (20), West (10), South (30), East (30), North (35), South (20), East (15), North (40), West (20), East (20), North (20), West (20), East (20), North (25), West (20), East (20), South (25 + 20 + 40 + 20 = 105)]

According to the left-hand-rule in the reference site above, we will further reduce the samples where we have East-West and North-South directions in pairs. This is quite simple to understand, as when the robot traverses to North and then comes to South along the same path, the efforts of traversing eventually cancel out. Same for the directions East-West. After cancelling the opposite direction efforts in the first iteration, the path and the samples look like below. 

[North (20), West (10), South (30), East (30), North (15), East (15), North (40), North (20), North (25), South (25 + 20 + 40 + 20 = 105)]

We will then perform the second iteration of cancellation of East-West and North-South. Below is the result after second iteration.  

[North (20), West (10), South (30), East (30), North (15), East (15), North (40), North (20), South (20 + 40 + 20 = 80)]

We will then perform the third iteration of cancellation of East-West and North-South. Below is the result after third iteration.  

[North (20), West (10), South (30), East (30), North (15), East (15), North (40), South (40 + 20 = 60)]

We will then perform the fourth iteration of cancellation of East-West and North-South. Below is the result after fourth and final iteration. 

[North (20), West (10), South (30), East (30), North (15), East (15), South (20)]

When we reach the above iteration and the shortest path does not reduce any further with the "cancellation of samples in opposite direction strategy", we will break the logic. Let us go through few important methods which I have used in the Cruiser class to come up with the shortest path. 

run(): This is the run() method of the Cruiser thread which has the important logic to find and follow the shortest path. It calls three main methods to complete our goal.

populateFirstPathSamples()

findShortedPath()

followShortestPath()

summarize(): While explaining this method I am assuming that you have gone through Limited Maze Solver article in detail. In addition to my explanation there, I want to mention that in this method we take into account all the consecutive direction changes. With each direction change, we keep a record of the number of samples in the same direction. Let us assume that while populating the First Path samples, we get the direction sequence as follows (as known from the direction property of the summary objects):

[4, 4, 2, 2, 2, 2, 3, 3, 3, 3, 3, 1, 1, 1, 1, 1]

This sequence will get recorded into our directionOutput objects as follows:

4 (2), 2 (4), 3 (5), 1 (5)

Here, the figures in the bracket represent the number of samples in the given direction. Each directionOutput object has four properties like summary objects, X, Y, direction and removal. In addition to that I have introduced one more property, numberOfSamples.  

Plotted TachoPoseProvider Readings

I realized that the Maze Solver was going little ahead of the path that was expected. To look for a reason, we decided to visualize the way the robot traverses the path. We exported the data which the pose provider gathered, to the computer. And we plotted the way the path looks like. The above image represents the "actual path" that is visualized by the robot. At the end, the robot diverges from the path that it is expected to travel. 

findShortedPath(): Calls the methods removeOppositeDirections42, removeOppositeDirections24, removeOppositeDirections13, removeOppositeDirections31 and removeUTurn iteratively until the shortest path size cannot be further reduced. Let us take a look at removeOppositeDirections42 method in detail. We will also take a look at removeUTurn method in detail. 

removeOppositeDirections42():  

Follow entire shortest path and check whether the directions 4 and direction 2 appear consecutively. Check the corresponding number of samples with each direction as explained above. Then call the decideWhichOneToRemove() method by passing the entire shortest path and the index of direction 2. Extract the number of samples for both, the direction 2 and its previous direction 4 in the direction array. Find the difference between the samples of direction 2 and direction 4. If the difference is less than DIFFERENCE_BETWEEN_EQUAL_SAMPLES, treat the distance between the two directions 2 and 4 as equal and nullify them by removing both of them from the direction array. 

NOTE: I have created the constant DIFFERENCE_BETWEEN_EQUAL_SAMPLES = 11, because due to technical/processing lags, the difference between two equal and opposite directions, may not be exactly zero. So, I have created a threshold of 11 samples to consider the equal and opposite number of samples.    

If the difference between the samples of equal and opposite direction is greater than 11, that means there is unequal distance travelled in the directions. In such case, subtract the longer distance from the shorter distance and set the difference appropriately in either direction 2 or direction 4 (based on the direction which is travelled more)

removeUTurn(): 

Follow the entire path and check if any of the directions 1, 2, 3 and 4 has number of samples less than 11, we remove all those directions from the shortest path, considering either it is a U-Turn or it is noise.  

followShortestPath(): The algorithm for following shortest path goes as follows:

1. Extract the number of samples in the heading direction of the robot from the shortest path array
2. Follow the path with PID controller pilot for the above number of samples   
3. Get two more samples from the pilot
4. Check if the direction property of those two samples match with the next direction in the shortest path array
5. If the directions match, move the shortest path array pointer to the next direction and continue with step 1
6. If the directions do not match, align the direction by calling the alignDirection() method, then move the shortest path array pointer to the next direction. Add the STABILIZATION_ SAMPLES to the number of samples in a given direction. Then continue with step 1

NOTE: I have added the STABILIZATION_SAMPLES in order to give the robot some time, to align in a given direction appropriately. However, from the performance of the robot, it looks like it is NOT required to add them. Because due to addition of them, the effect is little different. Whenever the robot is expected to the change the direction, considerable delay is present. Like the robot should turn much earlier before taking U-Turn number 1. However, it almost reaches till the end. This is because of accumulation of all previous stabilization_samples during direction changes. Same behavior is evident at U-Turn number 5. The robot is expected to stop immediately after taking the turn, however it goes ahead much further.   

alignDirection(): This method is used to turn and align the robot in different directions based on the required direction changes in the shortest path. I call the steer API of the robot which turns it onto a circular path. Please note that the entire turning will not be in the first stretch, because I have used angle 60, -60, -90 to turn. This method may need to be called twice to get the robot at the appropriate angle. For example: we need to call the pilot1.steer(-180, 60) twice to get a turn reach 90 degrees. We may need to call the pilot1.steer(-200, -90) twice to get a turn of 180 degrees. If we leave the robot at an intermediate angle, Line-Follower behavior will come to help, to automatically bring the robot on the black stripe from the white surface. 

NOTE: The performance of the robot is dependent on tuning the following properties appropriately:

• STABILIZATION_SAMPLES
• DIFFERENCE_BETWEEN_EQUAL_SAMPLES
• U_TURN_SAMPLES
• TOTAL_SAMPLES

The TOTAL_SAMPLES constant works in the first phase of robot traversal of the path. These are the samples required to complete the traversal across complete trajectory. This number is coined with a bit of trial and error and observations.

 

Limited Maze Solver With DifferentialPilot and PIDController

Maze solver being a complex topic for me, I decided to start with a simplest maze first (check the video). The program is inspired by the video here. The main reference for finding the shortest path is here. The code for this program is given below. The program works with DifferentialPilot and PIDController classes supported by leJOS. The program works in two phases. In the first phase, it samples the entire path for the direction in which the robot is moving. It then applies the shortest path algorithm over the direction. In the second phase robot follows the shortest path. I will explain all the methods in the program below. 

Cruiser(): This is the Cruiser class constructor which internally calls pidInitialize(), pilotInitialize(), pid1Initialize(), pilot1Initialize(). 

run(): This is the method over the Cruiser thread which implements the shortest path following algorithm (from the left), by calling following methods in sequence:

• populateFirstPathSamples()
• findShortedPath()
• followShortestPath()

populateFirstPathSamples(): It populates the summary array. This array contains the instantaneous values of X and Y coordinate of the robot's pose and also the Direction of the robot's pose while traversing the path for the first time. I have introduced a variable removal in order to use it for plotting the trajectory of the shortest path. This method executes the following steps in order to populate the summary array:

• Call getASample(), which return the X and Y coordinates for the robot, based on its pose while collecting the sample
• Populate X, Y, Direction and Removal properties for the sample in a Direction object and create a single entry in the summary array. Please note that the Direction object has a direction property too. 
• Repeat the above steps for 180 times (The figure 180 samples collection, is coined through a bit of trial and error). You guessed it right, there will be 180 entries in the summary array at the end of the traversing the entire path for the first time. Each entry will be of type Direction object 

getASample(): Steers the entire path with the PID controller and samples the path after each 10 milli seconds. It does such sampling 10 times, gets the X and Y position at each sample. It averages X and Y over these 10 samples. It puts the X and Y values in the sample array. So, by this time you should know that each sample consists of only 2 elements, the X and Y coordinates of robot's pose (averaged over 10 samples). 

getDirection(): This method decides the direction in which the robot is moving as per the following figure. 

This method assigns the following numbers with the directions:

North = 1

East = 2

South = 3

West = 4

Below is the algorithm for setting up the direction variable

• The input to this method is the X and Y coordinates of the current pose and its previous pose. 
• dx is difference between the X coordinate of the current pose and X coordinate of previous pose
• dy is the difference between Y coordinate of current pose and Y coordinate of previous pose
• I have set direction of movement from South to North to be when the line passing through the two poses of the robot has a slope less than 1 and dx is greater than 0
• I have set direction of movement from North to South to be when the line passing through the two poses of the robot has a slope less than 1 and dx is less than 0
• I have set direction of movement from East to West to be when the line passing through the two poses of the robot has a slope greater than 1 and dy is greater than 0
• I have set direction of movement from West to East to be when the line passing through the two poses of the robot has a slope greater than 1 and dy is less than 0

summarize(): This method executes the following crucial steps:

• Out of 180 summary samples, remove all the summary samples where direction = 0. That means neither of East, West, North, South is associated with it
• Out of the remaining summary samples, consider only those where consecutive samples does NOT have the same direction. That means I am considering only those consecutive summary elements with changes in the direction to be on the shortest path. 

findShortedPath(): As mentioned in the summarize() method above, the shortest path list takes all the summary samples which represent change in the direction, as an input. Then it removes all the samples where the previous three direction changes look like 4-1-2 in a sequence corresponding to West-North-East. This sequence represent the U turn (which is little delayed turn) which the robot takes at the dead end. I have come up with this sequence removal after observing the logs which I print in the program. 

followShortestPath(): As we know now, that the shortest path constitutes the changes in the direction of the robot. To follow the path, consider the following algorithm. 

• Collect the first three samples from pilot1 for the second phase. 
• Don't take any action until the three samples are aligned in the same direction as shortest path. Just keep following the path. 
• The moment the direction of the three samples get misaligned, start taking a turn. Then stabilize until the stabilization count gets decremented to zero. 
• After mis-aligning and consequent stabilization, if the three consecutive samples get aligned with the next index of the shortest path, increment the shortest path index to point to the next direction.
• Then start with the next three samples of pilot1 and repeat the same procedure again!   

NOTE: The number three for three consecutive samples while following the shortest path is considered after some trial and error. We can even go for comparing two samples or four samples. However, two samples will make the algorithm more error prone. And four samples will create some delay in processing.  

Line Follower with LeJOS DifferentialPilot and PIDController classes

Below is the program which makes use of LeJOS DifferentialPilot and PIDController classes to make a line follower. The underlying PID Controller concept is the same as explained in the article Line Follower with PID Controller. The appropriate values of Kp, Ki and Kd are set in the program. Higher values of Ki makes the robot unstable in line following (makes it go round and round around itself). Lower values of Kd gives a delayed response when following sharp runs (like a right/left/U turn). Higher values of Kp makes the robot follow the line in a shaky manner. The video clip shows the performance of the program.

Let me explain the Cruiser class of the LeJOS program below. The core classes, their attributes and methods used in the Cruiser class are explained in the LeJOS APIs at the below links:

• DifferentialPilot
• OdometryPoseProvider
• PIDController

The program starts with setting the parameters - wheel diameter, track width, left motor, right motor and reverse setting which are required for DifferentialPilot to function. The OdometryPoseProvider is then initialized with DifferentialPilot class object passed to its constructor, in order to track the pose of the robot in the 2D-space. The pose represents the current location and the heading of the robot. For the pose, the distance travelled by the robot is measured in Pilot units (that means if we have set the wheel diameter and track width in inches, we will get the distance travelled in inches. If we have set them in centimeters, we will get the distance in centimeters). The heading angle is measured in degrees. The pose readings are required if we want to plot the trajectory of the robot. An example of expected trajectory is given below. 

     

After feeding the instantaneous pose readings into a graph plotter (like plotly), we get to see the actual trajectory as follows:

To take a look at good in-place U-Turns at the dead ends, please take a look at below video. 

In our example of Line Follower, the pose readings do not exactly match with the actuals after the robot performs U-turns at the dead-ends. If we apply certain correction for in-place U-Turns, we may be able to get the exact trajectory. However, I have not applied any correction for this plot but we do get an idea of the trajectory that the robot is following. More concrete use of the pose, will be explained in the Maze Solver article. For now, in the Cruiser, we need to set the travel speed and the rotate speed of the robot. Travel speed is in the wheel-diameter unit per second and the rotate speed is in degrees per second. Further, we initialize the PIDController constructor with the set-point (in our case, the threshold value), set the required PID parameters and pass the instantaneous value of the light intensity to the doPID () method. The Kp, Ki and Kd needs to be tuned appropriately in order for the robot to follow the black line over white surface. 

NOTE: It would serve great help if you read the below article before going through code given in current article. That will make you familiar with the calibration concept. 

Line Follower with PID Controller