Add Lesson11 programs

Lesson11/Programs/ArenaBot/.classpath

+<?xml version="1.0" encoding="UTF-8"?>
+<classpath>
+	<classpathentry kind="src" path="src"/>
+	<classpathentry kind="con" path="org.lejos.nxt.ldt.LEJOS_LIBRARY_CONTAINER/nxt"/>
+	<classpathentry kind="output" path="bin"/>
+</classpath>

Lesson11/Programs/ArenaBot/.project

+<?xml version="1.0" encoding="UTF-8"?>
+<projectDescription>
+	<name>ArenaBot</name>
+	<comment></comment>
+	<projects>
+	</projects>
+	<buildSpec>
+		<buildCommand>
+			<name>org.eclipse.jdt.core.javabuilder</name>
+			<arguments>
+			</arguments>
+		</buildCommand>
+		<buildCommand>
+			<name>org.lejos.nxt.ldt.leJOSBuilder</name>
+			<arguments>
+			</arguments>
+		</buildCommand>
+	</buildSpec>
+	<natures>
+		<nature>org.lejos.nxt.ldt.leJOSNature</nature>
+		<nature>org.eclipse.jdt.core.javanature</nature>
+	</natures>
+</projectDescription>

Lesson11/Programs/ArenaBot/src/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;
+          }
+      }
+}
+  

Lesson11/Programs/ArenaBot/src/ArenaBot.java

+
+import lejos.nxt.*;
+import lejos.nxt.comm.RConsole;
+import lejos.robotics.localization.OdometryPoseProvider;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.robotics.navigation.Pose;
+import lejos.util.Delay;
+
+public class ArenaBot
+{	
+    private static double distFactor = 0.99;
+	private static double wheelDiameterL = 5.595*distFactor, wheelDiameterR = 5.6*distFactor, trackWidth = 16.27;
+	private static double travelSpeed = 15, rotateSpeed = 45;
+	private static NXTRegulatedMotor left = Motor.C;
+	private static NXTRegulatedMotor right = Motor.B;
+	   
+	private static DifferentialPilot pilot = new DifferentialPilot(wheelDiameterL,wheelDiameterR,trackWidth, left, right,false);
+	private static OdometryPoseProvider poseProvider = new OdometryPoseProvider(pilot);
+
+	public static void main(String[] args)
+	{ 	   
+	   RConsole.open();
+	   pilot.setTravelSpeed(travelSpeed);
+	   pilot.setRotateSpeed(rotateSpeed);
+	   Pose initialPose = new Pose(0,0,0);
+       poseProvider.setPose(initialPose);
+
+ 	    
+       Button.ESCAPE.addButtonListener(new ButtonListener() {
+    	   @Override
+    	   public void buttonReleased(Button b) {			
+    	   }
+    	   
+    	   @Override
+    	   public void buttonPressed(Button b) {
+    		   RConsole.close();
+    		   System.exit(0); 				
+    	   }
+		});
+	  
+		
+		StayInRing stay = new StayInRing(pilot);
+		stay.calibrate();
+		
+		Wander wander = new Wander(pilot);
+		
+		Behavior[] behaviorList =
+		{
+			wander, stay
+		};
+		Arbitrator arbitrator = new Arbitrator(behaviorList); 
+		LCD.drawString("Press to start ", 0, 3);
+		Button.waitForAnyPress();
+		LCD.clear();
+		show();
+		Delay.msDelay(1000);
+		
+		arbitrator.start();
+	}
+	
+	public static void show()
+	{
+		Pose p = poseProvider.getPose();
+		LCD.clear();
+		LCD.drawString("Pose X " + p.getX(), 0, 0);
+		LCD.drawString("Pose Y " + p.getY(), 0, 1);
+		LCD.drawString("Pose V " + p.getHeading(), 0, 2);
+	}
+	
+}
+
+
+
+
+
+

Lesson11/Programs/ArenaBot/src/Behavior.java

+import lejos.robotics.navigation.DifferentialPilot;
+
+
+
+
+/**
+* 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 abstract class Behavior {
+	
+	protected volatile boolean _suppressed;
+	
+	protected DifferentialPilot pilot;
+   
+   /**
+   * 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 abstract 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()
+   {
+	   _suppressed = false;
+   }
+   
+   /**
+   * 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()
+   {
+	   _suppressed = true;
+	   pilot.stop();
+	   ArenaBot.show();
+   }
+   
+   protected void waitWhileActive()
+   {
+	    while (!_suppressed )
+	    {
+	      Thread.yield(); //don't exit till suppressed
+	    }
+   }
+   
+   protected void wait(int waitTime)
+   {
+	   	int now = (int)System.currentTimeMillis();
+	    while (!_suppressed && ((int)System.currentTimeMillis()< now + waitTime) )
+	    {
+	      Thread.yield(); //don't exit till suppressed
+	    }
+   }
+   
+   protected void waitForMovement()
+   {
+	    while (!_suppressed && (pilot.isMoving()) )
+	    {
+	      Thread.yield(); //don't exit till suppressed
+	    }
+   }
+   
+}
\ No newline at end of file

Lesson11/Programs/ArenaBot/src/BetterRandom.java

+import java.util.Random;
+
+
+public class BetterRandom extends Random {
+	
+	@Override
+	public boolean nextBoolean()
+	{
+		//since nextInt(int n) always returns an uneven number, nextBoolean will currently always return true. This fixes that.
+		int unevenNumber = nextInt(101);
+		return unevenNumber > 50;
+	}
+}

Lesson11/Programs/ArenaBot/src/SlaveIOStreams.java

+import java.io.DataInputStream;
+import java.io.DataOutputStream;
+
+import lejos.nxt.LCD;
+import lejos.nxt.Sound;
+import lejos.nxt.comm.Bluetooth;
+import lejos.nxt.comm.USB;
+import lejos.nxt.comm.NXTConnection;
+
+public class SlaveIOStreams {
+
+	private NXTConnection conn;
+	private boolean usb;
+	private DataOutputStream dOut;
+    private DataInputStream dIn;
+    
+	public SlaveIOStreams(boolean usb)
+	{
+		this.usb = usb;
+	}
+	
+	public void open()
+	{
+		Sound.beep();
+		
+        if ( usb ){
+        	LCD.drawString("Waiting USB", 0, 0);       
+        	conn = USB.waitForConnection();
+        	LCD.drawString("Connected USB", 0, 0);
+        }
+        else
+        {
+        	LCD.drawString("Waiting BT", 0, 0);         
+        	conn = Bluetooth.waitForConnection();
+        	LCD.drawString("Connected BT", 0, 0);
+        }
+        
+        Sound.twoBeeps();
+        
+		dIn  = conn.openDataInputStream();
+		dOut = conn.openDataOutputStream();
+	}
+	
+	public float input()
+	{
+		float result;
+        try
+        {
+            result = dIn.readFloat();
+        }
+        catch (Exception e) 
+        {
+            result = -1;
+        } 
+        return result;
+
+	}
+	
+	public boolean output(float r)
+	{ 	
+		boolean result;
+        try
+        {
+    		dOut.writeFloat(r);
+    		dOut.flush();
+    		result = true;
+        }
+        catch (Exception e) 
+        {
+        	result = false;
+        } 
+        return result;
+	}
+	
+	public boolean close()
+	{
+		boolean result;
+        try
+        {   
+        	dOut.close();
+        	dIn.close();
+            conn.close();
+        	result = true;
+        }
+        catch (Exception e) 
+        {
+        	result = false;
+        }         
+        return result;
+	}
+
+}

Lesson11/Programs/ArenaBot/src/StayInRing.java

+import lejos.nxt.*;
+import lejos.robotics.navigation.DifferentialPilot;
+
+
+public class StayInRing extends Behavior {
+
+	LightSensor rightLight = new LightSensor(SensorPort.S1, true);
+	LightSensor leftLight = new LightSensor(SensorPort.S4, true);
+	TouchSensor touchSensor = new TouchSensor(SensorPort.S2);
+	int rightThreshold = 0;
+	int leftThreshold = 0;
+	final int ActivePrio = Integer.MAX_VALUE; //self preservation!
+	
+	final int ROTATION = 200;
+	
+	volatile int rlightVal = 0;
+	volatile int llightVal = 0;
+	volatile boolean touch = false;
+	
+	boolean isEdgeAtRight = false;
+	boolean isEdgeAtLeft = false;
+	int whiteDetections = 0;
+	
+	Thread lightReader;
+	
+	public StayInRing(DifferentialPilot pilot)
+	{
+		this.pilot = pilot;
+		lightReader = new Thread(new Runnable() {
+			
+			@Override
+			public void run() {
+				while(true)
+				{
+					rlightVal = rightLight.getLightValue();
+					llightVal = leftLight.getLightValue();
+					touch = touchSensor.isPressed();
+				}				
+			}
+		});
+		
+		lightReader.setDaemon(true);
+		lightReader.start();
+	}
+	
+	public void calibrate()
+	{
+		int rblackLight = 0, rwhiteLight = 0;
+		int lblackLight = 0, lwhiteLight = 0;
+		
+		while(Button.ENTER.isDown());
+		
+		LCD.clear();
+		LCD.drawString("Calibrate black.", 0, 0);
+		while(Button.ENTER.isUp())
+		{
+			rblackLight = rightLight.getLightValue();
+			lblackLight = leftLight.getLightValue();
+			LCD.drawInt(rblackLight, 3, 4, 1);
+			LCD.drawInt(lblackLight, 3, 0, 1);
+		}
+		
+		while(Button.ENTER.isDown());
+		
+		LCD.clear();
+		LCD.drawString("Calibrate white.", 0, 0);
+		while(Button.ENTER.isUp())
+		{
+			rwhiteLight = rightLight.getLightValue();
+			lwhiteLight = leftLight.getLightValue();
+			LCD.drawInt(rwhiteLight, 3, 4, 1);
+			LCD.drawInt(lwhiteLight, 3, 0, 1);
+		}
+		
+		final float alpha = 0.3f;
+		
+		rightThreshold = (int) (rblackLight * (1-alpha) + rwhiteLight*alpha);
+		leftThreshold = (int)(lblackLight * (1-alpha) + lwhiteLight*alpha);
+		
+		LCD.clear();
+		LCD.drawString("Thrsh: ", 0, 0);
+		LCD.drawInt(rightThreshold, 3, 11, 0);
+		LCD.drawInt(leftThreshold, 3, 7, 0);
+		
+		while(Button.ENTER.isDown());
+	}
+	
+	@Override
+	public int takeControl() {
+
+		isEdgeAtRight = rlightVal < rightThreshold;
+		isEdgeAtLeft = llightVal < leftThreshold;
+		
+		return isEdgeAtRight || isEdgeAtLeft || touch ? ActivePrio : 0;
+	}
+
+	@Override
+	public void action() {
+		super.action();
+		
+		//take local copies, since takeControl is called continuously, updating these variables.
+		boolean _isEdgeAtRight = isEdgeAtRight;
+		boolean _isEdgeAtLeft = isEdgeAtLeft;
+		boolean _touch = touch;
+		LCD.drawString("Light or touch!", 0, 4);
+		pilot.travel(-10, true);
+		waitForMovement();
+		
+	    LCD.clear(4);
+	    
+	    if(_isEdgeAtRight && _isEdgeAtLeft)
+	    {
+	    	LCD.drawString("Both light!", 0, 4);
+	    	pilot.rotate(90, true); 
+		    waitForMovement();
+	    }
+	    else if (_touch)
+	    {
+	    	LCD.drawString("Touch!", 0, 4);
+	    	pilot.rotate(90, true); 
+		    waitForMovement();
+	    }
+	    else if (_isEdgeAtRight)
+		{
+	    	pilot.rotate(45, true); 
+		    LCD.drawString("Light right!", 0, 4);
+		    waitForMovement();
+		}
+		else if (_isEdgeAtLeft)
+		{
+			pilot.rotate(-45, true); 
+		    LCD.drawString("Light left!", 0, 4);
+		    waitForMovement();
+		}
+	    ArenaBot.show();
+	    LCD.clear(4);
+	}
+}

Lesson11/Programs/ArenaBot/src/Wander.java

+import java.util.Random;
+
+import lejos.nxt.LCD;
+import lejos.robotics.navigation.DifferentialPilot;
+
+
+public class Wander extends Behavior {	
+	
+	final int MINANGLE = 10;
+	final int MAXANGLE = 180;
+	final int MINFORWARD = 10;
+	final int MAXFORWARD = 50;
+		
+	Random rnd = new BetterRandom();
+	
+	public Wander(DifferentialPilot pilot) {
+		this.pilot = pilot;
+	}
+	
+	@Override
+	public int takeControl() {
+		return 1; //lowest active priority. Always wander when nothing else happens.
+	}
+	
+	@Override
+	public void action() {
+		super.action();
+		
+		boolean doRotation = rnd.nextBoolean();
+    	
+    	if(doRotation)
+    	{
+    		boolean turnRight = rnd.nextBoolean();
+    		int turnSign = turnRight ? 1 : -1;
+    		int angle = MINANGLE + rnd.nextInt(MAXANGLE-MINANGLE);		    		
+    		LCD.drawString("Rotate deg: " + turnSign*angle, 0, 3);
+    		pilot.rotate(turnSign*angle, true);