Add PilorRoute with Avoid

Lesson11/Programs/PilotRouteAvoid/.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/PilotRouteAvoid/.project

+<?xml version="1.0" encoding="UTF-8"?>
+<projectDescription>
+	<name>PilotRouteAvoid</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/PilotRouteAvoid/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/PilotRouteAvoid/src/Avoid.java

+import lejos.nxt.*;
+import lejos.robotics.navigation.DifferentialPilot;
+
+
+public class Avoid extends Behavior {
+
+	UltrasonicSensor distSensor = new UltrasonicSensor(SensorPort.S3);
+	final int ActivePrio = Integer.MAX_VALUE; //self preservation!
+	
+	volatile int distance = 0;
+	
+	Thread distReader;
+	
+	public Avoid(DifferentialPilot pilot)
+	{
+		this.pilot = pilot;
+		distReader = new Thread(new Runnable() {
+			
+			@Override
+			public void run() {
+				while(true)
+				{
+					distance = distSensor.getDistance();
+				}				
+			}
+		});
+		
+		distReader.setDaemon(true);
+		distReader.start();
+	}
+	
+	@Override
+	public int takeControl() {
+		return distance < 10 ? ActivePrio : 0;
+	}
+
+	@Override
+	public void action() {
+		super.action();
+		
+		LCD.drawString("Avoid!", 0, 4);
+		pilot.travel(-10, true);
+		waitForMovement();
+		
+	    PilotRouteAvoid.updatePosition();
+	    LCD.clear(4);
+	}
+}

Lesson11/Programs/PilotRouteAvoid/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();
+	   PilotRouteAvoid.updatePosition();
+   }
+   
+   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/PilotRouteAvoid/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/PilotRouteAvoid/src/Drive.java

+import lejos.nxt.LCD;
+import lejos.robotics.navigation.DifferentialPilot;
+
+
+public class Drive extends Behavior {	
+	
+	public Drive(DifferentialPilot pilot) {
+		this.pilot = pilot;
+	}
+	
+	@Override
+	public int takeControl() {
+		return 1; //lowest active priority. Always drive when nothing else happens.
+	}
+	
+	@Override
+	public void action() {
+		super.action();
+		int travelDist = 1;
+    	LCD.drawString("Travel dist: " + travelDist, 0, 3);
+    	pilot.travel(travelDist, true);
+    	waitForMovement();
+    	
+    	PilotRouteAvoid.updatePosition();
+    	LCD.clear(3);
+	}
+
+}

Lesson11/Programs/PilotRouteAvoid/src/PilotRouteAvoid.java

+import lejos.nxt.*;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.robotics.navigation.Move;
+import lejos.util.Delay;
+
+public class PilotRouteAvoid
+{
+    private static SlaveIOStreams PC;
+    private static double distFactor = 0.99;
+    private static double wheelDiameterL = 5.595*distFactor, wheelDiameterR = 5.6*distFactor, trackWidth = 16.27;
+	private static double travelSpeed = 5, rotateSpeed = 45;
+	private static NXTRegulatedMotor left = Motor.B;
+	private static NXTRegulatedMotor right = Motor.C;
+	private static LightSensor lightsensor = new LightSensor(SensorPort.S4);
+	   
+	private static DifferentialPilot pilot = new DifferentialPilot(wheelDiameterL,wheelDiameterR,trackWidth, left, right,false);
+	
+	private static void sendMove(Move move)
+	{	
+		PC.output((move.getMoveType() == Move.MoveType.TRAVEL? 0:1 ));
+		PC.output(move.getDistanceTraveled() * 10);
+		PC.output(move.getAngleTurned());
+	}
+	
+	private static void sendLightValue(int value)
+	{
+		PC.output(value);
+	}
+	
+	public static void updatePosition() {
+		sendMove(pilot.getMovement());
+		sendLightValue(lightsensor.getLightValue());
+	}
+	
+	
+	public static void main(String[] args) 
+	{	
+		Button.ESCAPE.addButtonListener(new ButtonListener() {
+    	   @Override
+    	   public void buttonReleased(Button b) {			
+    	   }
+    	   
+    	   @Override
+    	   public void buttonPressed(Button b) {
+    		   PC.close();
+    		   System.exit(0); 				
+    	   }
+		});
+		
+		PC = new SlaveIOStreams(false);
+    	PC.open();
+ 	   
+ 	    pilot.setTravelSpeed(travelSpeed);
+ 	    pilot.setRotateSpeed(rotateSpeed);
+		
+		Avoid avoid = new Avoid(pilot);
+		
+		Drive drive = new Drive(pilot);
+		
+		Behavior[] behaviorList =
+		{
+			drive, avoid
+		};
+		
+		Arbitrator arbitrator = new Arbitrator(behaviorList); 
+		
+		LCD.drawString("Pilot route", 0, 0);
+		Button.waitForAnyPress();
+		LCD.clear();
+		Delay.msDelay(100);
+		
+		arbitrator.start();
+	}
+}

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