Minor changes to stop robot from getting stuck in a corner

src/nxt/BothLightsBehavior.java

@@ -15,7 +15,7 @@ public BothLightsBehavior(SensorPort port, SensorPort port2) {
 	}
 
 	public boolean takeControl() {
-		System.out.println("BOTH LIGHTS BEHAVIOR");
+		//System.out.println("BOTH LIGHTS BEHAVIOR");
 		if ((leftLight.readValue() < dark && rightLight.readValue() < dark) || CircleBot.winflag == true) {
 			return true;
 		}

src/nxt/CircleBot.java

@@ -7,6 +7,8 @@
 
 public class CircleBot {
 	public static boolean winflag = false;
+	public static int moved = 0;
+
 	public static void main(String [] args) {
 		Behavior b1 = new WallBehavior(SensorPort.S3);
 		Behavior b2 = new ForwardBehavior();

src/nxt/ForwardBehavior.java

@@ -8,19 +8,21 @@ public class ForwardBehavior implements Behavior{
 
 	private boolean suppressed = false;
 
+
 	public boolean takeControl() {
-		// maybe should be !winFlag??
 		return true;
 	}
 
 
 	public void action() {
 		suppressed = false;
+
 		Motor.C.setSpeed(800);
 		Motor.A.setSpeed(800);
 		Motor.A.forward();
 		Motor.C.forward();
 
+
 		while ( !suppressed ){
 			Thread.yield();
 		}

src/nxt/LeftLightBehavior.java

@@ -14,8 +14,8 @@ public LeftLightBehavior(SensorPort port, SensorPort port2) {
 		rightLight = new LightSensor(port2);
 	}
 	public boolean takeControl() {
-		System.out.println("LEFT BEHAVIOR");
-		//take control if leftLight < dark
+		//System.out.println("LEFT BEHAVIOR");
+
 		if (leftLight.readValue() < dark && rightLight.readValue() > dark){
 			return true;
 		}

src/nxt/RightLightBehavior.java

@@ -15,7 +15,7 @@ public RightLightBehavior(SensorPort port, SensorPort port2) {
 	}
 	public boolean takeControl() {
 		//take control if leftLight < dark
-		System.out.println("RIGHT BEHAVIOR");
+		//System.out.println("RIGHT BEHAVIOR");
 		if (rightLight.readValue() < dark && leftLight.readValue() > dark){
 			return true;
 		}

src/nxt/WallBehavior.java

@@ -6,47 +6,65 @@
 
 public class WallBehavior implements Behavior{
 
-	int winflag = 0;
+	boolean lastMoveTiny = false;
+	boolean turn = false;
 
 	private TouchSensor touch; 
 	public WallBehavior(SensorPort port1) {
-		
+
 		touch = new TouchSensor(port1);
-		
+
 	}
 
 	private boolean suppressed = false;
-	
+
 	public boolean takeControl() {
 		if (touch.isPressed())
 			return true;
 		return false;
-		
+
 	}
 
-	
+
 	public void action() {
-		// Back up
-		// Turn yourself around
 		suppressed = false;
+		CircleBot.moved = Motor.A.getTachoCount();
+		if (CircleBot.moved < 3000 && CircleBot.moved > 10){
+			lastMoveTiny = true;
+		}
+		else {
+			lastMoveTiny = false;
+		}
+
+		// Back up
 		Motor.A.resetTachoCount();
 		while (Motor.A.getTachoCount() > -250) {
 			Motor.A.backward();
 			Motor.C.backward();
 		}
-
-		Motor.A.resetTachoCount();
-		while (Motor.A.getTachoCount() < 1000) {
-			Motor.A.forward();
-			Motor.C.backward();
+
+		// Turn yourself around
+		if (turn){
+			Motor.A.resetTachoCount();
+			while (Motor.A.getTachoCount() < 975) {
+				Motor.A.forward();
+				Motor.C.backward();
+			}
+		}
+		else {
+			Motor.C.resetTachoCount();
+			while (Motor.C.getTachoCount() < 900) {
+				Motor.C.forward();
+				Motor.A.backward();
+			}
+		}
+		if (!lastMoveTiny){
+			turn = !turn;
 		}
 
-//		Motor.A.forward();
-//		Motor.C.forward();
-
 	}
 
-	
+
 	public void suppress() {
 		suppressed = true;
 	}

src/nxt/WinBehavior.java

@@ -19,17 +19,13 @@ public WinBehavior(SensorPort port, SensorPort port2) {
 
 	@Override
 	public boolean takeControl() {
-		System.out.println("WIN BEHAVIOR");
 		return CircleBot.winflag;
 	}
 
 	@Override
 	public void action() {
 		suppressed = false;
-
-//		while(!Motor.A.isMoving() && !Motor.B.isMoving()) {
-//			;
-//		}
+
 		while ((leftLight.readValue() < dark || rightLight.readValue() < dark)) {
 			Motor.A.lock(100);
 			Motor.C.lock(100);