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