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:
- Extract the number of samples in the heading direction of the robot from the shortest path array
- Follow the path with PID controller pilot for the above number of samples
- Get two more samples from the pilot
- Check if the direction property of those two samples match with the next direction in the shortest path array
- If the directions match, move the shortest path array pointer to the next direction and continue with step 1
- 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
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.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| 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; | |
| } | |
| } | |
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| 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; | |
| } | |
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| 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; | |
| } | |
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| 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; | |
| } | |
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| 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(); | |
| } | |
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| 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; | |
| } | |
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| 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(); | |
| } | |
| } |
