Lego NXT - Lizard

This Lizard is a creation from the book "The Unofficial LEGO MINDSTORMS NXT 2.0 Inventor's Guide, 2nd Edition" by Laurens Valk and David J. Perdue. Below is a video clip demonstrating the "Walking", "Object Detection" and "Turning" functionalities of the Lizard for the code snippet provided in LeJOS.

Let me explain the code functionality. The code has a single class MovLizard which has the main method.  The main method is responsible for Walking, Object Detection and Turning functions of the Lizard. However before explaining the code, I need to explain some important points of the Lizard's assembly which matter for our software.

• The Lizard is built with two touch sensors which are connected to its legs assembly such that when each of the touch sensors gets pressed the front and back legs get synchronized. By synchronized, I mean the front and back legs coordinate with each other very well. The front-right and back-left legs move forward at a time. And front-left and back-right legs move forward the next time.
• The Lizard is also built with an ultrasonic sensor which allows it to detect an object placed in front of it.

The main program starts with an initWalk function which invokes synchronize function. The synchronize function rotates motor A and motor B until the touch sensor A and touch sensor B gets pressed respectively. The motors stop when the touch sensor gets pressed. The initWalk function then rotates motor B to make the touch sensor B un-pressed and get the Lizard to start walking.

The main program then starts both the motors A and B in a continuous while loop. Apart from running the motors the while loop also checks whether there is an object within a distance of 30 cm. If an object is detected, the Lizard synchronizes itself again. It takes an approximate 90 degrees turn by stopping motor A and keeping other motor B running for around 25 seconds. If we want an exact 90 degrees turn, we can also attach a gyro-sensor with the Lizard assembly. However, currently there is no gyro-sensor on the lizard. So, once the turn is completed, the initWalk method is called again to synchronize the legs. The Lizard changes its course of movement and starts walking in a direction 90 degrees to the previous one. 

   

	import lejos.nxt.Button;
	import lejos.nxt.MotorPort;
	import lejos.nxt.NXTRegulatedMotor;
	import lejos.nxt.SensorPort;
	import lejos.nxt.TouchSensor;
	import lejos.nxt.UltrasonicSensor;
	import lejos.util.Delay;
	public class MovLizard {
	public static void main(String[] args) {
	// TODO Auto-generated method stub
	NXTRegulatedMotor motora = new NXTRegulatedMotor(MotorPort.A);
	NXTRegulatedMotor motorb = new NXTRegulatedMotor(MotorPort.B);
	TouchSensor sensora = new TouchSensor(SensorPort.S1);
	TouchSensor sensorb = new TouchSensor(SensorPort.S2);
	UltrasonicSensor sensor3 = new UltrasonicSensor(SensorPort.S3);
	initWalk(motora, motorb, sensora, sensorb);
	while (!Button.ESCAPE.isPressed()) {
	motora.backward();
	motorb.backward();
	// ------------------------
	if (sensor3.getDistance() <= 30) {
	synchronize(motora, motorb, sensora, sensorb);
	turn(motora, motorb);
	initWalk(motora, motorb, sensora, sensorb);
	}
	}
	}
	private static void turn(NXTRegulatedMotor motora, NXTRegulatedMotor motorb) {
	motorb.setSpeed(360);
	motora.setSpeed(0);
	motorb.backward();
	Delay.msDelay(25000);
	}
	private static void synchronize(NXTRegulatedMotor motora,
	NXTRegulatedMotor motorb, TouchSensor sensora, TouchSensor sensorb) {
	motora.setSpeed(180);
	motorb.setSpeed(180);
	while (!sensora.isPressed() && Button.ESCAPE.isUp()) {
	motora.forward();
	}
	motora.stop();
	while (!sensorb.isPressed() && Button.ESCAPE.isUp()) {
	motorb.forward();
	}
	motorb.stop();
	}
	private static void initWalk(NXTRegulatedMotor motora,
	NXTRegulatedMotor motorb, TouchSensor sensora, TouchSensor sensorb) {
	synchronize(motora, motorb, sensora, sensorb);
	motorb.rotate(540);
	}
	}

view raw MovLizard.java hosted with ❤ by GitHub

Arduino - Upload sketch using Bluetooth - Notes

1. HC-05 and Arduino Connectivity for AT mode

- Connect 5v to 5v
- Connect GND to GND
- Connect Tx to pin 2
- Connect Rx to pin 3 and GND through resistors (2k and 1k) as shown below



- EN and state pins remain hanging
- Remove the 5v connection to HC-05
- Remove the connections to Pin 2 and 3 of arduino
- Connect the arduino to PC to power on using USB
- Upload the sketch below

#include <SoftwareSerial.h>

SoftwareSerial BTserial(2, 3); // RX | TX // Connect the HC-05 TX to Arduino pin 2 RX.

// Connect the HC-05 RX to Arduino pin 3 TX

char c = ' ';

void setup() {

Serial.begin(9600);

Serial.println("Arduino is ready");

Serial.println("Remember to select Both NL & CR in the serial monitor");

// HC-05 default serial speed for AT mode is 38400

BTserial.begin(38400);

}

void loop() {

// Keep reading from HC-05 and send to Arduino Serial Monitor

if (BTserial.available()) {

c = BTserial.read();

Serial.write(c);

}

// Keep reading from Arduino Serial Monitor and send to HC-05

if (Serial.available()) {

c = Serial.read();

BTserial.write(c); }

}


- Keep holding the button on HC-05
- Connect 5v to 5v, the HC-05 will go into AT mode
- Release the button on HC-05
- Reconnect hc-05 to pin 2 and 3
- Execute the following AT commands through serial monitor (make sure the baud rate is set to 9600 and line endings is set to be "Both NL & CR")

• AT+ROLE=0
• AT+POLAR=1,0
• AT+UART=115200,0,0

- Remove the USB connection to arduino

2. Setup hc-05 with PC

- Power arduino with AA cells (4.8 v)
- From the Control Panel select Add a Device.
- Add the Bluetooth module.
- Select enter pairing code option.
- Enter "1234" for the pairing code.
- At the Add a device success window, click Close.
- Device ready to use. The OS will create two serial COM ports associated with the device. Always use the one with the lower number.

3. Solder a jumper onto Bluetooth module on Pin 32

4. Build ONLY the RESET pin connections and change pin 2 and 3 connections to pin 0 and 1 respectively as shown below - keeping rest all the above circuit the same




5. Program arduino with Bluetooth.

6. References

http://www.joshuanewell.com/electronics/arduino-programming-via-bluetooth

https://create.arduino.cc/projecthub/youssef-sabaa/arduino-bluetooth-programming-shield-wireless-upload-code-1a397c

https://www.instructables.com/id/AT-Commands-for-Bluetooth-Module-HC-05-W-EN-Pin-an/

http://www.martyncurrey.com/arduino-with-hc-05-bluetooth-module-in-slave-mode/

Arduino Basic (Zig-Zag) Line Follower

Hello all, below is our first robot (A line follower), using actual electronics components. So far we were working with NXT and EV3 Line Followers, where electronics was almost hidden in the brick. We used a DIY (Do It Yourselves) kit from the local robotics store. The DIY kit looks like below.

It consists of the following components

We also purchased a switch to connect and disconnect the batteries from the rest of the circuit. Our switch looks like below. 

I am listing the specifications for the switch below just for reference:

We purchased two Infrared Object Sensors as below

The other electronics components which we used are:

1. Arduino microcontroller
2. L293D - V1 motor driver shield 

3. Four AA batteries

We assembled our car as per the instructions provided in the DIY kit. We assembled the circuit for Line-Follower as shown in the below circuit diagram. 

I am listing down the steps of the assembly below. 

• We stacked Motor-Shield over Arduino-Uno. 
• The AA batteries are placed into the battery holder provided with DIY chassis. 
• The red wire of the battery holder is soldered to I end of the switch
• The black wire of the battery holder is connected to the ground of the shield
• The O end of the switch is connected to +M of the shield. 
• The black and red wires are soldered to both the yellow DC motors (provided with the chassis) as shown below. The wires are then connected to the M3 and M4 ports of the shield. The polarity of the connections matter, in the sense that if we reverse the connections, the motors will rotate in opposite directions. As a result, our car moves in opposite direction. So we need to first decide which way we want our car to move and then make the connections. 
• To know more about the Arduino-L293D stacking and about the motor connections, please refer to video stacking-and-motor-connections. It is a great source of information. 

• We mounted both the infrared sensors below the car, such that they will be closer to the ground and hence near the black line. Hence if you see the video, you will not be able to see the IR sensors on top of the car. They get somewhat hidden. We connected Vcc and GND of both the Infrared sensors to +M and GND of the shield respectively. 
• We connected the OUT pin of the LEFT infrared sensor to pin 13 of the shield.
• We connected the OUT pin of the RIGHT infrared sensor to pin 2 of the shield. 

NOTE: With Left and Right, I mean physical mounting of the sensors on the car on the Left and Right side respectively (in the below portion of the car). The Left sensor will track the line when the robot moves anti-clockwise. While the right sensor will track the line when the robot moves clockwise. The video below shows only the clock-wise movement. In order to move the robot anti-clockwise, you will have to physically place the robot pointing to the anti-clockwise direction. Most probably, the robot will follow the inner edge of the line, when it follows the line anti-clockwise (As you must have observed that the robot follows outer edge of the line when moving clockwise). However, you might have to try and confirm it. 

Before putting the IR sensors to use, we need to calibrate them. Please use the instructions in the site IR Sensor Calibration For Line Follower. This site lists the instructions for line-follower as well as object detection. Make sure you follow the instructions for line-follower. Let me note a couple of points, this site does not mention. 

• It mentions making of the connections after the calibration section. However, we have to make sure that we will make the connections, before the calibration. That will help the LED glow based on the potentiometer setting and we can visualize it. 
• It mentions the turning ON and OFF of signal LED. However it does not show where the signal LED is located on the IR sensor board. As one might think either one of the black or white LED might be a signal LED. However, that is not the case. The signal LED is placed on the board. It lights ON or OFF based on the placement of the sensor on the white surface or the black line respectively.  

We have also used an IR receiver to receive signals from the remote control. Below is a picture for our remote control as well as the IR receiver. Please refer to the video at IR Receiver Connections With Arduino to get the IR Receiver connected with Arduino (in reality the shield) and receive signals from the remote control. 

This is the first version of our line follower program. Slowly, we have to modify it in such a way that, we do not see / feel the jerks. The current programming logic is, if the robot is currently on white surface, turn the motors to bring it on black surface. And if it is currently on the black surface, bring it on the white surface. Therefore, the robot moves in a zig-zag manner.

The code for the Line Follower is given below. 

https://github.com/swatimshah/car_framework/blob/master/line_follower.cpp
https://github.com/swatimshah/car_framework/blob/master/line_follower.h

https://github.com/swatimshah/car_framework/blob/master/my_motor.h

https://github.com/swatimshah/car_framework/blob/master/my_remote.h

We used Arduino IDE to develop the code. line_follower.cpp is the main file, which uses Arduino.h, WProgram.h as built-in libraries those are must for the program to function. The IRremote.h is a library module which needs to be imported for the program to function remotely with a remote control. Now, I will go into the details of only those header files and cpp files which are developed by me. Those are listed above. The line_follower.cpp has the while loop which keeps checking if the STOP button is pressed over the remote control. If the STOP button is not pressed, the car gets the reading from left and right IR sensor. The logic then decides the action to be performed based on the inputs scenarios: There are four distinct scenarios here:

1. Left = 1, Right = 1
2. Left = 0, Right = 0
3. Left = 1, Right = 0
4. Left = 0, Right = 1

In each of the scenario, the program takes action to rotate motors in certain way. After which when stop button is pressed, the car stops.  

 

References

IR Sensor Interfacing with Arduino - Robocraze

Adafruit Motor Shield Tutorial : 6 Steps - Instructables

Lesson 95: Using L293D 4 DC Motors Shield for Arduino UNO and Mega | Arduino Step By Step Course (youtube.com)

Robots For Fun

How to Use Infrared Remotes and Receivers on the Arduino - Ultimate Guide to the Arduino #26 (youtube.com)

Hope you will also enjoy the article and the video and let me know if you have any questions...

Arduino Wall Follower Patrolling Over Room - Using PID Controller

For this small project, I have adopted the basic concept of PID controller from the below article
http://www.inpharmix.com/jps/PID_Controller_For_Lego_Mindstorms_Robots.html

I applied this concept (for Lego Mindstorms Robot), to Arduino robot. I applied the "Line follower" formula given in the above article, to "wall follower". As per the above site, the "Line Follower" formula is:
   error = LightValue - offset        ! calculate the error by subtracting the offset
   integral = integral + error        ! calculate the integral
   derivative = error - lastError     ! calculate the derivative
   Turn = Kp*error + Ki*integral + Kd*derivative  ! the "P term" the "I term" and the "D term"
 
In case of Wall Follower, "offset" is the setpoint (The constant distance which we desire to keep between wall and robot). The LightValue (the instantaneous light sensor readings) is substituted by instantaneous ultrasonic sensor readings. The "Turn" variable can return the pwm value for arduino to vary the speed of the motor. But in my case, I am using "Adafruit Motor Shield V1" (https://learn.adafruit.com/adafruit-motor-shield/overview) which is stacked on top of arduino. This shield drives the two dc motors. I have downloaded and imported its libraries. These libraries allow us to control the speed of the motor without getting into PWM details. The speed can vary between 0 to 255. In my case, I am directly controlling the speed with the PID formula. That is

speedCorrection = Kp*error + Ki*integral + Kd*derivative;
leftWheelSpeed = 128 - speedCorrection;
rightWheelSpeed = 128 + speedCorrection;

Just for information: For the results in the below video, I am applying three PIDs. One for right turn, One for left turn and One for straight line. So I had to keep adjusting 9 parameters to tune it for patrolling the room. I began with specifying kp = 1 for straight drive. Then slowly keep on increasing or reducing kp. The robot oscillates but it follows the wall. Then I applied kd to reduce the oscillations and stabilize the robot. I have put some ki value (which is really small) just for experiment sake, but it is not causing any harm to robot. In future, there might be a possibility to calculate the PID tuning values through Neural Network or something. But I have not explored that possibility until now.

As far as my observation goes, the PID constants depend upon the geography of the surrounding area. For example, if you have 3 right and 2 left turns in the room/lab, then you have one set of PID constants. However, if you change the room where you have 2 right and 3 left turns, then you have totally different set of PID constants. The "bottom line" is, you will "have to" tune the PID for your requirements. I would suggest the following approach for tuning:

1. Just focus on getting the robot follow a straight wall first. No turns. Remove the code for right/left turns and apply it to the robot.
2. Once the straight wall is followed properly, introduce left turn. Add the left turn code.
3. Introduce right turn at the end.

Some thoughts about left turn: As per my observation, the left turn of robot totally depends on the following

• PID parameters
• The voltage
• The speed of the robot.

To reduce the speed of the robot: You can try reducing the speed at the turn. In stead of using 128 as a reference speed, use 64 as reference speed. So the equation will be

If (correction > 0 && correction > 64)
Correction = 64;
If (correction < 0 && correction < -64)
Correction = -64;

leftWheelSpeed = 64 - correction
rightWheelSpeed = 64 + correction

As per my observations: When we change the primary speed, we have to change the PID tuning parameters (Kp, Ki, Kd) also. Only then it works well. If you reduce the primary speed to half (64), try to divide Kp, Ki, Kd to half and see the changes in the movement. After observing the changes, take your own decision (regarding tuning), as per your intuition.

Voltage Variations: Regarding the "voltage", it slowly reduces, as and when you experiment with the robot. This impacts the way the robot turns.

I am sharing a video of my wall follower robot which is able to patrol all over the room successfully. It starts from the sofa and ends up near the sofa. The right turn seem to be drastic. Other than that everything seems ok. This is all using PID controller. It took nearly 500 experiments (with different PID values) over last 1.5 months to reach this stage. Each experiment took almost 10 min. This robot is very sensitive to voltage changes. If the voltage is between 5.3V to 5.6V, it works correctly. Otherwise not. I will need to design a voltage regulator to maintain constant voltage to the microcontroller and sensors. Hope you will enjoy the video.

Below is the Circuit Diagram of the Robotic Car

Please find the Wall Follower Code below.

https://gist.github.com/MindstormFan/0faab8a927a8824c9ab12d89b3ac1d05

Q & A

1. Sometimes, the robot takes a U turn instead of taking a left turn .. Why?

Answer: It is because the "correction" value overflows. When I implemented the if statement, to truncate the correction to 127 as below, the problem had vanished. I had added the following statements to resolve this issue:

if(correction > 0 && correction > 127)

correction = 127;

if(correction > 0 && correction > -127)

correction = -127;

Further to correctly follow the wall after any turn, I had inserted "initializeWall" function at appropriate places. Make sure that is added in the code properly.

Lastly, you might need to change the PID values for straight line slightly to take a left turn. As usual begin with kp, the change kd  then change ki.

2. A question about the code. My robot sees the obstacle in front of him but drives straight into it, it doesnt turn, what can cause this problem?

Answer: When my robot sees an object in front, it takes a right turn. I have put some extreme tuning parameters (like MY_Kp_Front=40 and MY_Kd_Front=20) to make the robot take a right turn. However, the same tuning parameters might not work for you, because of the "weight" of the robot and the "voltage" supplied to the robot. Here, you have to play with the turning params and come to a decision.

My robot patrols the complete room, only 30% of the time. For rest 70% of the time, it collides at one turn or the other. I am still working on this inconsistency. Meanwhile, I feel that, there has to be some "fall back mechanism" for the robot to complete the wall following. (For Example: when the robot collides at a turn, you can make it "take reverse for few seconds, change the angle and move on".)

3. Did you guess what parameter is responsible for the first move from starting position? If you dont know what I mean I will try to explain below: When I start the robot and put it in my target distance from the wall (which is 12cm) it goes nice and straight, but when I put closer or further from the wall the robot drives into it. Increasing Kd should help?

Answer: If the robot crashed into the wall or moved away from the wall (when kept little away or closer to the wall), that means error is getting amplified a lot in the beginning. Try reducing Kp (first) and increasing Kd (second) and both (third).

4.  Isint a negative number of motor speed a problem? For example if I set motor speed to "-40", will it see it as 40?

Answer: How the motor handles negative numbers, totally depends upon the implementation of its library/driver. For me, I am using Adafruit Motor Shield. It stores the speed value, in a unsigned int (0 to 256). Hence I have chosen a primary speed to be 128 and applied the PID correction on top of it. If the PID value goes more than 127, I have truncated it to be 127. Also, if the PID value is less than -127, i have truncated it to be -127. That will make "primary speed + correction"  to range from 0 to 256.

5. My robot turns left too early and he ends up stuck in the wall like this: 

Answer: You try changing the PID coefficients for Left turn. If you are following my code, I have a different set of coefficients for Left turn. Try changing the derivative coefficient (Kd) for left turn. Decrease the Kd value and see the difference. In my code, the below "Correction" equation has the PID coefficients for left turn.

//PID for left turn

int speed = 2.5 * WF_Error + 8 * WF_Derivative;

In the above equation, the Kd value is 8. Reduce it.

6. I changed from 6V (which was not enough) to 9V. After doing that I had to change speeds of course, because 255 on Maximum was too much. But after doing that my robot freaked out at left turns again. It is turning wrong when there is a short wall after obstacle. Example in the picture: 

so the red line is the path of my robot. Looks like it has no time to settle down after bigger error. I tried to change Kd for left turn, I tried to lower speed more. I have no idea how to solve this problem.

Answer: I also observed that, there is a right turn, followed by an immediate left turn. The right turn PID might be affecting left PID here. Can you try making the right turn PID values little lower (if you have made them drastically high).

Omni-Biped with LeJOS

Hello all, please find here our version of Omni-Biped (one creation from Daniele Benedettelli's book 'Creating Cool Mindstorm's NXT Robots'). We have also implemented the exact version of Omni-Biped given in the book, which can be found at the location Omni-Biped. So let me list the changes which we have made in our version of robot: 

• We have implemented our own additional hardware of the robot, which can be considered a taller upper part of the body
• We have also implemented the head, which can be moved through 180 degrees, close to human head
• We have implemented additional software which will move the head through 180 degrees 

Rest of the functions are as per the original version of the robot, which are: We can move the robot forward, backward and make it take a turn. It also moves its head around while turning. All this is done with a small Java program with LeJOS NXJ. Please find the Video clip of the robot's performance and the corresponding Java program below. 

Let me explain the LeJOS program a little. This is a simple program compared to the program inspired by single-Task C program from the book, which I implemented in LeJOS. We have kept this program as simple as possible, just because it is made for children of age 10 to 12. This program does not have the logic for re-align legs in the beginning or after taking a turn. However, we realized that without re-aligning legs, the robot functions almost similar to its original version. That might be because of its solid mechanical assembly. If we set both A and B motors forward, the robot moves forward. If we move the motors backwards, the robot moves backwards. If we move one motor forward and the other backwards, the robot turns. We have used motor C to rotate the head. the head rotates in 90, -180 and 90 degrees sequence. This sequence brings the head back to the original position. This sequence is repeated again and again while turning. Turning and moving the head tasks work parallelly. Please check the below Java program, load it into your NXT kit and see if you get same results. 

	package rishiomnibiped;
	import lejos.nxt.Button;
	import lejos.nxt.LCD;
	import lejos.nxt.Motor;
	import lejos.nxt.SensorPort;
	import lejos.nxt.UltrasonicSensor;
	import lejos.util.Delay;
	public class rishiomnibiped {
	public static void main(String[] args) {
	// TODO Auto-generated method stub
	Motor.A.setSpeed(300);
	Motor.B.setSpeed(300);
	Motor.A.forward();
	Motor.B.forward();
	Delay.msDelay(10000);
	Motor.A.backward();
	Motor.B.backward();
	Delay.msDelay(10000);
	Motor.A.forward();
	Motor.B.backward();
	Delay.msDelay(10000);
	while (Button.ENTER.isUp()) {
	Motor.C.setSpeed(150);
	Motor.C.rotate(90);
	Motor.C.rotate(-180);
	Motor.C.rotate(90);
	}
	}
	}

view raw OmniBiped.java hosted with ❤ by GitHub

Hope you enjoyed the article!

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.  

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.

 

	import java.util.ArrayList;
	import java.util.Random;
	import lejos.nxt.Button;
	import lejos.nxt.LCD;
	import lejos.nxt.Motor;
	import lejos.robotics.RegulatedMotor;
	import lejos.robotics.navigation.DifferentialPilot;
	import lejos.util.Datalogger;
	import lejos.util.Delay;
	import lejos.util.PIDController;
	public class Cruiser extends Thread {
	// private static final int SHORTEST_PATH_SAMPLES = 115;
	// private static final int SAMPLES_TO_IGNORE = 10;
	private static final int STABILIZATION_SAMPLES = 20;
	private static final int PILOT_ROTATE_SPEED = 30;
	private static final int PILOT_TRAVEL_SPEED = 4;
	private static final float PID_LIMIT_LOW = -150.0f;
	private static final float PID_LIMIT_HIGH = 150.0f;
	private static final float PID_KD_VALUE = 200.0f;
	private static final float PID_KI_VALUE = 0.009f;
	private static final float PID_KP_VALUE = 20.0f;
	private static final int DIFFERENCE_BETWEEN_EQUAL_SAMPLES = 11;
	private static final int U_TURN_SAMPLES = 11;
	private static final int TOTAL_SAMPLES = 530;
	// private static final float CORRECTION = 3;
	int color;
	final int threshold;
	final DifferentialPilot pilot;
	final PIDController pid;
	final TachoPoseProvider provider;
	final DifferentialPilot pilot1;
	final PIDController pid1;
	final TachoPoseProvider provider1;
	TachoPose lastTachoPose = new TachoPose();
	RegulatedMotor leftMotor = Motor.B;
	RegulatedMotor rightMotor = Motor.C;
	public Cruiser() {
	float wheelDiameter = Float.parseFloat("2.2");
	float trackWidth = Float.parseFloat("4.9");
	boolean reverse = Boolean.parseBoolean("false");
	pilot = new DifferentialPilot(wheelDiameter, trackWidth, leftMotor,
	rightMotor, reverse);
	provider = new TachoPoseProvider(lastTachoPose);
	pilotInitialize();
	threshold = (LtdMazeSolver.lvh.getBlack() + LtdMazeSolver.lvh
	.getWhite()) / 2;
	pid = new PIDController(threshold, 10);
	pidInitialize();
	// ---------------------------------------------------------------------
	pilot1 = new DifferentialPilot(wheelDiameter, trackWidth, leftMotor,
	rightMotor, reverse);
	provider1 = new TachoPoseProvider(lastTachoPose);
	pilot1Initialize();
	pid1 = new PIDController(threshold, 10);
	pid1Initialize();
	leftMotor.resetTachoCount();
	rightMotor.resetTachoCount();
	}
	private void pidInitialize() {
	pid.setPIDParam(PIDController.PID_KP, PID_KP_VALUE);
	pid.setPIDParam(PIDController.PID_KI, PID_KI_VALUE);
	pid.setPIDParam(PIDController.PID_KD, PID_KD_VALUE);
	pid.setPIDParam(PIDController.PID_LIMITHIGH, PID_LIMIT_HIGH);
	pid.setPIDParam(PIDController.PID_LIMITLOW, PID_LIMIT_LOW);
	}
	private void pilotInitialize() {
	pilot.setTravelSpeed(PILOT_TRAVEL_SPEED);
	pilot.setRotateSpeed(PILOT_ROTATE_SPEED);
	}
	private void pid1Initialize() {
	pid1.setPIDParam(PIDController.PID_KP, PID_KP_VALUE);
	pid1.setPIDParam(PIDController.PID_KI, PID_KI_VALUE);
	pid1.setPIDParam(PIDController.PID_KD, 400);
	pid1.setPIDParam(PIDController.PID_LIMITHIGH, PID_LIMIT_HIGH);
	pid1.setPIDParam(PIDController.PID_LIMITLOW, PID_LIMIT_LOW);
	}
	private void pilot1Initialize() {
	pilot1.setTravelSpeed(PILOT_TRAVEL_SPEED);
	pilot1.setRotateSpeed(PILOT_ROTATE_SPEED);
	}
	public void run() {
	LCD.clear();
	LCD.drawString("Started Cruiser", 0, 2);
	Delay.msDelay(3000);
	Datalogger logger = new Datalogger();
	ArrayList<Direction> summary = populateFirstPathSamples(logger);
	stopPilot();
	ArrayList<Direction> finalizedShortestPath = findShortedPath(
	summarize(summary, logger), logger);
	//logger.transmit();
	summary.clear();
	followShortestPath(finalizedShortestPath, logger);
	pilot1.stop();
	}
	private ArrayList<Direction> populateFirstPathSamples(Datalogger logger) {
	int mark = 0;
	float[] sampleDirection = new float[1];
	float[] sampleValue = new float[2];
	ArrayList<Direction> summaryList = new ArrayList<Direction>();
	while (!Button.ESCAPE.isPressed() && mark < TOTAL_SAMPLES) {
	sampleValue = getASample();
	Direction direction = new Direction();
	summaryList.add(direction);
	direction.setX(sampleValue[0]);
	direction.setY(sampleValue[1]);
	direction.setRemoval(1);
	if (mark > 0) {
	sampleDirection = getDirection(summaryList.get(mark).getX(),
	summaryList.get(mark).getY(), summaryList.get(mark - 1)
	.getX(), summaryList.get(mark - 1).getY(),
	logger);
	summaryList.get(mark).setDirection(sampleDirection[0]);
	}
	++mark;
	}
	for (int i = 0; i < summaryList.size(); i += 2) {
	logger.writeLog(summaryList.get(i).getY(), summaryList.get(i)
	.getX(), summaryList.get(i).getDirection(), summaryList
	.get(i).getNumberOfSamples());
	}
	return summaryList;
	}
	private void followShortestPath(ArrayList<Direction> shortestPath,
	Datalogger logger) {
	int expectedShortestPathIndex = 0;
	float[] sampleDirection = new float[1];
	float[] sampleValue = new float[2];
	ArrayList<Direction> summaryList = new ArrayList<Direction>();
	int mark = 0;
	float previousDirection = 4;
	float nextDirection = 0;
	Direction dummy = new Direction();
	dummy.setDirection(0);
	dummy.setNumberOfSamples(0);
	shortestPath.add(dummy);
	Button.ESCAPE.waitForPressAndRelease();
	// printLogTwoDiaArray(shortestPath, logger);
	pilot.reset();
	pilot1.reset();
	leftMotor.resetTachoCount();
	rightMotor.resetTachoCount();
	LCD.clear();
	Delay.msDelay(3000);
	while (!Button.ESCAPE.isPressed()
	&& expectedShortestPathIndex < (shortestPath.size() - 1)) {
	if (expectedShortestPathIndex != 0)
	previousDirection = shortestPath.get(
	expectedShortestPathIndex - 1).getDirection();
	// Sample 0
	sampleValue = getASampleForPilot1();
	Direction direction = new Direction();
	summaryList.add(direction);
	direction.setX(sampleValue[0]);
	direction.setY(sampleValue[1]);
	direction.setRemoval(1);
	direction.setDirection(previousDirection);
	++mark;
	// proceed until number of samples are in a given direction of
	// shortest path
	float driveLimit = 0;
	while (mark < shortestPath.get(expectedShortestPathIndex)
	.getNumberOfSamples() && !Button.ESCAPE.isPressed()) {
	sampleValue = getASampleForPilot1();
	Direction directionN = new Direction();
	summaryList.add(directionN);
	directionN.setX(sampleValue[0]);
	directionN.setY(sampleValue[1]);
	directionN.setRemoval(1);
	sampleDirection = getDirection(summaryList.get(mark).getX(),
	summaryList.get(mark).getY(), summaryList.get(mark - 1)
	.getX(), summaryList.get(mark - 1).getY(),
	logger);
	summaryList.get(mark).setDirection(sampleDirection[0]);
	++mark;
	}
	--mark;
	nextDirection = shortestPath.get(expectedShortestPathIndex + 1)
	.getDirection();
	LCD.drawInt((int) nextDirection, 0, 2);
	if (summaryList.get(mark).getDirection() == summaryList.get(
	mark - 1).getDirection()
	&& summaryList.get(mark).getDirection() == nextDirection
	&& summaryList.get(mark - 1).getDirection() == nextDirection) {
	++expectedShortestPathIndex;
	} else {
	boolean directionChanged = alignDirection(summaryList.get(mark)
	.getDirection(), summaryList.get(mark - 1)
	.getDirection(), nextDirection);
	++expectedShortestPathIndex;
	if (directionChanged) {
	float originalSamples = shortestPath.get(
	expectedShortestPathIndex).getNumberOfSamples();
	shortestPath.get(expectedShortestPathIndex)
	.setNumberOfSamples(
	originalSamples + STABILIZATION_SAMPLES);
	}
	}
	summaryList = new ArrayList<Direction>();
	mark = 0;
	}
	pilot1.stop();
	}
	private boolean alignDirection(float currentDirection,
	float currentDirection1, float nextDirection) {
	if (currentDirection == 1 && currentDirection1 == 1
	&& nextDirection == 2) {
	pilot1.steer(-180, -60);
	return true;
	} else if (currentDirection == 1 && currentDirection1 == 1
	&& nextDirection == 4) {
	pilot1.steer(-180, 60);
	return true;
	} else if (currentDirection == 2 && currentDirection1 == 2
	&& nextDirection == 3) {
	pilot1.steer(-180, -60);
	return true;
	} else if (currentDirection == 2 && currentDirection1 == 2
	&& nextDirection == 1) {
	pilot1.steer(-180, 60);
	return true;
	} else if (currentDirection == 3 && currentDirection1 == 3
	&& nextDirection == 4) {
	pilot1.steer(-180, -60);
	return true;
	} else if (currentDirection == 3 && currentDirection1 == 3
	&& nextDirection == 2) {
	pilot1.steer(-180, 60);
	return true;
	} else if (currentDirection == 4 && currentDirection1 == 4
	&& nextDirection == 3) {
	pilot1.steer(-180, 60);
	return true;
	} else if (currentDirection == 4 && currentDirection1 == 4
	&& nextDirection == 1) {
	pilot1.steer(-180, -60);
	return true;
	}
	else if (currentDirection == 1 && currentDirection1 == 1
	&& nextDirection == 3) {
	pilot1.steer(-200, -90);
	return true;
	} else if (currentDirection == 2 && currentDirection1 == 2
	&& nextDirection == 4) {
	pilot1.steer(-200, -90);
	return true;
	} else if (currentDirection == 3 && currentDirection1 == 3
	&& nextDirection == 1) {
	pilot1.steer(-200, -90);
	return true;
	} else if (currentDirection == 4 && currentDirection1 == 4
	&& nextDirection == 2) {
	pilot1.steer(-200, -90);
	return true;
	} else {
	return false;
	}
	}
	private void printLogTwoDiaArray(float[][] shortestPath, Datalogger logger) {
	for (int index = 0; index < shortestPath[0].length; ++index) {
	logger.writeLog(shortestPath[0][index], shortestPath[1][index],
	shortestPath[2][index], shortestPath[4][index]);
	}
	}
	private float[] getASampleForPilot1() {
	TachoPose curpose = null;
	float[] sample = { 0.0f, 0.0f };
	int loopIndex = 0;
	provider1.setPose(lastTachoPose);
	while (loopIndex < 17) {
	color = LFUtils.getAvgLightValue();
	pilot1.steer(pid1.doPID(color));
	Delay.msDelay(10);
	++loopIndex;
	}
	curpose = provider1.getPose();
	lastTachoPose = curpose;
	sample[0] = lastTachoPose.getX();
	sample[1] = lastTachoPose.getY();
	return sample;
	}
	private float[] getDirection(float x2, float y2, float x1, float y1,
	Datalogger logger) {
	float[] direction = new float[1];
	x2 = oneDecRound(x2);
	y2 = oneDecRound(y2);
	x1 = oneDecRound(x1);
	y1 = oneDecRound(y1);
	float dx = x2 - x1;
	float dy = y2 - y1;
	float slope = Math.abs(dy / dx);
	if (slope < 1) {
	if ((dx > 0))
	direction[0] = 1; // North
	else
	direction[0] = 3; // South
	} else {
	if ((dy > 0))
	direction[0] = 4; // West
	else
	direction[0] = 2; // East
	}
	return direction;
	}
	private float oneDecRound(float x) {
	// return (int)x;
	return (float) (((int) (x * 1000)) / 1000f);
	// return (((float)(Math.round(x*10)))/10);
	}
	private void stopPilot() {
	pilot.stop();
	}
	private float[] getASample() {
	TachoPose curpose = null;
	int loopIndex = 0;
	float[] sample = { 0.0f, 0.0f };
	provider.setPose(lastTachoPose);
	while (loopIndex < 15) {
	color = LFUtils.getAvgLightValue();
	pilot.steer(pid.doPID(color));
	Delay.msDelay(10);
	++loopIndex;
	}
	curpose = provider.getPose();
	lastTachoPose = curpose;
	sample[0] = lastTachoPose.getX();
	sample[1] = lastTachoPose.getY();
	return sample;
	}
	private ArrayList<Direction> summarize(ArrayList<Direction> summary,
	Datalogger logger) {
	int myIndex0 = 0;
	while (myIndex0 < summary.size()) {
	if (((Direction) summary.get(myIndex0)).getDirection() == 0) {
	summary.remove(myIndex0);
	myIndex0 = -1;
	}
	++myIndex0;
	}
	ArrayList<Direction> directionOutput = new ArrayList<Direction>();
	Direction dir = new Direction();
	dir.setX(((Direction) summary.get(0)).getX());
	dir.setY(((Direction) summary.get(0)).getY());
	dir.setDirection(((Direction) summary.get(0)).getDirection());
	dir.setRemoval(((Direction) summary.get(0)).getRemoval());
	directionOutput.add(dir);
	int i = 1;
	int directionSeries = 0;
	int numberOfSamples = 0;
	while (i < summary.size()) {
	if (directionOutput.get(directionSeries).getDirection() != ((Direction) summary
	.get(i)).getDirection()) {
	// directionOutput[3][directionSeries] = -1.0f;
	++directionSeries;
	Direction dir1 = new Direction();
	directionOutput.add(dir1);
	directionOutput.get(directionSeries).setX(
	((Direction) summary.get(i)).getX());
	directionOutput.get(directionSeries).setY(
	((Direction) summary.get(i)).getY());
	directionOutput.get(directionSeries).setDirection(
	((Direction) summary.get(i)).getDirection());
	directionOutput.get(directionSeries).setRemoval(
	((Direction) summary.get(i)).getRemoval());
	numberOfSamples = 1;
	directionOutput.get(directionSeries).setNumberOfSamples(
	numberOfSamples);
	} else {
	directionOutput.get(directionSeries).setNumberOfSamples(
	++numberOfSamples);
	}
	++i;
	}
	return directionOutput;
	}
	private float getAverage(float[] x) {
	// TODO Auto-generated method stub
	float sum = 0.0f;
	for (int i = 0; i < 1; ++i)
	sum += x[i];
	return sum / 1;
	}
	private ArrayList<Direction> findShortedPath(
	ArrayList<Direction> direction, Datalogger logger) {
	// TODO Auto-generated method stub
	ArrayList<Direction> shortestPath = direction;
	for (int index = 0; index < direction.size(); ++index) {
	logger.writeLog(direction.get(index).getX(), direction.get(index)
	.getY(), direction.get(index).getDirection(), direction
	.get(index).getNumberOfSamples());
	}
	int originalShortestPathLength = shortestPath.size();
	removeUTurn(shortestPath);
	while (true) {
	removeOppositeDirections42(shortestPath);
	removeUTurn(shortestPath);
	removeOppositeDirections24(shortestPath);
	removeUTurn(shortestPath);
	removeOppositeDirections13(shortestPath);
	removeUTurn(shortestPath);
	removeOppositeDirections31(shortestPath);
	removeUTurn(shortestPath);
	if (originalShortestPathLength == shortestPath.size())
	break;
	else
	originalShortestPathLength = shortestPath.size();
	}
	for (int index = 0; index < shortestPath.size(); ++index) {
	logger.writeLog(shortestPath.get(index).getX(),
	shortestPath.get(index).getY(), shortestPath.get(index)
	.getDirection(), shortestPath.get(index)
	.getNumberOfSamples());
	}
	return shortestPath;
	}
	private void removeOppositeDirections31(ArrayList<Direction> shortestPath) {
	// TODO Auto-generated method stub
	int originalShortestPathLength = shortestPath.size();
	int myIndex2 = 1;
	while (true) {
	while (myIndex2 < shortestPath.size()) {
	if (shortestPath.get(myIndex2 - 1).getDirection() == 3
	&& shortestPath.get(myIndex2).getDirection() == 1) {
	decideWhichOneToRemove(shortestPath, myIndex2);
	myIndex2 = 0;
	}
	++myIndex2;
	}
	removeUTurn(shortestPath);
	if (originalShortestPathLength == shortestPath.size())
	break;
	else
	originalShortestPathLength = shortestPath.size();
	}
	}
	private void removeOppositeDirections13(ArrayList<Direction> shortestPath) {
	// TODO Auto-generated method stub
	int originalShortestPathLength = shortestPath.size();
	int myIndex2 = 1;
	while (true) {
	while (myIndex2 < shortestPath.size()) {
	if (shortestPath.get(myIndex2 - 1).getDirection() == 1
	&& shortestPath.get(myIndex2).getDirection() == 3) {
	decideWhichOneToRemove(shortestPath, myIndex2);
	myIndex2 = 0;
	}
	++myIndex2;
	}
	removeUTurn(shortestPath);
	if (originalShortestPathLength == shortestPath.size())
	break;
	else
	originalShortestPathLength = shortestPath.size();
	}
	}
	private void decideWhichOneToRemove(ArrayList<Direction> shortestPath,
	int myIndex2) {
	float samplesForMyIndex2 = shortestPath.get(myIndex2)
	.getNumberOfSamples();
	float samplesForMyIndex2Minus1 = shortestPath.get(myIndex2 - 1)
	.getNumberOfSamples();
	float difference = Math.abs(samplesForMyIndex2Minus1
	- samplesForMyIndex2);
	if (difference < DIFFERENCE_BETWEEN_EQUAL_SAMPLES) {
	shortestPath.remove(myIndex2);
	shortestPath.remove(myIndex2 - 1);
	} else {
	if (samplesForMyIndex2 > samplesForMyIndex2Minus1) {
	shortestPath.get(myIndex2).setNumberOfSamples(
	samplesForMyIndex2 - samplesForMyIndex2Minus1);
	shortestPath.remove(myIndex2 - 1);
	} else {
	shortestPath.get(myIndex2 - 1).setNumberOfSamples(
	samplesForMyIndex2Minus1 - samplesForMyIndex2);
	shortestPath.remove(myIndex2);
	}
	}
	}
	private void removeOppositeDirections24(ArrayList<Direction> shortestPath) {
	// TODO Auto-generated method stub
	int originalShortestPathLength = shortestPath.size();
	int myIndex2 = 1;
	while (true) {
	while (myIndex2 < shortestPath.size()) {
	if (shortestPath.get(myIndex2 - 1).getDirection() == 2
	&& shortestPath.get(myIndex2).getDirection() == 4) {
	decideWhichOneToRemove(shortestPath, myIndex2);
	myIndex2 = 0;
	}
	++myIndex2;
	}
	removeUTurn(shortestPath);
	if (originalShortestPathLength == shortestPath.size())
	break;
	else
	originalShortestPathLength = shortestPath.size();
	}
	}
	private void removeOppositeDirections42(ArrayList<Direction> shortestPath) {
	// TODO Auto-generated method stub
	int originalShortestPathLength = shortestPath.size();
	int myIndex2 = 1;
	while (true) {
	while (myIndex2 < shortestPath.size()) {
	if (shortestPath.get(myIndex2 - 1).getDirection() == 4
	&& shortestPath.get(myIndex2).getDirection() == 2) {
	decideWhichOneToRemove(shortestPath, myIndex2);
	myIndex2 = 0;
	}
	++myIndex2;
	}
	removeUTurn(shortestPath);
	if (originalShortestPathLength == shortestPath.size())
	break;
	else
	originalShortestPathLength = shortestPath.size();
	}
	}
	private void removeUTurn(ArrayList<Direction> shortestPath) {
	int myIndex = 0;
	// float[][] appliedShortestPath = new float [4][200];
	while (myIndex < shortestPath.size()) {
	if (shortestPath.get(myIndex).getDirection() == 1
	&& shortestPath.get(myIndex).getNumberOfSamples() < U_TURN_SAMPLES) {
	shortestPath.remove(myIndex);
	myIndex = -1;
	} else if (shortestPath.get(myIndex).getDirection() == 2
	&& shortestPath.get(myIndex).getNumberOfSamples() < U_TURN_SAMPLES) {
	shortestPath.remove(myIndex);
	myIndex = -1;
	} else if (shortestPath.get(myIndex).getDirection() == 3
	&& shortestPath.get(myIndex).getNumberOfSamples() < U_TURN_SAMPLES) {
	shortestPath.remove(myIndex);
	myIndex = -1;
	} else if (shortestPath.get(myIndex).getDirection() == 4
	&& shortestPath.get(myIndex).getNumberOfSamples() < U_TURN_SAMPLES) {
	shortestPath.remove(myIndex);
	myIndex = -1;
	}
	++myIndex;
	}
	}
	}

view raw Cruiser.java hosted with ❤ by GitHub

	public class Direction {
	private float direction;
	private float x;
	private float y;
	private float removal;
	private float numberOfSamples;
	public float getDirection() {
	return direction;
	}
	public void setDirection(float direction) {
	this.direction = direction;
	}
	public float getX() {
	return x;
	}
	public void setX(float x) {
	this.x = x;
	}
	public float getY() {
	return y;
	}
	public void setY(float y) {
	this.y = y;
	}
	public float getRemoval() {
	return removal;
	}
	public void setRemoval(float removal) {
	this.removal = removal;
	}
	public float getNumberOfSamples() {
	return numberOfSamples;
	}
	public void setNumberOfSamples(float numberOfSamples) {
	this.numberOfSamples = numberOfSamples;
	}
	}

view raw Direction.java hosted with ❤ by GitHub

	public class LFUtils {
	private static final int NUMBER_OF_SAMPLES = 5;
	public LFUtils() {
	}
	public static int getAvgLightValue() {
	int sum = 0;
	for (int i = 0; i < NUMBER_OF_SAMPLES; i++) {
	sum += LtdMazeSolver.lightSensor.getLightValue();
	}
	return sum / NUMBER_OF_SAMPLES;
	}
	}

view raw LFUtils.java hosted with ❤ by GitHub

	public class LineValueHolder {
	private int white;
	private int black;
	public LineValueHolder() {
	}
	public void setWhite(int value) {
	white = value;
	}
	public int getWhite() {
	return white;
	}
	public void setBlack(int value) {
	black = value;
	}
	public int getBlack() {
	return black;
	}
	}

view raw LineValueHolder.java hosted with ❤ by GitHub

	import lejos.nxt.Button;
	import lejos.nxt.LCD;
	import lejos.nxt.LightSensor;
	import lejos.nxt.SensorPort;
	import lejos.nxt.Sound;
	public class LtdMazeSolver {
	static LineValueHolder lvh = new LineValueHolder();
	static LightSensor lightSensor = new LightSensor(SensorPort.S1);
	public LtdMazeSolver() throws InterruptedException {
	waitForUser("white");
	lvh.setWhite(getThreshold());
	waitForUser(null);
	waitForUser("black");
	lvh.setBlack(getThreshold());
	waitForUser(null);
	}
	private synchronized void waitForUser(String message)
	throws InterruptedException {
	if (message != null) {
	LCD.drawString(message, 0, 2, false);
	}
	Sound.twoBeeps();
	Button.ESCAPE.waitForPressAndRelease();
	}
	private int getThreshold() {
	LFUtils calib = new LFUtils();
	int value = calib.getAvgLightValue();
	LCD.drawInt(value, 4, 0, 3);
	return value;
	}
	private void initialize() {
	Thread cruiser = new Thread(new Cruiser());
	cruiser.start();
	}
	public static void main(String[] args) throws InterruptedException {
	LtdMazeSolver lfDiffPilotPID = new LtdMazeSolver();
	lfDiffPilotPID.initialize();
	}
	}

view raw LtdMazeSolver.java hosted with ❤ by GitHub

	public class TachoPose {
	private int _leftTC = 0;
	private int _rightTC = 0;
	private float x = 0;
	private float y = 0;
	private float heading = 0;
	private float distance = 0;
	public int getLeftTC() {
	return _leftTC;
	}
	public void setLeftTC(int _leftTC) {
	this._leftTC = _leftTC;
	}
	public int getRightTC() {
	return _rightTC;
	}
	public void setRightTC(int _rightTC) {
	this._rightTC = _rightTC;
	}
	public float getX() {
	return x;
	}
	public void setX(float x) {
	this.x = x;
	}
	public float getY() {
	return y;
	}
	public void setY(float y) {
	this.y = y;
	}
	public float getHeading() {
	return heading;
	}
	public void setHeading(float heading) {
	this.heading = heading;
	}
	public float getDistance() {
	return distance;
	}
	public void setDistance(float distance) {
	this.distance = distance;
	}
	}

view raw TachoPose.java hosted with ❤ by GitHub

	import lejos.nxt.Motor;
	import lejos.robotics.RegulatedMotor;
	public class TachoPoseProvider {
	private final RegulatedMotor _left = Motor.B;
	private final RegulatedMotor _right = Motor.C;
	private final float trackWidth = 4.9f;
	private byte _parity = 1;
	private TachoPose lastTachoPose;
	private static final float TWO_PI = 6.284f;
	public TachoPoseProvider(TachoPose tachoPose) {
	// TODO Auto-generated constructor stub
	lastTachoPose = tachoPose;
	}
	public TachoPose getPose() {
	// TODO Auto-generated method stub
	int lsamp = getLeftCount();
	int rsamp = getRightCount();
	int L_ticks = lsamp - lastTachoPose.getLeftTC();
	int R_ticks = rsamp - lastTachoPose.getRightTC();
	lastTachoPose.setLeftTC(lsamp);
	lastTachoPose.setRightTC(rsamp);
	float left_inches = (float) L_ticks / 45.0f;
	float right_inches = (float) R_ticks / 45.0f;
	float inches = (left_inches + right_inches) / 2.0f;
	float theta = lastTachoPose.getHeading()
	+ normalize(((left_inches - right_inches) / trackWidth));
	float normalizedTheta = normalize(theta);
	lastTachoPose.setY(lastTachoPose.getY()
	+ (inches * (float) Math.cos(normalizedTheta)));
	lastTachoPose.setX(lastTachoPose.getX()
	+ (inches * (float) Math.sin(normalizedTheta)));
	lastTachoPose.setHeading(normalizedTheta);
	return lastTachoPose;
	}
	float normalize(float theta) {
	float normalized = theta % TWO_PI;
	normalized = (normalized + TWO_PI) % TWO_PI;
	return normalized <= 3.142 ? normalized : normalized - TWO_PI;
	}
	public void setPose(TachoPose aPose) {
	// TODO Auto-generated method stub
	lastTachoPose = aPose;
	}
	private int getLeftCount() {
	return _parity * _left.getTachoCount();
	}
	private int getRightCount() {
	return _parity * _right.getTachoCount();
	}
	}

view raw TachoPoseProvider.java hosted with ❤ by GitHub

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.  

	import java.util.ArrayList;
	import lejos.nxt.Button;
	import lejos.nxt.LCD;
	import lejos.nxt.Motor;
	import lejos.robotics.RegulatedMotor;
	import lejos.robotics.localization.OdometryPoseProvider;
	import lejos.robotics.navigation.DifferentialPilot;
	import lejos.robotics.navigation.Navigator;
	import lejos.robotics.navigation.Pose;
	import lejos.robotics.navigation.Waypoint;
	import lejos.util.Datalogger;
	import lejos.util.Delay;
	import lejos.util.PIDController;
	public class Cruiser extends Thread {
	int color;
	final int threshold;
	final DifferentialPilot pilot;
	final PIDController pid;
	final OdometryPoseProvider provider;
	final DifferentialPilot pilot1;
	final PIDController pid1;
	final OdometryPoseProvider provider1;
	public Cruiser() {
	float wheelDiameter = Float.parseFloat("2.1");
	float trackWidth = Float.parseFloat("3.7");
	RegulatedMotor leftMotor = Motor.B;
	RegulatedMotor rightMotor = Motor.C;
	boolean reverse = Boolean.parseBoolean("false");
	pilot = new DifferentialPilot(wheelDiameter, trackWidth, leftMotor,
	rightMotor, reverse);
	provider = new OdometryPoseProvider(pilot);
	pilotInitialize();
	threshold = (LtdMazeSolver.lvh.getBlack() + LtdMazeSolver.lvh
	.getWhite()) / 2;
	pid = new PIDController(threshold, 10);
	pidInitialize();
	// ---------------------------------------------------------------------
	pilot1 = new DifferentialPilot(wheelDiameter, trackWidth, leftMotor,
	rightMotor, reverse);
	provider1 = new OdometryPoseProvider(pilot1);
	pilot1Initialize();
	pid1 = new PIDController(threshold, 10);
	pid1Initialize();
	}
	private void pidInitialize() {
	pid.setPIDParam(PIDController.PID_KP, 12.0f);
	pid.setPIDParam(PIDController.PID_KI, 0.003f);
	pid.setPIDParam(PIDController.PID_KD, 250.0f);
	pid.setPIDParam(PIDController.PID_LIMITHIGH, 250.0f);
	pid.setPIDParam(PIDController.PID_LIMITLOW, -250.0f);
	pid.setPIDParam(PIDController.PID_I_LIMITHIGH, 0.0f);
	pid.setPIDParam(PIDController.PID_I_LIMITLOW, 0.0f);
	pid.setPIDParam(PIDController.PID_DEADBAND, 1);
	}
	private void pilotInitialize() {
	pilot.setMinRadius(5.9);
	pilot.setTravelSpeed(3);
	pilot.setRotateSpeed(200);
	pilot.addMoveListener(provider);
	}
	private void pid1Initialize() {
	pid1.setPIDParam(PIDController.PID_KP, 12.0f);
	pid1.setPIDParam(PIDController.PID_KI, 0.003f);
	pid1.setPIDParam(PIDController.PID_KD, 250.0f);
	pid1.setPIDParam(PIDController.PID_LIMITHIGH, 250.0f);
	pid1.setPIDParam(PIDController.PID_LIMITLOW, -250.0f);
	pid1.setPIDParam(PIDController.PID_I_LIMITHIGH, 0.0f);
	pid1.setPIDParam(PIDController.PID_I_LIMITLOW, 0.0f);
	pid1.setPIDParam(PIDController.PID_DEADBAND, 1);
	}
	private void pilot1Initialize() {
	// pilot1.setMinRadius(25);
	pilot1.setTravelSpeed(3);
	pilot1.setRotateSpeed(30);
	// pilot1.addMoveListener(provider1);
	}
	public void run() {
	LCD.clear();
	LCD.drawString("Started Cruiser", 0, 2);
	Delay.msDelay(3000);
	Datalogger logger = new Datalogger();
	float[][] summary = populateFirstPathSamples(logger);
	stopPilot();
	float[][] finalizedShortestPath = findShortedPath(
	summarize(summary, logger), logger);
	Button.ESCAPE.waitForPressAndRelease();
	int stoppingPoint = summary[0].length - 1;
	followShortestPath(finalizedShortestPath, summary[0][stoppingPoint],
	summary[1][stoppingPoint], logger);
	logger.transmit();
	}
	private float[][] populateFirstPathSamples(Datalogger logger) {
	int mark = 0;
	float[] sampleDirection = new float[1];
	float[] sampleValue = new float[2];
	ArrayList<Direction> summaryList = new ArrayList<Direction>();
	while (!Button.ESCAPE.isPressed() && mark < 180) {
	sampleValue = getASample();
	Direction direction = new Direction();
	summaryList.add(direction);
	direction.setX(sampleValue[0]);
	direction.setY(sampleValue[1]);
	direction.setRemoval(1);
	if (mark > 0) {
	sampleDirection = getDirection(summaryList.get(mark).getX(),
	summaryList.get(mark).getY(), summaryList.get(mark - 1)
	.getX(), summaryList.get(mark - 1).getY(),
	logger);
	summaryList.get(mark).setDirection(sampleDirection[0]);
	}
	// logger.writeLog(summary[0][mark], summary[1][mark],
	// summary[2][mark], summary[3][mark]);
	++mark;
	}
	float[][] summary = new float[4][summaryList.size()];
	for (int i = 0; i < summaryList.size(); i++) {
	summary[0][i] = summaryList.get(i).getX();
	summary[1][i] = summaryList.get(i).getY();
	summary[2][i] = summaryList.get(i).getDirection();
	summary[3][i] = summaryList.get(i).getRemoval();
	}
	return summary;
	}
	private void followShortestPath(float[][] shortestPath, float stoppingX,
	float stoppingY, Datalogger logger) {
	int expectedShortestPathIndex = 0;
	float[] sampleDirection = new float[1];
	float[] sampleValue = new float[2];
	ArrayList<Direction> summaryList = new ArrayList<Direction>();
	int mark = 0;
	int stabilizationIndex = 0;
	Button.ESCAPE.waitForPressAndRelease();
	printLogTwoDiaArray(shortestPath, logger);
	pilot.reset();
	pilot1.reset();
	LCD.clear();
	Delay.msDelay(3000);
	// Sample 0
	sampleValue = getASampleForPilot1();
	Direction direction = new Direction();
	summaryList.add(direction);
	direction.setX(sampleValue[0]);
	direction.setY(sampleValue[1]);
	direction.setRemoval(1);
	++mark;
	// Sample 1
	sampleValue = getASampleForPilot1();
	Direction direction1 = new Direction();
	summaryList.add(direction1);
	direction1.setX(sampleValue[0]);
	direction1.setY(sampleValue[1]);
	direction1.setRemoval(1);
	sampleDirection = getDirection(summaryList.get(mark).getX(),
	summaryList.get(mark).getY(), summaryList.get(mark - 1).getX(),
	summaryList.get(mark - 1).getY(), logger);
	summaryList.get(mark).setDirection(sampleDirection[0]);
	++mark;
	while (!Button.ESCAPE.isPressed()
	&& expectedShortestPathIndex < shortestPath[0].length
	&& summaryList.get(mark - 1).getX() < stoppingX) {
	sampleValue = getASampleForPilot1();
	Direction directionN = new Direction();
	summaryList.add(directionN);
	directionN.setX(sampleValue[0]);
	directionN.setY(sampleValue[1]);
	directionN.setRemoval(1);
	sampleDirection = getDirection(summaryList.get(mark).getX(),
	summaryList.get(mark).getY(), summaryList.get(mark - 1)
	.getX(), summaryList.get(mark - 1).getY(), logger);
	summaryList.get(mark).setDirection(sampleDirection[0]);
	if (summaryList.get(mark).getDirection() == shortestPath[2][expectedShortestPathIndex]
	&& summaryList.get(mark - 1).getDirection() == shortestPath[2][expectedShortestPathIndex]
	&& summaryList.get(mark - 2).getDirection() == shortestPath[2][expectedShortestPathIndex]
	&& stabilizationIndex == 0)
	;
	if (summaryList.get(mark).getDirection() != shortestPath[2][expectedShortestPathIndex]
	&& summaryList.get(mark - 1).getDirection() != shortestPath[2][expectedShortestPathIndex]
	&& summaryList.get(mark - 2).getDirection() != shortestPath[2][expectedShortestPathIndex]
	&& stabilizationIndex == 0) {
	if ((expectedShortestPathIndex < (shortestPath[0].length - 1))
	&& (summaryList.get(mark).getDirection() == shortestPath[2][expectedShortestPathIndex + 1])
	&& (summaryList.get(mark - 1).getDirection() == shortestPath[2][expectedShortestPathIndex + 1])
	&& (summaryList.get(mark - 2).getDirection() == shortestPath[2][expectedShortestPathIndex + 1]))
	++expectedShortestPathIndex;
	else {
	pilot1.rotate(-35);
	stabilizationIndex = 20;
	}
	}
	// logger.writeLog(summary[0][mark], summary[1][mark],
	// summary[2][mark], summary[3][mark]);
	++mark;
	if (stabilizationIndex > 0)
	--stabilizationIndex;
	}
	pilot1.stop();
	}
	private void printLogTwoDiaArray(float[][] shortestPath, Datalogger logger) {
	for (int index = 0; index < shortestPath[0].length; ++index) {
	logger.writeLog(shortestPath[0][index], shortestPath[1][index],
	shortestPath[2][index], shortestPath[3][index]);
	}
	}
	private float[] getASampleForPilot1() {
	Pose curpose = null;
	int avgIndex = 0;
	float[] x = new float[10];
	float[] y = new float[10];
	float averageX = 0;
	float averageY = 0;
	float[] sample = { 0.0f, 0.0f };
	while (avgIndex < 10) {
	color = LFUtils.getAvgLightValue();
	pilot1.steer(pid1.doPID(color));
	curpose = provider1.getPose();
	Delay.msDelay(10);
	x[avgIndex] = curpose.getX();
	y[avgIndex] = curpose.getY();
	++avgIndex;
	}
	averageX = getAverage(x);
	averageY = getAverage(y);
	sample[0] = averageX;
	sample[1] = averageY;
	return sample;
	}
	private float[] getDirection(float x2, float y2, float x1, float y1,
	Datalogger logger) {
	float[] direction = new float[1];
	x2 = oneDecRound(x2);
	y2 = oneDecRound(y2);
	x1 = oneDecRound(x1);
	y1 = oneDecRound(y1);
	float dx = x2 - x1;
	float dy = y2 - y1;
	float slope = Math.abs(dy / dx);
	if (slope < 1) {
	if ((dx > 0))
	direction[0] = 1; // North
	else
	direction[0] = 3; // South
	} else {
	if ((dy > 0))
	direction[0] = 4; // West
	else
	direction[0] = 2; // East
	}
	logger.writeLog(x2, y2, slope, direction[0]);
	return direction;
	}
	private float oneDecRound(float x) {
	// return (int)x;
	return (float) (((int) (x * 1000)) / 1000f);
	// return (((float)(Math.round(x*10)))/10);
	}
	private void stopPilot() {
	pilot.stop();
	}
	private float[] getASample() {
	Pose curpose = null;
	int avgIndex = 0;
	float[] x = new float[10];
	float[] y = new float[10];
	float averageX = 0;
	float averageY = 0;
	float[] sample = { 0.0f, 0.0f };
	while (avgIndex < 10) {
	color = LFUtils.getAvgLightValue();
	pilot.steer(pid.doPID(color));
	curpose = provider.getPose();
	Delay.msDelay(10);
	x[avgIndex] = curpose.getX();
	y[avgIndex] = curpose.getY();
	++avgIndex;
	}
	averageX = getAverage(x);
	averageY = getAverage(y);
	sample[0] = averageX;
	sample[1] = averageY;
	return sample;
	}
	private float[][] summarize(float[][] summary, Datalogger logger) {
	ArrayList<Direction> temp = new ArrayList<Direction>();
	for (int i = 0; i < summary[0].length; ++i) {
	Direction myDirection = new Direction();
	myDirection.setX(summary[0][i]);
	myDirection.setY(summary[1][i]);
	myDirection.setDirection(summary[2][i]);
	myDirection.setRemoval(summary[3][i]);
	temp.add(myDirection);
	}
	int myIndex0 = 0;
	while (myIndex0 < temp.size()) {
	if (temp.get(myIndex0).getDirection() == 0) {
	temp.remove(myIndex0);
	myIndex0 = -1;
	}
	++myIndex0;
	}
	float[][] tempArray = new float[4][temp.size()];
	for (int j = 0; j < temp.size(); j++) {
	tempArray[0][j] = temp.get(j).getX();
	tempArray[1][j] = temp.get(j).getY();
	tempArray[2][j] = temp.get(j).getDirection();
	tempArray[3][j] = temp.get(j).getRemoval();
	}
	ArrayList<Direction> directionOutput = new ArrayList<Direction>();
	Direction dir = new Direction();
	dir.setX(tempArray[0][0]);
	dir.setY(tempArray[1][0]);
	dir.setDirection(tempArray[2][0]);
	dir.setRemoval(tempArray[3][0]);
	directionOutput.add(dir);
	int i = 1;
	int directionSeries = 0;
	while (i < tempArray[0].length) {
	if (directionOutput.get(directionSeries).getDirection() != tempArray[2][i]) {
	// directionOutput[3][directionSeries] = -1.0f;
	++directionSeries;
	Direction dir1 = new Direction();
	directionOutput.add(dir1);
	directionOutput.get(directionSeries).setX(tempArray[0][i]);
	directionOutput.get(directionSeries).setY(tempArray[1][i]);
	directionOutput.get(directionSeries).setDirection(
	tempArray[2][i]);
	directionOutput.get(directionSeries)
	.setRemoval(tempArray[3][i]);
	}
	++i;
	}
	float[][] directionOutputArray = new float[4][directionOutput.size()];
	for (int j = 0; j < directionOutput.size(); j++) {
	directionOutputArray[0][j] = directionOutput.get(j).getX();
	directionOutputArray[1][j] = directionOutput.get(j).getY();
	directionOutputArray[2][j] = directionOutput.get(j).getDirection();
	directionOutputArray[3][j] = directionOutput.get(j).getRemoval();
	}
	return directionOutputArray;
	}
	private float getAverage(float[] x) {
	// TODO Auto-generated method stub
	float sum = 0.0f;
	for (int i = 0; i < 10; ++i)
	sum += x[i];
	return sum / 10;
	}
	private float[][] findShortedPath(float[][] direction, Datalogger logger) {
	// TODO Auto-generated method stub
	ArrayList<Direction> shortestPath = new ArrayList<Direction>();
	for (int index = 0; index < direction[0].length; ++index) {
	logger.writeLog(direction[0][index], direction[1][index],
	direction[2][index], direction[3][index]);
	}
	for (int i = 0; i < direction[0].length; ++i) {
	Direction myDirection = new Direction();
	myDirection.setX(direction[0][i]);
	myDirection.setY(direction[1][i]);
	myDirection.setDirection(direction[2][i]);
	myDirection.setRemoval(direction[3][i]);
	shortestPath.add(myDirection);
	}
	int myIndex = 2;
	// float[][] appliedShortestPath = new float [4][200];
	while (myIndex < shortestPath.size()) {
	if (shortestPath.get(myIndex - 2).getDirection() == 4
	&& shortestPath.get(myIndex - 1).getDirection() == 1
	&& shortestPath.get(myIndex).getDirection() == 2) {
	shortestPath.remove(myIndex);
	shortestPath.remove(myIndex - 1);
	shortestPath.remove(myIndex - 2);
	myIndex = 1;
	}
	++myIndex;
	}
	// int originalShortestPathLength = shortestPath.size();
	//
	// int myIndex2 = 1;
	//
	// while (true) {
	//
	// while (myIndex2 < shortestPath.size()) {
	//
	// if (shortestPath.get(myIndex2 - 1).getDirection() == 4
	// && shortestPath.get(myIndex2).getDirection() == 2) {
	// shortestPath.remove(myIndex2);
	// shortestPath.remove(myIndex2 - 1);
	// myIndex2 = 0;
	// }
	//
	// ++myIndex2;
	// }
	//
	// if (originalShortestPathLength == shortestPath.size())
	// break;
	// else
	// originalShortestPathLength = shortestPath.size();
	// }
	float[][] shortestPathArray = new float[4][shortestPath.size()];
	for (int j = 0; j < shortestPath.size(); j++) {
	shortestPathArray[0][j] = shortestPath.get(j).getX();
	shortestPathArray[1][j] = shortestPath.get(j).getY();
	shortestPathArray[2][j] = shortestPath.get(j).getDirection();
	shortestPathArray[3][j] = shortestPath.get(j).getRemoval();
	}
	for (int index = 0; index < shortestPathArray[0].length; ++index) {
	logger.writeLog(shortestPathArray[0][index],
	shortestPathArray[1][index], shortestPathArray[2][index],
	shortestPathArray[3][index]);
	}
	return shortestPathArray;
	}
	}

view raw Cruiser.java hosted with ❤ by GitHub

	public class Direction {
	private float direction;
	private float x;
	private float y;
	private float removal;
	public float getDirection() {
	return direction;
	}
	public void setDirection(float direction) {
	this.direction = direction;
	}
	public float getX() {
	return x;
	}
	public void setX(float x) {
	this.x = x;
	}
	public float getY() {
	return y;
	}
	public void setY(float y) {
	this.y = y;
	}
	public float getRemoval() {
	return removal;
	}
	public void setRemoval(float removal) {
	this.removal = removal;
	}
	}

view raw Direction.java hosted with ❤ by GitHub

	public class LFUtils {
	private static final int NUMBER_OF_SAMPLES = 20;
	public LFUtils() {
	}
	public static int getAvgLightValue() {
	int sum = 0;
	for (int i = 0; i < NUMBER_OF_SAMPLES; i++) {
	sum += LtdMazeSolver.lightSensor.getLightValue();
	}
	return sum / NUMBER_OF_SAMPLES;
	}
	}

view raw LFUtils.java hosted with ❤ by GitHub

	public class LineValueHolder {
	private int white;
	private int black;
	public LineValueHolder() {
	}
	public void setWhite(int value) {
	white = value;
	}
	public int getWhite() {
	return white;
	}
	public void setBlack(int value) {
	black = value;
	}
	public int getBlack() {
	return black;
	}
	}

view raw LineValueHolder.java hosted with ❤ by GitHub

	import lejos.nxt.Button;
	import lejos.nxt.LCD;
	import lejos.nxt.LightSensor;
	import lejos.nxt.SensorPort;
	import lejos.nxt.Sound;
	public class LtdMazeSolver {
	static LineValueHolder lvh = new LineValueHolder();
	static LightSensor lightSensor = new LightSensor(SensorPort.S1);
	public LtdMazeSolver() throws InterruptedException {
	waitForUser("white");
	lvh.setWhite(getThreshold());
	waitForUser(null);
	waitForUser("black");
	lvh.setBlack(getThreshold());
	waitForUser(null);
	}
	private synchronized void waitForUser(String message)
	throws InterruptedException {
	if (message != null) {
	LCD.drawString(message, 0, 2, false);
	}
	Sound.twoBeeps();
	Button.ESCAPE.waitForPressAndRelease();
	}
	private int getThreshold() {
	LFUtils calib = new LFUtils();
	int value = calib.getAvgLightValue();
	LCD.drawInt(value, 4, 0, 3);
	return value;
	}
	private void initialize() {
	Thread cruiser = new Thread(new Cruiser());
	cruiser.start();
	}
	public static void main(String[] args) throws InterruptedException {
	LtdMazeSolver lfDiffPilotPID = new LtdMazeSolver();
	lfDiffPilotPID.initialize();
	}
	}

view raw LtdMazeSolver.java hosted with ❤ by GitHub