import lejos.nxt.*;
public class MazeNavi2 // error located at line 344
{
static TouchSensor touch1 = new TouchSensor(SensorPort.S1); // touch1.isPressed() <--boolean
static TouchSensor touch2 = new TouchSensor(SensorPort.S4); // ...&& touch2.isPressed()
static LightSensor light = new LightSensor(SensorPort.S2); // light.read(Normalized)Value()
static SoundSensor sound = new SoundSensor(SensorPort.S3); // sound.readValue <--(0-100)?
/*
NEED TO REMEMBER:
Motor.A : right wheel -- rotate to turn left
Motor.C : left wheel -- rotate to turn right
*/
private static int RED; // can go left, right, or straight, should stop and poll here
private static int START_COLOR; // finishes when this color is seen
private static int END_COLOR; // reads stacked values from MemArray once this color is seen
public static final int CLAP_THRES = 70; // the threshold for a clap, this is *important*
// INPUT DATA
public static final int MAZE_WIDTH = 4;
public static final int MAZE_LENGTH = 4;
public static final int MAZE_START_W = 0;
public static final int MAZE_START_L = 0;
public static final int MAZE_START_DIR = 4; // the starting direction
public static final int MAZE_END_W = 3;
public static final int MAZE_END_L = 2;
// END INPUT DATA
private static int direction = MAZE_START_DIR;
/*
(relative to x-y coordinate plane of maze given by MAZE_START_W & MAZE_START_L)
1 : north
2 : west
3 : south
4 : east
turn left : ++
turn right : --
*/
private static boolean[][] map = new boolean[MAZE_WIDTH][MAZE_LENGTH]; // Array containing information for all of the blocks
/*
false : not been traversed
true : has been traversed
index of 0,0 : southwestern most block
*/
private static int mapIndexW = MAZE_START_W, mapIndexL = MAZE_START_L;
public static final int NUM_SQUARES = MAZE_LENGTH * MAZE_WIDTH; // the total number of squares in the maze
public static final int THRESHOLD = 20; // the threshold for seeing colors
public static final int AVG_SPEED = 200;
public static final int MAX_SPEED = 400;
public static final long PAUSE = 300;
public static final long CLAP_PAUSE_TOT = 2000; // total amount of time to wait for a clap, in ms
public static final int CLAP_REPS = 20;
// public static final double ROBO_LENGTH = 205;
public static final double CLAP_PAUSE = CLAP_PAUSE_TOT / CLAP_REPS;
public static final double BLOCK_SIZE = 279.4; // the *exact* size of one square, in mm
public static final int ADJ_RATIO = (int)BLOCK_SIZE/3;
public static final double WHEEL_CIRC = 173; // the *exact* circumference of one wheel, in mm
public static final double MM_PER_DEGREE = WHEEL_CIRC / 360;
public static final int BLOCK_DEGREES = (int)(BLOCK_SIZE / MM_PER_DEGREE)+1; // the amount of degrees to rotate to travel one square
public static final int ONEEIGHTY_TURN_DEGREES = 303; // the amount of degrees to rotate one wheel to turn at a 90 degree angle
public static final int TURN_DEGREES = 150;
private static boolean readMem = false; // should be true when END_COLOR is read, writesMem when false
private static boolean victory = false; // should be true when making a decision on which direction to go
private static boolean touched = false; // should be true when touch1.isPressed() || touch2.isPressed()
private static boolean calibrate = true;
private static boolean checkFinish = true;
private static boolean moved, noGood;
private static boolean wait;
private static int c = 0;
private static int value;
private static int x;
private static int[] memArray = new int[NUM_SQUARES]; // Array containing all of the correct decisions at areas where touch sensor is activated, or green squares
/*
0 : go straight -- should only be necessary on green squares
1 : turn right
2 : turn left
*/
private static int memIndex = 1; // index of memArray
public static void main (String[] aArg) throws Exception
{
light.setFloodlight(true);
Motor.A.regulateSpeed(true);
Motor.C.regulateSpeed(true);
//Motor.A.smoothAcceleration(true);
//Motor.C.smoothAcceleration(true);
map[MAZE_START_W][MAZE_START_L] = true;
while(calibrate)
{
if(touch1.isPressed() || touch2.isPressed())
{
value = light.readNormalizedValue();
if(c == 0)
{
RED = value;
Thread.sleep(500);
RED = RED + light.readNormalizedValue();
RED = RED / 2;
LCD.drawInt(RED, 0, 0);
LCD.refresh();
Thread.sleep(5000);
LCD.clear();
}
/*else if(c == 1)
{
END_COLOR = value;
Thread.sleep(500);
END_COLOR += light.readNormalizedValue();
END_COLOR /= 2;
LCD.drawInt(END_COLOR, 0, 0);
LCD.refresh();
Thread.sleep(5000);
LCD.clear();
}
else if(c == 2)
{
START_COLOR = value;
Thread.sleep(500);
START_COLOR += light.readNormalizedValue();
START_COLOR /= 2;
LCD.drawInt(START_COLOR, 0, 0);
LCD.refresh();
Thread.sleep(5000);
LCD.clear();
}*/
else if(c == 2)
{
LCD.clear();
calibrate = false;
}
c++; //hehe
}
}
memArray[0] = 3; // set initial value to three to prevent an array out of bounds exception
while(!Button.ESCAPE.isPressed() && !victory)
{
try{ Thread.sleep((long)PAUSE); }
catch(Exception e) {}
update();
if (!readMem) // finding the end
{
if (!touched)
{
if (light.readNormalizedValue()RED-THRESHOLD)
startDecisionSeq(0);
}
}
else // retracing back to start
{
if(!touched)
{
if (mapIndexW == MAZE_START_W && mapIndexL == MAZE_START_L)//light.readNormalizedValue()START_COLOR-THRESHOLD)
victory();
else if (light.readNormalizedValue()RED-THRESHOLD)
startRevDecisionSeq();
}
}
touched = false;
moveForward(true);
} // end of main loop
}
public static void moveForward(boolean check)
{
Motor.A.rotate(BLOCK_DEGREES, true);
Motor.C.rotate(BLOCK_DEGREES, false);
if(touch1.isPressed() || touch2.isPressed())
{
if(check)
{
touched = true;
if(!readMem)
startDecisionSeq(0);
else
startRevDecisionSeq();
}
}
else
{
if(direction == 1) // checking north
mapIndexL++;
else if(direction == 2) // checking west
mapIndexW--;
else if(direction == 3) // checking south
mapIndexL--;
else if(direction == 4) // checking east
mapIndexW++;
map[mapIndexW][mapIndexL] = true;
}
update();
if (mapIndexW == MAZE_END_W && mapIndexL == MAZE_END_L && checkFinish)//light.readNormalizedValue()START_COLOR-THRESHOLD)
{
startEndSeq();
checkFinish = false;
}
}
public static void update()
{
LCD.clear();
LCD.drawInt(light.readNormalizedValue(), 0, 0);
LCD.drawInt(mapIndexW, 0, 2);
LCD.drawInt(mapIndexL, 2, 2);
if(direction == 1) // checking north
LCD.drawString("Facing North", 0, 4);
else if(direction == 2) // checking west
LCD.drawString("Facing West", 0, 4);
else if(direction == 3) // checking south
LCD.drawString("Facing South", 0, 4);
else if(direction == 4) // checking east
LCD.drawString("Facing East", 0, 4);
if (light.readNormalizedValue()RED-THRESHOLD)
LCD.drawString("RED!", 0, 6);
LCD.refresh();
}
public static void checkDirection()
{
if(direction > 4)
direction = direction - 4;
else if(direction < 1)
direction = direction + 4;
}
public static boolean checkMap(int direction)
{
if(direction == 1 || direction == 5) // checking north
{
if(mapIndexL < MAZE_LENGTH-1)
return map[mapIndexW][mapIndexL+1];
else
return true;
}
else if(direction == 2) // checking west
{
if(mapIndexW > 0)
return map[mapIndexW-1][mapIndexL];
else
return true;
}
else if(direction == 3) // checking south
{
if(mapIndexL > 0)
return map[mapIndexW][mapIndexL-1];
else
return true;
}
else if(direction == 4 || direction == 0) // checking east
{
if(mapIndexW < MAZE_WIDTH-1)
return map[mapIndexW+1][mapIndexL];
else
return true;
}
else // something's wrong -- report error
{
LCD.drawString("Error: Invalid direction passed", 0, 6);
LCD.refresh();
try {Thread.sleep(2000);}
catch (Exception e){}
return false;
}
}
public static void startDecisionSeq(int dir)
{
if(touched)
{
readjust();
turnLeft(); // this may be what's screwing me up...
dir++;
turn(dir);
}
else
{
turn(dir);
}
}
public static void readjust()
{
Motor.A.stop();
Motor.C.stop();
Motor.A.rotate((-1*ADJ_RATIO), true);
Motor.C.rotate((-1*ADJ_RATIO), false);
}
public static void waitForClap(int dir)
{
int i=0;
wait = true;
Motor.A.stop();
Motor.C.stop();
if(dir<2)
{
while(i CLAP_THRES)
{
wait = false;
if(dir == 0)
turnLeft();
else if(dir == 1)
turnAround();
}
i++;
}
}
}
public static void turn(int dir)
{
waitForClap(dir);
if(!wait)
{
dir++;
waitForClap(dir);
if(!wait && !touched)
dir++;
memArray[memIndex] = dir;
memIndex++;
moveForward(true);
}
else if(touched) // should have already turned left, dir = 1
{
moveForward(false);
if(touch1.isPressed() || touch2.isPressed()) // if this way's no good
{
readjust();
turnAround();
dir++; // it should equal 2 now
moveForward(false);
if(touch1.isPressed() || touch2.isPressed()) // if this way's no good, either...dead end?
{
readjust();
escape();
}
else
memArray[memIndex] = dir;
memIndex++;
}
else // that worked...i guess
{
memArray[memIndex] = dir;
memIndex++;
}
}
else // it was a red square...what to do...dir = 0
{
moved = false;
if(!checkMap(direction)) // have I been here before..?
{
moveForward(false); //// <----THIS IS THE CODE THAT MESSED ME UP, it detects a red square, then moves forward if it can, then makes the universal moveForward(true) call built into the end of the loop (line 156), thus skipping every second red square.
moved = true;
noGood = false;
}
else noGood = true;
if((touch1.isPressed() || touch2.isPressed()) || noGood) // if this way's no good
{
if(moved)
readjust();
moved = false;
turnLeft();
dir++; // now it's 1
if(!checkMap(direction)) // have I been here before..?
{
moveForward(false);
moved = true;
noGood = false;
}
else noGood = true;
if((touch1.isPressed() || touch2.isPressed()) || noGood) // if this way's no good
{
if(moved)
readjust();
moved = false;
turnAround();
dir++; // now it's 2
if(!checkMap(direction)) // have I been here before..?
{
moveForward(false);
moved = true;
noGood = false;
}
else noGood = true;
if((touch1.isPressed() || touch2.isPressed()) || noGood) // if this way's no good, either...dead end?
{
if(moved)
readjust();
moved = false;
escape();
}
else // that worked...i guess
{
memArray[memIndex] = dir;
memIndex++;
}
}
else // that worked...i guess
{
memArray[memIndex] = dir;
memIndex++;
}
}
else // that worked...i guess
{
memArray[memIndex] = dir;
memIndex++;
}
}
}
public static void escape()
{
int dir;
readMem = true;
touched = false;
turnRight(); // last resort pos is always right, so turn right again to turn around
moveForward(false);
while(readMem)
{
if(touch1.isPressed() || touch2.isPressed())
{
touched = true;
startRevDecisionSeq(); // face the direction from which you came
turnAround();
if(!checkMap(direction+1) && readMem) // check left
{
turnLeft();
dir = 1;
moveForward(false);
if(touch1.isPressed() || touch2.isPressed())
{
readjust();
turnRight();
}
else
{
memIndex++;
memArray[memIndex] = dir;
readMem = false;
}
}
if(!checkMap(direction-1) && readMem) // check right
{
turnRight();
dir = 2;
moveForward(false);
if(touch1.isPressed() || touch2.isPressed())
{
readjust();
turnLeft();
}
else
{
memIndex++;
memArray[memIndex] = dir;
readMem = false;
}
}
if(readMem)
{
turnRight();
escape();
}
}
else if (light.readNormalizedValue()RED-THRESHOLD)
{
startRevDecisionSeq(); // face the direction from which you came
turnAround();
if(!checkMap(direction) && readMem) // check ahead
{
dir = 0;
moveForward(false);
if(touch1.isPressed() || touch2.isPressed())
{
readjust();
}
else
{
memIndex++;
memArray[memIndex] = dir;
readMem = false;
}
}
if(!checkMap(direction+1) && readMem) // check left
{
turnLeft();
dir = 1;
moveForward(false);
if(touch1.isPressed() || touch2.isPressed())
{
readjust();
turnRight();
}
else
{
memIndex++;
memArray[memIndex] = dir;
readMem = false;
}
}
if(!checkMap(direction-1) && readMem) // check right
{
turnRight();
dir = 2;
moveForward(false);
if(touch1.isPressed() || touch2.isPressed())
{
readjust();
turnLeft();
}
else
{
memIndex++;
memArray[memIndex] = dir;
readMem = false;
}
}
if(readMem)
{
turnRight();
escape();
}
}
touched = false;
moveForward(false);
}
}
public static void startEndSeq()
{
if(touched)
readjust();
else
{
//Motor.A.rotate(BLOCK_DEGREES/2, true);
//Motor.C.rotate(BLOCK_DEGREES/2, false);
}
if(touch1.isPressed() || touch2.isPressed())
{
readjust();
}
for(int i=0; i<2; i++)
{
Sound.playTone((int)(501+Math.random()*1000), 100);
light.setFloodlight(false);
try{ Thread.sleep((long)500); }
catch(Exception e) {}
light.setFloodlight(true);
try{ Thread.sleep((long)500); }
catch(Exception e) {}
}
turnAround();
readMem = true;
}
public static void startRevDecisionSeq()
{
if(touched)
readjust();
else
{
//Motor.A.rotate(BLOCK_DEGREES/2, true);
//Motor.C.rotate(BLOCK_DEGREES/2, false);
}
if(memArray[memIndex] == 0) // go straight
memIndex--;
else if(memArray[memIndex] == 1) // turn right
{
turnRight();
memIndex--;
}
else if(memArray[memIndex] == 2) // turn left
{
turnLeft();
memIndex--;
}
else if(memArray[memIndex] == 3) // array out of bounds...assume victory
{
victory();
}
}
public static void turnRight()
{
Motor.A.stop();
Motor.C.stop();
Motor.C.rotate(TURN_DEGREES, true);
Motor.A.rotate(-1*TURN_DEGREES, false);
direction--;
checkDirection();
update();
}
public static void turnLeft()
{
Motor.A.stop();
Motor.C.stop();
Motor.A.rotate(TURN_DEGREES, true);
Motor.C.rotate(-1*TURN_DEGREES, false);
direction++;
checkDirection();
update();
}
public static void turnAround()
{
Motor.A.stop();
Motor.C.stop();
Motor.C.rotate(ONEEIGHTY_TURN_DEGREES, true);
Motor.A.rotate(-ONEEIGHTY_TURN_DEGREES, false);
direction = direction + 2;
checkDirection();
update();
}
public static void victory()
{
Motor.A.stop();
Motor.C.stop();
victory = true;
while(Button.ESCAPE.isPressed() == false) // victory dance
{
Sound.playTone((int)(501+Math.random()*1000), 100);
light.setFloodlight(false);
try{ Thread.sleep((long)500); }
catch(Exception e) {}
light.setFloodlight(true);
try{ Thread.sleep((long)500); }
catch(Exception e) {}
}
}
}