Added inital sumo robot code that detects white.

lesson9/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;
+          }
+      }
+}
+  

lesson9/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

lesson9/BumperCar.java

+import lejos.nxt.*;
+import lejos.robotics.RegulatedMotor;
+
+/**
+ * Demonstration of the Behavior subsumption classes.
+ * 
+ * Requires a wheeled vehicle with two independently controlled motors connected
+ * to motor ports A and C, and a touch sensor connected to sensor port 1 and an
+ * ultrasonic sensor connected to port 3;
+ * 
+ * @author Brian Bagnall and Lawrie Griffiths, modified by Roger Glassey
+ *
+ *         Uses a new version of the Behavior interface and Arbitrator with
+ *         integer priorities returned by takeCaontrol instead of booleans.
+ * 
+ *         Exit behavior inserted, local distance sampling thread and backward
+ *         drive added in DetectWall by Ole Caprani, 23-4-2012
+ */
+public class BumperCar {
+
+	public static void main(String[] args) {
+		Motor.B.setSpeed(2000);
+		Motor.A.setSpeed(400);
+		Motor.C.setSpeed(400);
+		Behavior b1 = new DriveForward();
+		Behavior b2 = new DetectWhite();
+		Behavior b3 = new Exit();
+		Behavior[] behaviorList = { b1, b2, b3 };
+		Arbitrator arbitrator = new Arbitrator(behaviorList);
+		LCD.drawString("Bumper Car", 0, 1);
+		Button.waitForAnyPress();
+		try {
+			Thread.sleep(3000);
+		} catch (InterruptedException e) {
+			// TODO Auto-generated catch block
+			e.printStackTrace();
+		}
+		Motor.B.backward();
+		arbitrator.start();
+	}
+}
+
+class DriveForward implements Behavior {
+
+	private boolean _suppressed = false;
+
+	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;
+		Motor.A.forward();
+		Motor.C.forward();
+		LCD.drawString("Drive forward", 0, 2);
+		while (!_suppressed) {
+			Thread.yield(); // don't exit till suppressed
+		}
+		Motor.A.stop(); // not strictly necessary, but good programming practice
+		Motor.C.stop();
+		LCD.drawString("Drive stopped", 0, 2);
+	}
+}
+
+class DetectWhite extends Thread implements Behavior {
+	private LightSensor light;
+//	private UltrasonicSensor sonar;
+	private boolean _suppressed = false;
+	private boolean active = false;
+	private int distance = 255;
+
+	public DetectWhite() {
+		light = new LightSensor(SensorPort.S3);
+//		sonar = new UltrasonicSensor(SensorPort.S3);
+		this.setDaemon(true);
+		this.start();
+	}
+
+	public void run() {
+//		while (true)
+//			distance = sonar.getDistance();
+	}
+
+	public int takeControl() {
+		return Math.max(0, light.readValue() - 40);
+	}
+	
+	public void suppress() {
+		_suppressed = true;// standard practice for suppress methods
+	}
+
+	public void action() {
+		_suppressed = false;
+		active = true;
+		Sound.beepSequenceUp();
+
+		// Backward for 1000 msec
+		LCD.drawString("Drive backward", 0, 3);
+		Motor.A.backward();
+		Motor.C.backward();
+		long now = System.currentTimeMillis();
+		while (!_suppressed && (System.currentTimeMillis() < now + 1000)) {
+			Thread.yield(); // don't exit till suppressed
+		}		
+
+		// Turn
+		LCD.drawString("Turn          ", 0, 3);		
+		Motor.A.rotate(-180, true);// start Motor.A rotating backward
+		Motor.C.rotate(-800, true); // rotate C farther to make the turn
+		while (!_suppressed && Motor.C.isMoving()) {
+			Thread.yield(); // don't exit till suppressed
+		}
+		Motor.A.stop();
+		Motor.C.stop();
+		LCD.drawString("Stopped       ", 0, 3);
+		Sound.beepSequence();
+		active = false;
+
+	}
+}
+
+class Exit implements Behavior {
+	private boolean _suppressed = false;
+
+	public int takeControl() {
+		if (Button.ESCAPE.isDown())
+			return 200;
+		return 0;
+	}
+
+	public void suppress() {
+		_suppressed = true;// standard practice for suppress methods
+	}
+
+	public void action() {
+		System.exit(0);
+	}
+}