Added final result.

lesson10/NXT/PilotAvoid.java

+import java.io.File;
+import java.util.Vector;
+
+import lejos.geom.Point;
 import lejos.nxt.*;
+import lejos.nxt.comm.RConsole;
+import lejos.robotics.localization.OdometryPoseProvider;
 import lejos.robotics.navigation.DifferentialPilot;
 import lejos.robotics.navigation.Move;
+import lejos.robotics.navigation.Pose;
+import lejos.util.Delay;
 
 public class PilotAvoid {
 	private SlaveIOStreams PC;
@@ -14,13 +22,24 @@ public class PilotAvoid {
 
 	DifferentialPilot pilot = new DifferentialPilot(leftWheelDiameter,
 			rightWheelDiameter, trackWidth, left, right, false);
+	public OdometryPoseProvider poseProvider = new OdometryPoseProvider(pilot);
+	
+	public Point desPoint;
 
-	public PilotAvoid(boolean usb) {
+	public PilotAvoid(boolean usb, float desX, float desY) {
 		PC = new SlaveIOStreams(usb);
 		PC.open();
+		
+		// Send destination to PC.
+		PC.output(desX);
+		PC.output(desY);
 
 		pilot.setTravelSpeed(travelSpeed);
 		pilot.setRotateSpeed(rotateSpeed);
+		
+		Pose initialPose = new Pose(0,0,0);	      
+	    poseProvider.setPose(initialPose);
+		desPoint = new Point(desX, desY);			
 	}
 
 	public void sendMove(Move move) {
@@ -29,74 +48,39 @@ public class PilotAvoid {
 		PC.output(move.getAngleTurned());
 	}
 
-	public void travel(double distance) {
-		pilot.travel(distance, true);
-	}
-
-	public void rotate(double angle) {
-		pilot.rotate(angle, true);
+	public void travel(double distance, boolean immediateReturn) {
+		pilot.travel(distance, immediateReturn);
 	}
 
-	public void go() {
-		Sound.beep();
-		while (!Button.ENTER.isDown())
-			Thread.yield();
-		Sound.twoBeeps();
-
-		travel(500);
-		rotate(90);
-		travel(200);
-		rotate(-90);
-		travel(500);
-
-		/*
-		 * travel(500); rotate(90); travel(200); rotate(-90); travel(500);
-		 * rotate(90); travel(200); rotate(90); travel(700); rotate(90);
-		 * travel(200);
-		 */
-
-		while (!Button.ENTER.isDown())
-			Thread.yield();
-
-		LCD.clear();
-		LCD.drawString("Closing", 0, 0);
-		if (PC.close())
-			LCD.drawString("Closed", 0, 0);
-		try {
-			Thread.sleep(2000);
-		} catch (Exception e) {
-		}
-	}
+	public void rotate(double angle, boolean immediateReturn) {
+		pilot.rotate(angle, immediateReturn);		
+	}	
 
 	public static void main(String[] args) {
-		PilotAvoid pilot = new PilotAvoid(false);
+		PilotAvoid pilot = new PilotAvoid(false, 1000, 500);
 
 		LCD.clear();
-		LCD.drawString("PilotRouteMM", 0, 0);
-		// route.go();
+		LCD.drawString("PilotAvoid", 0, 0);
 		
 		Sound.beep();
 		while (!Button.ENTER.isDown())
 			Thread.yield();
 		Sound.twoBeeps();
 		
-		Behavior b1 = new FollowRoute(pilot);
-		Behavior b2 = new AvoidObstacle(pilot);		
-		Behavior[] behaviorList = { b1, b2};
+		Behavior b1 = new FindGoal(pilot);
+		Behavior b2 = new AvoidObstacle(pilot);
+		Behavior b3 = new RecognizeGoal(pilot);	
+		Behavior[] behaviorList = { b1, b2, b3};
 		Arbitrator arbitrator = new Arbitrator(behaviorList);
 		arbitrator.start();
 	}	
 }
 
-class FollowRoute implements Behavior {
-	private static final int TRAVEL = 0;
-	private static final int ROTATE = 1;	
-	
+class FindGoal implements Behavior {	
 	private boolean _suppressed = false;
 	private PilotAvoid pilot;
-	private int stepsDone = 0;
 
-	public FollowRoute(PilotAvoid pilot) {
+	public FindGoal(PilotAvoid pilot) {
 		this.pilot = pilot;
 	}
 
@@ -110,62 +94,66 @@ class FollowRoute implements Behavior {
 
 	public void action() {
 		_suppressed = false;
+		Sound.beepSequence();
+		Pose p = pilot.poseProvider.getPose();
 		
-		if (stepsDone < 1 && !_suppressed) {
-			doStep(TRAVEL, 500);			
-		}
-		if (stepsDone < 2 && !_suppressed) {
-			doStep(ROTATE, 90);			
-		}
-		if (stepsDone < 3 && !_suppressed) {
-			doStep(TRAVEL, 200);			
-		}
-		if (stepsDone < 4 && !_suppressed) {
-			doStep(ROTATE, -90);			
-		}
-		if (stepsDone < 5 && !_suppressed) {
-			doStep(TRAVEL, 500);			
-		}
+		// Calculate angle based on destination.
+		float angle = p.angleTo(pilot.desPoint);
+//		float degrees = (angle > 0) ? angle - p.getHeading() : angle + p.getHeading();
+		float degrees = - p.getHeading() + angle;
 		
-		Motor.A.stop(true); // not strictly necessary, but good programming practice
-		Motor.C.stop(true);		
-	}
-	
-	public void doStep(int moveType, double value) {
-		switch (moveType) {
-		case TRAVEL : pilot.travel(value); break;
-		case ROTATE : pilot.rotate(value); break;
-		default : System.err.println("Invalid moveType in doStep(): " + moveType); return;
-		}		
-		stepsDone++;
+//		if (angle < 0 && p.getHeading() < 0) {
+//			degrees = angle - p.getHeading();
+//		} else if (angle > 0 && p.getHeading() < 0) {
+//			degrees = angle + p.getHeading();
+//		} else if (angle > 0 && p.getHeading() > 0) {
+//			degrees = angle + p.getHeading();
+//		} else if (angle < 0 && p.getHeading() > 0) {
+//			degrees = angle - p.getHeading(); // done
+//		}
+		
+		pilot.rotate(degrees, false);							
+		pilot.sendMove(pilot.pilot.getMovement());
+		
+		pilot.travel(p.distanceTo(pilot.desPoint), true);		
 		while (!_suppressed && Motor.A.isMoving()) {			
-			Thread.yield(); // don't exit till suppressed
+			Thread.yield();
 		}
-		pilot.sendMove(pilot.pilot.getMovement());
-	}
+		pilot.sendMove(pilot.pilot.getMovement());		
+		
+		Motor.A.stop(true); // not strictly necessary, but good programming practice
+		Motor.C.stop(true);				
+	}	
 }
 
 class AvoidObstacle extends Thread implements Behavior {
-	UltrasonicSensor sonar = new UltrasonicSensor(SensorPort.S1);
-	private float distance = 255;
+	UltrasonicSensor sonar = new UltrasonicSensor(SensorPort.S2);
+	private int distance = 255;
+	private int count = 0;
+	private boolean active = false;
 
 	private boolean _suppressed = false;
 	private PilotAvoid pilot;
 
 	public AvoidObstacle(PilotAvoid pilot) {
 		this.pilot = pilot;
-		this.isDaemon();
+		this.setDaemon(true);
 		this.start();
 	}
 	
 	public void run() {
 		while (true) {
 			distance = sonar.getDistance();
+			LCD.drawInt(distance, 3, 0, 2);
 		}
 	}
 
 	public int takeControl() {
-		return (distance < 15) ? 20 : 0;
+		count++;
+		int t = (distance < 25 || active) ? 20 : 0;
+		LCD.drawInt(t, 2, 0, 3);
+		LCD.drawInt(count, 6, 0, 4);
+		return t;
 	}
 
 	public void suppress() {
@@ -174,15 +162,75 @@ class AvoidObstacle extends Thread implements Behavior {
 
 	public void action() {
 		_suppressed = false;
-		
+		active = true;
 		Sound.buzz();
 		
+		// Look for a clear passage.		
+		pilot.rotate(45, true);
+		while (!_suppressed && Motor.A.isMoving()) {			
+			Thread.yield();
+		}
+		pilot.sendMove(pilot.pilot.getMovement());
+		float disLeft = sonar.getDistance() * 10;		
+		
+		pilot.rotate(-90, true);
+		while (!_suppressed && Motor.A.isMoving()) {			
+			Thread.yield();
+		}
+		pilot.sendMove(pilot.pilot.getMovement());
+		float disRight = sonar.getDistance() * 10;
+		
 		// Take evasive action.
+		if (disLeft > disRight) {
+			pilot.rotate(90, true);
+			while (!_suppressed && Motor.A.isMoving()) {			
+				Thread.yield();
+			}
+			pilot.sendMove(pilot.pilot.getMovement());
+		} 
+		
+		pilot.travel(Math.min(Math.max(disLeft - 200, disRight - 200), 300), true);
 		
-		while (!_suppressed) {
-			Thread.yield(); // don't exit till suppressed
+		while (!_suppressed && Motor.A.isMoving()) {			
+			Thread.yield();
 		}
-//		Motor.A.stop(true); // not strictly necessary, but good programming practice
-//		Motor.C.stop(true);
+		pilot.sendMove(pilot.pilot.getMovement());
+		
+		Motor.A.stop(true); // not strictly necessary, but good programming practice
+		Motor.C.stop(true);
+		active = false;
 	}
 }
+
+class RecognizeGoal implements Behavior {	
+	private boolean _suppressed = false;
+	private PilotAvoid pilot;
+	private Pose p;
+
+	public RecognizeGoal(PilotAvoid pilot) {
+		this.pilot = pilot;
+		
+	}
+
+	public int takeControl() {
+		// Check if we reached the goal.
+		p = pilot.poseProvider.getPose();
+		float d = p.distanceTo(pilot.desPoint);
+		LCD.drawInt((int)d, 5, 0, 5);
+		return (d <= 10) ? 50 : 0; 
+	}
+
+	public void suppress() {
+		_suppressed = true;// standard practice for suppress methods
+	}
+
+	public void action() {
+		_suppressed = false;
+		// We reached the target!
+		Sound.beepSequenceUp();
+		while (!Button.ESCAPE.isDown()) {
+			Delay.msDelay(100);
+		}
+		System.exit(0);
+	}	
+}
\ No newline at end of file

lesson10/PC/InputOutputStreams.java

@@ -29,7 +29,7 @@ public class InputOutputStreams
 		}
 		else {
 			// Connect to any NXT using Bluetooth
-			if (!conn.connectTo("btspp://")){
+			if (!conn.connectTo("btspp://NXT8")){
 				return "No NXT found using Bluetooth";
 			}
 		}

lesson10/PC/PilotGUI.java

@@ -21,8 +21,8 @@ public class PilotGUI extends JFrame {
     // program will automatically adjust if these three constants are changed
     static final int VIS_HEIGHT=1080;  // visualization height  
     static final int VIS_WIDTH=1920;   // visualization width
-    static final int Y_OFFSET=30;     // vertical offset to visualization
-    static final int X_OFFSET=20;     // horizontal offset to visualization
+    static final int Y_OFFSET=200;     // vertical offset to visualization
+    static final int X_OFFSET=200;     // horizontal offset to visualization
     static final int POINT_WIDTH=1;  // width of points shown in the visualization
                                       // VIS_HEIGHT + 2 * Y_OFFSET is the window height
                                       // VIS_WIDTH + 2 * X_OFFSET is the window width
@@ -34,6 +34,8 @@ public class PilotGUI extends JFrame {
     ArrayList<Point> route;           // the route as seen from the vehicle
     
     ArrayList<ParticleSet> particleSets = new ArrayList<ParticleSet>();
+	private int desX;
+	private int desY;
     
     // an inner class to handle the visualization window being closed manually
     class MyWindowAdapter extends WindowAdapter {   
@@ -43,9 +45,11 @@ public class PilotGUI extends JFrame {
     }
     
     // the constructor for the Visualize class    
-    public PilotGUI(ParticleSet particles) 
+    public PilotGUI(ParticleSet particles, int desX, int desY) 
     { 
     	 this.particles = particles;
+    	 this.desX = desX;
+    	 this.desY = desY;
          this.setSize(new Dimension(VIS_WIDTH+X_OFFSET*2, VIS_HEIGHT+Y_OFFSET*2));
          this.setTitle("Monte Carlo Localization");
          this.setVisible(true);
@@ -101,7 +105,9 @@ public class PilotGUI extends JFrame {
             }
         }
         
-
+        // Draw destination.
+        g2.setColor(Color.RED);
+        g2.fillOval(winX(desX) - 5, winY(desY) + 5, 10, 10);
     }
     
     private int winX(int x)

lesson10/PC/PilotMonitor.java

@@ -19,18 +19,23 @@ public class PilotMonitor
 	private boolean USB = false;
 	private Route route = new Route();
     private ParticleSet particles  = new ParticleSet(1000);
-    private PilotGUI view = new PilotGUI(particles);
+    private PilotGUI view;
     private Move move;
     
     public PilotMonitor()
     {
-    	String m;
-        view.update(route.getRoute(), false);
+    	String m;        
         NXT = new InputOutputStreams(USB);
         do {
         	m = NXT.open();
         	System.out.println(m);
         } while ( m != "Connected");
+        
+        // Receive destination from NXT.
+        int desX = (int)NXT.input();
+        int desY = (int)NXT.input();
+        view = new PilotGUI(particles, desX, desY);
+        view.update(route.getRoute(), false);
     }
     
     private Move getMove()

lesson11/NXT/ArenaRobot.java

@@ -54,9 +54,9 @@ public class ArenaRobot {
 
 		Behavior b1 = new Wander(pilot);
 		Behavior b2 = new AvoidEdge(pilot);
-		Behavior b3 = new Exit(pilot);		
+		Behavior b3 = new Exit(pilot, 180);		
 		Behavior[] behaviorList = {b1, b2, b3};
-		Arbitrator arbitrator = new Arbitrator(behaviorList);
+		Arbitrator arbitrator = new Arbitrator(behaviorList);		
 		arbitrator.start();
 	}	
 }
@@ -85,7 +85,7 @@ class Wander implements Behavior {
 
 		double rand = Math.random(); 
 		if (rand > 0.5) {
-			pilot.travel(rand * 2000);
+			pilot.travel(rand * 200);
 		} else {
 			pilot.rotate((Math.random() > 0.5) ? rand * 180 : rand * -180);
 		}	
@@ -160,13 +160,18 @@ class AvoidEdge extends Thread implements Behavior {
 class Exit implements Behavior {
 	private boolean _suppressed = false;
 	private ArenaRobot pilot;
+	private int seconds;
+	private long startTime;
 
-	public Exit(ArenaRobot pilot) {
+	public Exit(ArenaRobot pilot, int seconds) {
 		this.pilot = pilot;
+		this.seconds = seconds;
+		startTime = System.currentTimeMillis();
 	}
 
 	public int takeControl() {
-		return (Button.ESCAPE.isDown()) ? 100 : 0 ; // this behavior always wants control.
+		return (Button.ESCAPE.isDown() || 
+				System.currentTimeMillis() > (startTime + seconds * 1000)) ? 100 : 0 ;
 	}
 
 	public void suppress() {

lesson11/NXT/LightTest.java

+import lejos.nxt.Button;
+import lejos.nxt.LCD;
+import lejos.nxt.LightSensor;
+import lejos.nxt.SensorPort;
+
+
+public class LightTest {
+	
+	public static void main (String[] args) {
+		LightSensor sensor = new LightSensor(SensorPort.S1);	
+		
+		while (!Button.ESCAPE.isDown()) {			
+			LCD.drawString("Light: ", 0, 5);
+			LCD.drawInt(sensor.readNormalizedValue(), 4, 10, 5);
+		}
+	}
+}

lesson11/NXT/PilotControl2D.java

+import lejos.nxt.*;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.robotics.navigation.Move;
+import lejos.util.Delay;
+
+public class PilotControl2D
+{
+    private SlaveIOStreams PC;
+    private LightSensor light = new LightSensor(SensorPort.S1);
+    
+    double mult = 1.007;
+    double leftWheelDiameter = 55 * mult, rightWheelDiameter = 54.84 * mult, trackWidth = 165;
+    double travelSpeed = 50, rotateSpeed = 45;
+    NXTRegulatedMotor left = Motor.A;
+    NXTRegulatedMotor right = Motor.C;
+	   
+    DifferentialPilot pilot = new DifferentialPilot(leftWheelDiameter, rightWheelDiameter, trackWidth, left, right, false);
+	private boolean haveLocation = false;
+
+	public PilotControl2D(boolean usb) 
+	{
+    	PC = new SlaveIOStreams(usb);
+    	PC.open();
+ 	   
+ 	    pilot.setTravelSpeed(travelSpeed);
+ 	    pilot.setRotateSpeed(rotateSpeed);
+	}
+	
+	private void sendMove(Move move)
+	{	
+		PC.output((move.getMoveType() == Move.MoveType.TRAVEL? 0:1 ));
+		PC.output(move.getDistanceTraveled());
+		PC.output(move.getAngleTurned());
+	}
+	
+	private void sendLight() {
+		PC.output(light.getLightValue());
+	}
+	
+	private void travel(double distance)
+	{	
+		pilot.travel(distance);
+		sendMove(pilot.getMovement());
+		PC.output(light.readNormalizedValue());
+	}
+	
+	private void rotate(double angle)
+	{	
+		pilot.rotate(angle);
+		sendMove(pilot.getMovement());
+		PC.output(light.readNormalizedValue());
+	}
+	
+	public void go()
+	{				
+		Sound.beep();
+		while ( ! Button.ENTER.isDown()) Thread.yield();
+		Sound.twoBeeps();
+		
+		travel(0);
+		PC.input();
+		for (int i = 0; i < 6; i++)	{
+			travel(115);
+			Delay.msDelay(500);
+			if (PC.input() == 1) {
+				// I have my location!
+				Sound.buzz();
+				haveLocation = true;
+				break;
+			}
+    	}
+		if (!haveLocation) {
+			rotate(90);
+			for(int j = 0; j < 4; j++) {
+				travel(115);
+				Delay.msDelay(500);
+				if (PC.input() == 1) {
+					// I have my location!
+					Sound.buzz();
+					haveLocation = true;
+					break;
+				}
+			}
+		}
+		if (!haveLocation) {
+			rotate(90);
+			for(int j = 0; j < 4; j++) {
+				travel(115);
+				Delay.msDelay(500);
+				if (PC.input() == 1) {
+					// I have my location!
+					Sound.buzz();
+					haveLocation = true;
+					break;
+				}
+			}
+		}
+				
+		while ( ! Button.ENTER.isDown()) Thread.yield();
+		
+        LCD.clear();	
+        LCD.drawString("Closing",0,0);
+    	if ( PC.close() ) LCD.drawString("Closed",0,0); 
+        try {Thread.sleep(2000);} catch (Exception e){}
+	}
+	
+	
+	public static void main(String[] args) 
+	{
+		PilotControl2D route = new PilotControl2D(false);
+		
+		LCD.clear();
+		LCD.drawString("PilotControl", 0, 0);
+		route.go();		
+	}
+}

lesson11/NXT/PilotController.java

+import lejos.nxt.*;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.robotics.navigation.Move;
+
+public class PilotController
+{
+    private SlaveIOStreams PC;
+    private LightSensor light = new LightSensor(SensorPort.S1);
+    
+    double mult = 1.007;
+    double leftWheelDiameter = 55 * mult, rightWheelDiameter = 54.84 * mult, trackWidth = 165;
+    double travelSpeed = 50, rotateSpeed = 45;
+    NXTRegulatedMotor left = Motor.A;
+    NXTRegulatedMotor right = Motor.C;
+	   
+    DifferentialPilot pilot = new DifferentialPilot(leftWheelDiameter, rightWheelDiameter, trackWidth, left, right, false);
+
+	public PilotController(boolean usb) 
+	{
+    	PC = new SlaveIOStreams(usb);
+    	PC.open();
+ 	   
+ 	    pilot.setTravelSpeed(travelSpeed);
+ 	    pilot.setRotateSpeed(rotateSpeed);
+	}
+	
+	private void sendMove(Move move)
+	{	
+		PC.output((move.getMoveType() == Move.MoveType.TRAVEL? 0:1 ));
+		PC.output(move.getDistanceTraveled());
+		PC.output(move.getAngleTurned());
+	}		
+	
+	private void travel(double distance)
+	{	
+		pilot.travel(distance);
+		sendMove(pilot.getMovement());
+		PC.output(light.readNormalizedValue());
+	}
+	
+	private void rotate(double angle)
+	{	
+		pilot.rotate(angle);