package com.cos399.goalfind;
import java.io.IOException;
//hardware imports
import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.Sound;
import lejos.nxt.addon.ColorHTSensor;
import lejos.robotics.RegulatedMotor;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.geom.Point;
import lejos.util.PilotProps;
import lejos.nxt.comm.RConsole;
public class GoalFindOne {
//Control wheel speed of pilot
private short wheelSpeed = 10;
private short pilotReverse = 1;
//Moves n units on the sides
private short travelDistance = 4;
//left and right rotate degrees
private short rightRotate = 91;
private short leftRotate = -91;
//boolean controllers
private boolean goalNotFound = true;
private boolean headLeft = false;
/* Integer Color Values using ColorHTSensor
*
* 0 Red
* 1 Green
* 2 Blue
* 3 Yellow
* 4 Magenta
* 5 Orange
* 6 White
* 7 Black
* 8 Pink
* 9 Gray
* 10 Light Gray
* 11 Dark Gray
* 12 Cyan
*/
//integer color values from sensor
private short startColorNum = 5;
private short goalColorNum = 4;
private short borderColorNum = 7;
private long startTime, goalFindTime;
//important global variables
private ColorHTSensor cs;
private DifferentialPilot pilot;
private RegulatedMotor leftMotor, rightMotor;
private float xAxis, yAxis;
private Point startPoint, intermediatePoint;
private String colorNames[] = {"RED", "GREEN", "BLUE", "YELLOW", "MAGENTA",
"ORANGE", "WHITE", "BLACK", "PINK", "GRAY", "LIGHT GRAY", "DARK GRAY",
"CYAN"};
//zero argument constructor, initializes important variables
public GoalFindOne() throws IOException{
PilotProps pp = new PilotProps();
pp.loadPersistentValues();
//initialize and set pilot properties
float wheelDiameter = Float.parseFloat(pp.getProperty(
PilotProps.KEY_WHEELDIAMETER, "2.25"));
float trackWidth = Float.parseFloat(pp.getProperty(
PilotProps.KEY_TRACKWIDTH, "6.6875"));
leftMotor = PilotProps.getMotor(pp.getProperty(
PilotProps.KEY_LEFTMOTOR, "C"));
rightMotor = PilotProps.getMotor(pp.getProperty(
PilotProps.KEY_RIGHTMOTOR, "A"));
boolean reverse = Boolean.parseBoolean(pp.getProperty(
PilotProps.KEY_REVERSE,"true"));
//initialize colorHTsensor and bind to port 1
cs = new ColorHTSensor(SensorPort.S1);
//initialize DifferentialPilot object with standardized properties
pilot = new DifferentialPilot(wheelDiameter, trackWidth, leftMotor,
rightMotor, reverse);
}
/* Start of goal finder. Creates a new Pose object with 0, 0 as X-Y
* coordinates and 0 degrees as heading. For this project, 0 degrees will
* be considered the left horizontal bearing
*/
private void execute(){
System.out.println("Any button to\nstart");
Button.waitForAnyPress();
LCD.clear();//clear the NXT display screen
pilot.setTravelSpeed(wheelSpeed);
//RConsole.open();
findCorner();
criscross();
}
/* Locates start position by going forward until reaching black edge, then
* execute a left rotation until another black edge is reached. This should
* position robot at lower left corner to begin systematic search. */
private void findCorner(){
LCD.clear();
LCD.drawString("Find Corner" , 0, 2);
//boolean controllers for first and second legs of finding corner
boolean firstLeg = true;
boolean secondLeg = false;
leftMotor.resetTachoCount();
rightMotor.resetTachoCount();
startTime = System.currentTimeMillis();
//RConsole.println("Set tachs to 0 and set start time");
while(firstLeg == true){
pilot.backward();
/* black edge was reached & left turn was made. Save distance from
* start point to border in placeholder Point, execute rotation and
* reset the tach counter */
if(cs.getColorID() == borderColorNum){
//RConsole.println("Border Found");
pilot.stop();
intermediatePoint = new Point(leftMotor.getTachoCount(), 0);
//RConsole.println(intermediatePoint.getX() + " " + intermediatePoint.getY());
pilot.travel(pilotReverse);
pilot.rotate(leftRotate);
resetTachCount();
firstLeg = false;
secondLeg = true;
}
}
while(secondLeg == true){
pilot.backward();
/* 2nd black edge was reached & direction was reversed. Calculate
* start point location from intermediate point and current tach
* reading. Robot is now at (0,0) of a 2D Cartesian graph and
* search is now ready to begin. */
if(cs.getColorID() == borderColorNum){
pilot.stop();
//RConsole.println("Corner Found");
startPoint = new Point((float)intermediatePoint.getX(),
leftMotor.getTachoCount());
xAxis = 0;
yAxis = 0;
//RConsole.println("Start Coordinates are: " + startPoint.toString());
pilot.travel(pilotReverse);
pilot.rotate(leftRotate);
resetTachCount();
secondLeg = false;
}
}
}
/* Function criss-crosses the board in a methodological search for the goal.
* At border encounters, the x and y axis coordinates are updated and tach
* resets are executed as necessary. */
private void criscross(){
//RConsole.println("Cris-cross");
//While goal hasn't been found
while (goalNotFound && cs.getColorID() != goalColorNum){
pilot.backward();
LCD.clear(2);
LCD.drawString("Criss-cross", 0, 2);
LCD.drawString("Color: " + Integer.toString(cs.getColorID()), 0, 6);
/* hit a boundary, stop and determine which side of board robot is
* based on boolean conditional and execute appropriate rotation and
* reset for another lateral pass */
if (cs.getColorID() == borderColorNum) {
pilot.stop();
LCD.drawString("Hit Border" , 0, 3);
if (headLeft){
headLeft = false;
executeRotation(leftRotate);
} else {
headLeft = true;
executeRotation(rightRotate);
}
}
/* Goal found, beep sound sequence and create end point to calculate
* bearing and distance home. */
else if (cs.getColorID() == goalColorNum ) {
pilot.stop();
Sound.beepSequence();
LCD.clear(3);
LCD.drawString("Found Goal", 0, 2);
goalNotFound = false;
goalFindTime = System.currentTimeMillis();
if (headLeft)
xAxis -= leftMotor.getTachoCount();
else
xAxis = leftMotor.getTachoCount();
returnHome(new Point(xAxis, yAxis));
}
}
}
/* Executes a return home by calculating difference between start and goal
* x and y axis points.
*
*/
private void returnHome(Point goal){
LCD.clear();
LCD.drawString("Return Home", 0, 2);
double axisDifference = 0;
//goal point is farther on x axis than start
if (startPoint.getX() > goal.getX()){
axisDifference = startPoint.getX() - goal.getX();
if (headLeft)
pilot.rotate(leftRotate);
else
pilot.rotate(rightRotate);
} else {
axisDifference = goal.getX() - startPoint.getX();
if (headLeft)
pilot.rotate(leftRotate);
else
pilot.rotate(rightRotate);
}
LCD.drawString("X-axis diff: " + Double.toString(axisDifference), 0, 4);
leftMotor.resetTachoCount();
while (leftMotor.getTachoCount() < axisDifference && cs.getColorID() != borderColorNum)
pilot.backward();
//start point is higher on the y axis than the goal
if (startPoint.getY() > goal.getY()){
axisDifference = startPoint.getY() - goal.getY();
if (headLeft)
pilot.rotate(rightRotate);
else
pilot.rotate(leftRotate);
} else {
axisDifference = goal.getY() - startPoint.getY();
if (headLeft)
pilot.rotate(leftRotate);
else
pilot.rotate(rightRotate);
}
LCD.clear(4);
LCD.drawString("Yaxis Diff: "+ Double.toString(axisDifference), 0, 4);
pilot.reset();
while (leftMotor.getTachoCount() < axisDifference && cs.getColorID() != borderColorNum)
pilot.backward();
displayTimeStats(System.currentTimeMillis());
Sound.beepSequenceUp();
pilot.rotate(1080);
}
// helper function to display runtime stats at end of project
private void displayTimeStats(long endTime){
int searchTime = (int)(goalFindTime - startTime);
int totalTime = (int)(endTime - startTime)/1000;
LCD.clear();
while (!Button.ESCAPE.isDown()){
LCD.drawString("Goal Complete!!!", 0, 0);
LCD.drawString("SensorHitBehavior Time: ", 0, 2);
LCD.drawString(Integer.toString(searchTime), 0, 3);
LCD.drawString("Total Time: ", 0, 5);
LCD.drawString(Integer.toString(totalTime), 0, 6);
}
System.exit(0);
}
//helper function to execute a rotation of int degrees and update coordinate
//location
private void executeRotation(int degrees){
pilot.travel(pilotReverse);
//hit left boundary, get tach count for X-Axis coordinateds
if (headLeft)
xAxis = 0;
else
xAxis = leftMotor.getTachoCount();
/*RConsole.println("HeadLeft: " + headLeft + "\nX-Axis Coordinate: " + xAxis +
"\nMotor Tach Count: " + leftMotor.getTachoCount());
*/
/* Rotate degrees, reset tach count. Moving forward increases Y-axis
* coordinates, and reset tach count once horizontal positioning has
* been set. */
pilot.rotate(degrees);
resetTachCount();
pilot.travel(travelDistance);
//RConsole.println("Yaxis Before: " + yAxis + "\nYaxis Tach Count: " + Math.abs(leftMotor.getTachoCount()));
yAxis += Math.abs(leftMotor.getTachoCount());
//RConsole.println("Yaxis After: " + yAxis);
pilot.rotate(degrees);
resetTachCount();
/*RConsole.println("Rotation at coordinates (" + xAxis + ", " + yAxis + ")" +
"Motor Tach Count: " + leftMotor.getTachoCount() + "");
*/
pilot.backward();
}
//helper function to reset tachometer count.
private void resetTachCount(){
leftMotor.resetTachoCount();
rightMotor.resetTachoCount();
//RConsole.println("Motor Tach Count: " + leftMotor.getTachoCount());
}
//main method
public static void main(String[] args) throws IOException{
GoalFindOne goalFinder = new GoalFindOne();
goalFinder.execute();
}
}