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

Line Follower with PID Controller

I am sharing the leJOS line follower program with PID controller. Here is the reference literature for PID controller. As we all know the PID Controller works based on continuous feedback from the system relative to a given set-point. If the system moves away from the set-point, the feedback is provided such that, it will return back to the set-point. The PID algorithm works so as to minimize the error between the actual and the set-point. In case of the Line-Follower program below, the system is the Line-Follower itself. The set-point is the light intensity which the system has to follow in order to continue moving on the black line and should not lose the track. In our case, we define the set-point as the average light intensity of black and white colors. We call the set-point as threshold. The way of configuring the threshold being average = (black + white)/2, the line-follower follows the edge of the black stripe placed on the white paper. I have put two perpendicular stripes for the line follower as a challenge, to know how precisely it can follow the turn. (It is relatively easy for the line follower to follow smooth curves. But most of the time in real-life scenario, we do not get smooth turns. We encounter perpendicular sharp turns). The other challenge was to find out how effectively the line-follower faces a dead-end. Hence in the below small experiment, I have incorporated two perpendicular turns and two dead-ends with black stripes. You can imagine it as a road, on which the car is moving with a relatively slow speed. The speed is kept slow in order for the Line-Follower not to lose its track. We can even increase the speed and try the same experiment, but let us keep that for some other time. 

Now let us look at the digitized-PID controller algorithm which I have incorporated in the LeJOS program below. The set-point is the threshold as described in the above paragraph. The feedback is provided in the form of error, which is the difference between the actual-light-intensity at any given instance and the threshold value. If the actual-light-intensity of the robot is more towards black or more towards white, the system will pull it back towards the threshold, i.e. the edge of the line. In the below digitized-PID algorithm, we convert the error, its integral and its derivative into power value which will be supplied to the right and left motors. The power will be supplied to the motors such that, if the car is too much on the black surface, it will be pulled on to the white surface and vice-versa. In the digitized version of the integral error, the past error value is added to the current error value iteratively. While the digitized version of derivative, the last error is subtracted from the current error iteratively. Please take a look into the following three lines of code for illustration, where color represents the current light intensity based on the robot's position on the black or white surface.

  

The correction variable represents the calculated value of power, where error term gets multiplied by Kp (the proportional coefficient), the integral term gets multiplied by Ki (the integral coefficient) and the derivative term gets multiplied by Kd (the derivative coefficient). The Kp, Ki and Kd need to be tuned in order for the Line-Follower to follow the line appropriately with a certain speed. In my case the default power of the Line-Follower is set to 20. This value is based on trial and error in order for the speed to be relatively slow. Below is the video clip to show the performance of the program for given kp, ki and kd parameters.  

NOTE - In order for the Calibration details of Black and White intensities, it would better to take a look at my Line Follower program first.

Observations: The program seems to be following the perpendicular turn well with the current Kp, Kd and Ki coefficients and the given speed. However, it does not take the U turn at the dead-end very well. For taking an in-place U turn there is a need for more fine-tuning of PID coefficients. To know about how a better in-place U-turn looks like, please take a look at my video In-Place U turn. I have not yet concluded anything about the 'effect of tuning the parameters' over the line follower. However, that would be reserved for the future. 

I hope you liked this article!

Omni Biped

Below is the Java program for the Omni Biped robot (one creation from Daniele Benedettelli's book 'Creating Cool Mindstorm's NXT Robots'). The main design of the program is inspired by the author's Single Task 'C' program given in the book. The robot walks forward until it encounters an obstacle in front of it. As soon as it encounters an object, it takes a turn until there is no further obstacle on its way. 

Let me explain about the program a bit. The program has two classes, OmniBiped and NXTUtils. The OmniBiped program has the main method which begins with calling the init() method. The init() method resets the tachometer count and realigns the legs. In Lego Mindstorms NXT, units of measurements used are in tachometer counts rather than rotation or degrees. One pulse of tachometer is equal to one degree. To complete one full step, the robot requires 3 complete rotations of the motor. That means it requires 1080 tacho-counts (or 1080 degress). Whereas to complete half a step, the robot requires 540 tacho-counts (or 540 degrees). The realignLegs() method aligns the legs such that the initial position of the legs will be as shown in the following picture. As you should see, the 

After the initial position, the feet take half step which look like below. 

After half a step, the feet take full step which look like below. 

While realigning, the program calculates the right_count and the left_count. These counts represent the number of degrees each leg require to complete a full step, in case the robot stops in an intermediate position of legs. If this count is less than half a step distance, the robot prefers to take the respective leg backwards (because that is much quicker to achieve the realignment). In case if this count is more than half a step distance, the robot prefers to take the respective leg forward. While moving the motors forward or backwards, the tachometer count gets updated. When the right_count as well as the left_count become less than zero, the motor stops that means the re-alignment process stops. 

 

Now let us come back to the main() method. It calls the init() method and realigns the legs. Then the robot begins walking, with both the legs moving forward. That means, the motors moving the respective legs are driving forward. They keep driving forward, until an obstacle occurs at less than 24 cm distance (as defined by the constant NEAR). Once, the obstacle occurs on its way, both the motors stop, the legs are realigned again and the robot moves either left or right randomly based on the direction variable. The direction variable can be either -1 or 0 or +1 (representative of left/right direction). The robot can move in any one of these two directions until there is no obstacle within the range of 50 cm (as defined by the constant FAR). 

The robot measures the distance to the obstacle by the getAvgUltrasonicSensorReading() method in the NXTUtils class. This method averages the distance to the obstacle over 5 samples. I calculated the average in order to smoothen out the spikes in the ultrasonic sensor readings. 

Below is a small clip showing the behavior of the robot. 

  

NOTE: Please look at the commented part of the code carefully and do not consider it. 

If we analyze the movement of robot, it moves straight quite well in the beginning. It also stops when it encounters an object at less than 24 cm. Once the object encounters the robot takes a turn quite well. However the robot appears quite stuck when it approaches the cable and the wall at an angle (not exactly in front of it). When object approaches at an angle, the robot has to take multiple turning movements, which appears quite slow.  

I hope you liked this article!

Reference Sites:

Using the Tacho-Motor Class (ev3dev.org)

LeJOS installation

Robotic Arm

Below is the simple LeJOS program for making the Robotic Arm work. The LeJOS firmware for NXT can be downloaded and installed on the NXT brick from LeJOS NXJ 0.9.1. After installation of LeJOS, please load my below program on NXT brick, press the LEFT, RIGHT, UP and DOWN buttons and the Touch Sensor. See the performance. The LEFT and RIGHT buttons should move the arm to the left and right. The ENTER and ESCAPE button should move the arm up and down. When the touch sensor is pressed and if the Claw is in open condition in the past (as defined by the toggle flag), the Claw gets closed. And if the Claw is in closed condition in the past, the Claw gets opened. As you guessed it correctly, the toggle flag stores the past condition in which the Claw was and reverses it. 

Line Follower

Here is a small clip showing the Line Follower performance and the LeJOS code associated with it. The robot here is the small car which is built using small booklet provided with NXT kit. The LeJOS firmware for NXT can be downloaded and installed on the NXT brick from LeJOS NXJ 0.9.1. The design of my Java program is heavily influenced by the Line Follower in the book "Programming Lego Mindstorms with Java" by Giulio Ferrai et all. The actual algorithm to follow the line is influenced by Jacek Fedorynski's Line Follower NXC program. The LeJOS program has four classes: LFJfedor, Cruiser, LFUtils and LineValueHolder. The LFJfedor class contains the main method and is the starting point of the program. This class initializes the light sensor and calibrates the sensor for Black and White values in its constructor. The black and white values are stored inside a Java Bean called LineValueHolder. LFJfedor class also starts the thread Cruiser.  The Cruiser class has the algorithm to follow the line, given the calibrated black and white values and the light intensity at any given point in time. LFUtils class calculates the average light intensity over 20 samples, at any given point of time. The average over 20 points is calculated in order to smoothen out the spikes in the light intensity (if they exist). Let me explain some of the program details below.  

In order to calibrate the threshold for White, the NXT brick beeps twice. It prints the message "white" on the screen of the NXT brick. The user is supposed to keep the light sensor on the White part of the line follower chart and press the ESCAPE button. This process will record the White light intensity in the Java Bean  LineValueHolder. Similarly, record the Black value when the NXT beeps twice again. It prints the message "black" on the NXT screen. The user is supposed to keep the light sensor on the black strip of the line follower chart and press the ESCAPE button. This process will record the Black light intensity in the Java Bean LineValueHolder. Once the White and Black intensities are recorded, the Line Follower program works as follows:

1. The Line follower follows the edge of the line where the threshold value is (Black + White)/2
2. The instantaneous value of the light is calculated by averaging over 20 light samples
3. The B Moter and C Moter is powered in proportion to the difference between the threshold and the instantaneous value of light (defined by the variable "color")
4.  The initial value of power = 22 and the constant multiplier 50 requires few attempts to be set correctly in order to follow the line, in a certain speed

I wish you enjoyed the article!

Maximize Sound

This is a small program which I have come up with, to learn about sound sensor and data wiring in NXT. Here the robot's "roaming boundary" is decided by decreasing sound level. That means, the robot tries to remain within the boundary where raw sound level drops by 100. The robot should move away from the sound source only until the sound level drops by 100. After which the robot is supposed to take a U turn and move more closer to the sound source. Let's go through the NXT-G program to achieve this goal. Please click on the below image in order to maximize it and take a look at the NXT-G program. Alternatively, you can also download the .rbt file and check out the comments to understand the NXT program.

Let me explain all the blocks to you one by one.

1. Drag and drop the Sound Sensor block on the sequence beam in the Work Area
2. Drag and drop the Variable block and put it next to the Sound Sensor block. Name this variable as maxsound
3. Stretch a yellow wire from the Sound Sensor Block to the input of the Variable block. This will initialize the maxsound variable with the sound sensor sample
4. Drag and drop the Motor Block on to the sequence beam and configure it using the configuration panel to move the motors B and C for two seconds
5. Drag and Drop the sound sensor block to collect the sound sample (after forward movement of the robot in step 4)
6. Drag and drop the Variable block and put it next to the sound sensor block. Name this variable as sound
7. Stretch a yellow wire from the Sound Sensor Block to the input of the Variable block. This will initialize the sound variable with the sound sensor sample
8. Drag and drop the variable block maxsound on the sequence beam, and put it next to all the previous blocks
9. Drag and drop the variable block sound on the sequence beam, and put it next to all the previous blocks
10. Drag and drop the Compare block and put it next to all the previous blocks
11. Stretch yellow wires from maxsound and sound variable blocks and make them as inputs to the Compare block
12. Drag and drop a Switch block and place it on to the sequence beam
13. Draw a data wire from the output of the Compare block to the input of the Switch block. If sound is greater than maxsound, the upper part of Switch block will be executed. If the sound is lesser than maxsound, the lower part of the Switch block will be executed
14. If sound is greater than maxsound, copy the value of sound variable in maxsound variable. That will keep maxsound variable updated with the maximum value of sound so far. Do this by dragging and dropping the sound and maxsound variables on to the upper part of the switch and stretching a wire from sound output to maxsound input 
15. Next, find the difference between the peak sound reading and the latest sound reading. Check if the difference between the two sound levels is greater than 100. We can achieve this by dragging and dropping the sound and maxsound blocks on the sequence beam and putting the Maths block next to them. Stretch the yellow wires from the sound and maxsound variables to the Maths block input. Then stretch a yellow wire from the output of the Maths block to the Compare block to check if the difference between sound and maxsound is greater than 100
16. Drag and drop a Switch block and place it on to the sequence beam. The Switch block has two parts, upper and lower. The upper part gets executed if the input is True. And the lower part gets executed if the input is False. Drag a wire from the output of the Compare block to the input of the Switch block. If the output of the Compare block is true (if the difference between sound and maxsound is greater than 100), the robot will take a U turn. This can be achieved by placing a Motor Block in the upper part of the Switch with only a single Motor on. In my case its a C motor 
17. Place the steps from 4 to 16 in a loop which will execute them again and again forever.

In Essence the entire algorithm of bringing back the robot after reaching a point where sound level drops by 100 is as below:

1. Keep the maxsound variable updated with maximum sound value so far
2. Keep comparing the maxsound with the latest sound sample (The latest sound sample will keep reducing as and when the robot moves away from the sound source)
3. If the difference between the maxsound and latest sound sample is greater than 100, make the robot take a U turn
4. If not, keep the robot moving forward

Please take a look at the functioning of the robot in the below video clip. 

Hope you enjoyed the article!

Please go through the below article to know about Lego data wiring in detail.

NXT-G Data Wiring