Almost complete solution. Avoid behavior is missing.

lesson10/NXT/Arbitrator.java

+
+/**
+ * Arbitrator controls which Behavior object will become active in
+ * a behavior control system. 
+ * 
+ * Make sure to call start() after the Arbitrator is instantiated.<br>
+ * 
+ * This class has three major responsibilities: <br> 
+ * 1. Determine the highest priority  behavior that returns <b> true </b> to takeControl()<br>   
+ * 2. Suppress the active behavior if its priority is less than highest priority. <br>   
+ * 3. When the action() method exits, call action() on the Behavior of highest priority.
+ * <br>  The Arbitrator assumes that a Behavior is no longer active when action() exits,
+ * <br>  therefore it will only call suppress() on the Behavior whose action() method is running.
+ * <br>  It can make consecutive calls of action() on the same Behavior.
+ * <br>  Requirements for a Behavior:
+ * <br>    When suppress() is called, terminate  action() immediately.
+ * <br>    When action() exits, the robot is in a safe state (e.g. motors stopped)
+ * @see Behavior
+ * @author Roger Glassey
+ * 
+ * Modified so the Behavior with the highest integer priority returned by
+ * takeControl is the active behavior. Also modified so start() never exits,
+ * there should always be a behavior that wants to be active.
+ * 
+ * Ole Caprani, 24-12-2012
+ * 
+ */
+public class Arbitrator
+{
+	
+    private final int NONE = -1;
+    private Behavior[] behavior;
+    private BehaviorAction actionThread;
+    private int currentBehavior = NONE;
+    private int currentPriority = NONE;
+ 
+    public Arbitrator(Behavior[] behaviorList)
+    {
+        behavior = behaviorList;
+        actionThread = new BehaviorAction();
+        actionThread.setDaemon(true);
+    }
+
+    /**
+     * This method starts the arbitration of Behaviors and runs an endless loop.
+     * The start() method will never return
+     */
+     public void start()
+     {
+         int highest, maxPriority;
+	     
+         actionThread.start();
+	     
+         while (true)
+         {   
+             // Find behavior with highest priority
+    	     maxPriority = -1; highest = -1;
+    	     for (int i = 0; i < behavior.length; i++)
+    	     {
+    	    	 int priority = behavior[i].takeControl();
+                 if (priority > maxPriority ) 
+                 {
+                    highest = i;
+                    maxPriority = priority;
+                 }
+    	     }
+             
+    	     // Start highest priority process and update currentPriority
+             if ( actionThread.current == NONE)  
+             {
+    	         currentBehavior = highest;
+    	         currentPriority = maxPriority;
+    	         actionThread.execute(highest);
+             }
+             else
+             if (  currentPriority < maxPriority )
+             {
+    	         behavior[currentBehavior].suppress();
+      	         currentBehavior = highest;
+      	         currentPriority = maxPriority;
+      	         actionThread.execute(highest);
+    	  
+             }
+             else
+            	 currentPriority = maxPriority;
+             Thread.yield();
+          }
+    }
+
+    /**
+     * Local thread that runs the action method for the currently
+     * highest priority behavior
+     */
+     private class BehaviorAction extends Thread
+     {
+         public int current = NONE;
+ 
+         public void run()
+         {
+             while (true)
+             {
+                 synchronized (this)
+                 {
+                     if ( current != NONE )
+                     {
+        	             behavior[current].action();
+        	             current = NONE;
+                     }
+                 }
+                 Thread.yield();
+             }
+          }
+    
+          public synchronized void execute(int index)
+          {
+    	     current = index;
+          }
+      }
+}
+  

lesson10/NXT/Behavior.java

+
+
+
+/**
+* The Behavior interface represents an object embodying a specific
+* behavior belonging to a robot. Each behavior must define three things: <BR>
+* 1) The circumstances to make this behavior seize control of the robot.
+* e.g. When the touch sensor determines the robot has collided with an object.<BR>
+* 2) The action to perform when this behavior takes control. 
+* e.g. Back up and turn.<BR>
+* 3) A way to quickly exit from the action when the Arbitrator selects  a higher
+ * priority behavior to take control.
+* These are represented by defining the methods takeControl(), action(),
+* and suppress() respectively. <BR>
+* A behavior control system has one or more Behavior objects. When you have defined
+* these objects, create an array of them and use that array to initialize an
+* Arbitrator object.
+*
+* @see Arbitrator
+  
+* @version 0.9  May 2011
+* Modified so takeControl returns an integer, Ole Caprani 24-12-2012
+*/
+public interface Behavior {
+   
+   /**
+   * The integer returned  indicates how much this behavior wants control of the robot.
+   * For example, a robot that reacts if a touch sensor is pressed: <BR>
+   * public int takeControl() { <BR>
+   *    if ( touch.isPressed() ) return 100;
+   *    return 0; <BR>
+   * } <BR>
+   * @return integer  Indicates if this Behavior should seize control.
+   */
+   public int takeControl();
+   
+   /**
+   * The code in action() represents the tasks  the robot performs when this
+   * behavior becomes active. It can be as complex as navigating around a
+   * room, or as simple as playing a tune.<BR>
+   * <B>The contract for implementing this method is:</B><BR>
+   *  If its task is  is complete, the method returns.
+    * It also  <B> must </B> return promptly when the suppress() method
+    * is called, for example by testing the boolean suppress flag.  <br>
+    * When this method exits, the robot is in a safe state for another behavior
+    * to run its action() method
+   */
+   public void action();
+   
+   /**
+   * The code in suppress() should cause the current behavior to exit. <BR>
+   * <B>The contract for implementing this method is:</B><BR>
+   *  Exit  quickly, for example, just set boolean flag.
+   */
+   public void suppress();
+   
+}
\ No newline at end of file

lesson10/NXT/PilotAvoid.java

+import lejos.nxt.*;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.robotics.navigation.Move;
+
+public class PilotAvoid {
+	private SlaveIOStreams PC;
+
+	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 PilotAvoid(boolean usb) {
+		PC = new SlaveIOStreams(usb);
+		PC.open();
+
+		pilot.setTravelSpeed(travelSpeed);
+		pilot.setRotateSpeed(rotateSpeed);
+	}
+
+	public void sendMove(Move move) {
+		PC.output((move.getMoveType() == Move.MoveType.TRAVEL ? 0 : 1));
+		PC.output(move.getDistanceTraveled());
+		PC.output(move.getAngleTurned());
+	}
+
+	public void travel(double distance) {
+		pilot.travel(distance, true);
+	}
+
+	public void rotate(double angle) {
+		pilot.rotate(angle, true);
+	}
+
+	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 static void main(String[] args) {
+		PilotAvoid pilot = new PilotAvoid(false);
+
+		LCD.clear();
+		LCD.drawString("PilotRouteMM", 0, 0);
+		// route.go();
+		
+		Sound.beep();
+		while (!Button.ENTER.isDown())
+			Thread.yield();
+		Sound.twoBeeps();
+		
+		Behavior b1 = new FollowRoute(pilot);
+		Behavior b2 = new AvoidObstacle(pilot);		
+		Behavior[] behaviorList = { b1, b2};
+		Arbitrator arbitrator = new Arbitrator(behaviorList);
+		arbitrator.start();
+	}	
+}
+
+class FollowRoute implements Behavior {
+	private static final int TRAVEL = 0;
+	private static final int ROTATE = 1;	
+	
+	private boolean _suppressed = false;
+	private PilotAvoid pilot;
+	private int stepsDone = 0;
+
+	public FollowRoute(PilotAvoid pilot) {
+		this.pilot = pilot;
+	}
+
+	public int takeControl() {
+		return 10; // this behavior always wants control.
+	}
+
+	public void suppress() {
+		_suppressed = true;// standard practice for suppress methods
+	}
+
+	public void action() {
+		_suppressed = false;
+		
+		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);			
+		}
+		
+		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++;
+		while (!_suppressed && Motor.A.isMoving()) {			
+			Thread.yield(); // don't exit till suppressed
+		}
+		pilot.sendMove(pilot.pilot.getMovement());
+	}
+}
+
+class AvoidObstacle extends Thread implements Behavior {
+	UltrasonicSensor sonar = new UltrasonicSensor(SensorPort.S1);
+	private float distance = 255;
+
+	private boolean _suppressed = false;
+	private PilotAvoid pilot;
+
+	public AvoidObstacle(PilotAvoid pilot) {
+		this.pilot = pilot;
+		this.isDaemon();
+		this.start();
+	}
+	
+	public void run() {
+		while (true) {
+			distance = sonar.getDistance();
+		}
+	}
+
+	public int takeControl() {
+		return (distance < 15) ? 20 : 0;
+	}
+
+	public void suppress() {
+		_suppressed = true;// standard practice for suppress methods
+	}
+
+	public void action() {
+		_suppressed = false;
+		
+		Sound.buzz();
+		
+		// Take evasive action.
+		
+		while (!_suppressed) {
+			Thread.yield(); // don't exit till suppressed
+		}
+//		Motor.A.stop(true); // not strictly necessary, but good programming practice
+//		Motor.C.stop(true);
+	}
+}

lesson10/NXT/PilotRoute.java

@@ -6,12 +6,13 @@ public class PilotRoute
 {
     private SlaveIOStreams PC;
     
-	private double wheelDiameter = 5.5, trackWidth = 16.0;
-	private double travelSpeed = 5, rotateSpeed = 45;
-	private NXTRegulatedMotor left = Motor.B;
-	private NXTRegulatedMotor right = Motor.C;
+    double mult = 1.007;
+    double leftWheelDiameter = 5.5 * mult, rightWheelDiameter = 5.484 * mult, trackWidth = 16.5;
+    double travelSpeed = 5, rotateSpeed = 45;
+    NXTRegulatedMotor left = Motor.A;
+    NXTRegulatedMotor right = Motor.C;
 	   
-	private DifferentialPilot pilot = new DifferentialPilot(wheelDiameter, trackWidth, left, right);
+    DifferentialPilot pilot = new DifferentialPilot(leftWheelDiameter, rightWheelDiameter, trackWidth, left, right, false);
 
 	public PilotRoute(boolean usb) 
 	{

lesson10/PC/PilotRoute.java → lesson10/NXT/PilotRouteMM.java

@@ -2,18 +2,19 @@ import lejos.nxt.*;
 import lejos.robotics.navigation.DifferentialPilot;
 import lejos.robotics.navigation.Move;
 
-public class PilotRoute
+public class PilotRouteMM
 {
     private SlaveIOStreams PC;
     
-	private double wheelDiameter = 5.5, trackWidth = 16.0;
-	private double travelSpeed = 5, rotateSpeed = 45;
-	private NXTRegulatedMotor left = Motor.B;
-	private NXTRegulatedMotor right = Motor.C;
+    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;
 	   
-	private DifferentialPilot pilot = new DifferentialPilot(wheelDiameter, trackWidth, left, right);
+    DifferentialPilot pilot = new DifferentialPilot(leftWheelDiameter, rightWheelDiameter, trackWidth, left, right, false);
 
-	public PilotRoute(boolean usb) 
+	public PilotRouteMM(boolean usb) 
 	{
     	PC = new SlaveIOStreams(usb);
     	PC.open();
@@ -47,11 +48,25 @@ public class PilotRoute
 		while ( ! Button.ENTER.isDown()) Thread.yield();
 		Sound.twoBeeps();
 		
-		travel(50);
+		travel(500);
+		rotate(90);
+		travel(200);
+		rotate(-90);
+		travel(500);	
+		
+		/*
+		travel(500);
 		rotate(90);	
-		travel(20);
+		travel(200);
 		rotate(-90);
-		travel(50);
+		travel(500);
+		rotate(90);
+		travel(200);
+		rotate(90);
+		travel(700);
+		rotate(90);
+		travel(200);
+		 */
 		
 		while ( ! Button.ENTER.isDown()) Thread.yield();
 		
@@ -64,10 +79,10 @@ public class PilotRoute
 	
 	public static void main(String[] args) 
 	{
-		PilotRoute route = new PilotRoute(false);
+		PilotRouteMM route = new PilotRouteMM(false);
 		
 		LCD.clear();
-		LCD.drawString("Pilot route", 0, 0);
+		LCD.drawString("PilotRouteMM", 0, 0);
 		route.go();		
 	}
 }

lesson10/NXT/PilotStraight.java

@@ -30,7 +30,7 @@ public class PilotStraight
    public static void main(String [] args)  
    throws Exception 
    {
-       double wheelDiameter = 5.5, trackWidth = 16.0;
+       double wheelDiameter = 5.5, trackWidth = 16.5;
        double travelSpeed = 5, rotateSpeed = 45;
        NXTRegulatedMotor left = Motor.A;
        NXTRegulatedMotor right = Motor.C;

lesson10/NXT/PilotStraight2.java

+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;
+/**
+ * A program that uses the DifferentialPilot to make a differential
+ * driven car drive in a square. During the drive the OdometryPoseProvider
+ * is used to display the Pose of the car on the LCD after each
+ * movement of the car.
+ * 
+ * Use the PC tool nxjconsoleviewer to show the LCD output on a PC
+ * for easier inspection.
+ * 
+ * @author  Ole Caprani
+ * @version 13.05.15
+ */
+public class PilotStraight2 
+{
+   private static void show(Pose p)
+   {
+      LCD.clear();
+       LCD.drawString("Pose X " + p.getX(), 0, 2);
+       LCD.drawString("Pose Y " + p.getY(), 0, 3);
+       LCD.drawString("Pose V " + p.getHeading(), 0, 4);
+   }
+
+   public static void main(String [] args)  
+   throws Exception 
+   {
+	   double mult = 1.007;
+       double leftWheelDiameter = 5.5 * mult, rightWheelDiameter = 5.484 * mult, trackWidth = 16.5;
+       double travelSpeed = 5, rotateSpeed = 45;
+       NXTRegulatedMotor left = Motor.A;
+       NXTRegulatedMotor right = Motor.C;
+	   
+       DifferentialPilot pilot = new DifferentialPilot(leftWheelDiameter, rightWheelDiameter, trackWidth, left, right, false);
+       OdometryPoseProvider poseProvider = new OdometryPoseProvider(pilot);
+       Pose initialPose = new Pose(0,0,0);
+       pilot.setTravelSpeed(travelSpeed);
+       pilot.setRotateSpeed(rotateSpeed);
+       
+       LCD.drawString("PilotStraightLoop", 0, 0);
+       Button.waitForAnyPress();
+       
+       while (!Button.ESCAPE.isDown()) {
+    	   poseProvider.setPose(initialPose);           
+           pilot.travel(50);           
+           show(poseProvider.getPose());
+           Button.waitForAnyPress();
+       }       
+       
+       pilot.stop();
+       LCD.drawString("Program stopped", 0, 0);
+       Button.waitForAnyPress();
+       Thread.sleep(2000);
+   }
+}

lesson10/NXT/PilotTurn2.java

+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;
+/**
+ * A program that uses the DifferentialPilot to make a differential
+ * driven car drive in a square. During the drive the OdometryPoseProvider
+ * is used to display the Pose of the car on the LCD after each
+ * movement of the car.
+ * 
+ * Use the PC tool nxjconsoleviewer to show the LCD output on a PC
+ * for easier inspection.
+ * 
+ * @author  Ole Caprani
+ * @version 13.05.15
+ */
+public class PilotTurn2 
+{
+   private static void show(Pose p)
+   {
+      LCD.clear();
+       LCD.drawString("Pose X " + p.getX(), 0, 2);
+       LCD.drawString("Pose Y " + p.getY(), 0, 3);
+       LCD.drawString("Pose V " + p.getHeading(), 0, 4);
+   }
+
+   public static void main(String [] args)  
+   throws Exception 
+   {
+	   double mult = 1.007;
+       double leftWheelDiameter = 5.5 * mult, rightWheelDiameter = 5.484 * mult, trackWidth = 16.5;
+       double travelSpeed = 5, rotateSpeed = 45;
+       NXTRegulatedMotor left = Motor.A;
+       NXTRegulatedMotor right = Motor.C;
+	   
+       DifferentialPilot pilot = new DifferentialPilot(leftWheelDiameter, rightWheelDiameter, trackWidth, left, right, false);
+       OdometryPoseProvider poseProvider = new OdometryPoseProvider(pilot);
+       Pose initialPose = new Pose(0,0,0);
+       pilot.setTravelSpeed(travelSpeed);
+       pilot.setRotateSpeed(rotateSpeed);
+       
+       LCD.drawString("PilotTurn2", 0, 0);
+       Button.waitForAnyPress();
+       
+       while (!Button.ESCAPE.isDown()) {
+    	   poseProvider.setPose(initialPose);           
+           pilot.rotate(720);           
+           show(poseProvider.getPose());
+           Button.waitForAnyPress();
+       }       
+       
+       pilot.stop();
+       LCD.drawString("Program stopped", 0, 0);
+       Button.waitForAnyPress();
+       Thread.sleep(2000);
+   }
+}

lesson10/PC/Particle.java

@@ -57,12 +57,11 @@ public class Particle
     float ym = (move.getDistanceTraveled() * ((float) Math.sin(Math.toRadians(pose.getHeading()))));
     float xm = (move.getDistanceTraveled() * ((float) Math.cos(Math.toRadians(pose.getHeading()))));
 
-
     pose.setLocation(new Point(
-    		         (float) (pose.getX() + xm + (distanceNoiseFactor * xm * rand.nextGaussian())),
-                     (float) (pose.getY() + ym + (distanceNoiseFactor * ym * rand.nextGaussian()))));
+    		         (float) (pose.getX() + xm + (distanceNoiseFactor * move.getDistanceTraveled() * rand.nextGaussian())),
+                     (float) (pose.getY() + ym + (distanceNoiseFactor * move.getDistanceTraveled() * rand.nextGaussian()))));
     pose.setHeading(
-       (float) (pose.getHeading() + move.getAngleTurned() + (angleNoiseFactor  * rand.nextGaussian())));
+       (float) (pose.getHeading() + move.getAngleTurned() + (angleNoiseFactor * move.getAngleTurned() * rand.nextGaussian())));
     pose.setHeading((float) ((int) (pose.getHeading() + 0.5f) % 360));
-  }
+  }  
 }

lesson10/PC/ParticleSet.java

-
 import lejos.geom.*;
 import lejos.robotics.*;
 import lejos.robotics.mapping.RangeMap;
@@ -9,89 +8,97 @@ import java.util.Random;
 /**
  * Represents a particle set for the particle filtering algorithm.