From: Mart Lubbers <mart@martlubbers.net>
Date: Tue, 5 Jan 2016 10:26:11 +0000 (+0100)
Subject: working multiple missions
X-Git-Url: https://git.martlubbers.net/?a=commitdiff_plain;h=7d9c29a46bae0afbc961e57f3013190b31ae5234;p=des2015.git

working multiple missions
---

diff --git a/dsl/runtime/specs/wander.tdsl b/dsl/runtime/specs/wander.tdsl
index 7bc0ef5..80cd169 100644
--- a/dsl/runtime/specs/wander.tdsl
+++ b/dsl/runtime/specs/wander.tdsl
@@ -22,4 +22,4 @@ Behaviour Measure
 	take control: Color is Green
 	action:
 		measure
-Mission stayinfield using Wander Measure StayInFieldL StayInFieldR and stops when Color is Cyan
\ No newline at end of file
+Mission stayinfield using Wander Measure StayInFieldL StayInFieldR and stops when Color is Blue
\ No newline at end of file
diff --git a/dsl/runtime/src/nl/ru/des/Arbitrator.java b/dsl/runtime/src/nl/ru/des/Arbitrator.java
new file mode 100644
index 0000000..c774d3f
--- /dev/null
+++ b/dsl/runtime/src/nl/ru/des/Arbitrator.java
@@ -0,0 +1,154 @@
+package nl.ru.des;
+
+import lejos.robotics.subsumption.Behavior;
+
+/**
+ * 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
+ */
+public class Arbitrator {
+
+	private final int NONE = -1;
+	private Behavior[] _behavior;
+	// highest priority behavior that wants control ; set by start() used by
+	// monitor
+	private int _highestPriority = NONE;
+	private int _active = NONE; // active behavior; set by monitor, used by
+								// start();
+	private boolean _returnWhenInactive;
+	private boolean stop;
+	/**
+	 * Monitor is an inner class. It polls the behavior array to find the
+	 * behavior of hightst priority. If higher than the active behavior, it
+	 * calls active.suppress()
+	 */
+	private Monitor monitor;
+
+	/**
+	 * Allocates an Arbitrator object and initializes it with an array of
+	 * Behavior objects. The index of a behavior in this array is its priority
+	 * level, so the behavior of the largest index has the highest the priority
+	 * level. The behaviors in an Arbitrator can not be changed once the
+	 * arbitrator is initialized.<BR>
+	 * <B>NOTE:</B> Once the Arbitrator is initialized, the method start() must
+	 * be called to begin the arbitration.
+	 * 
+	 * @param behaviorList
+	 *            an array of Behavior objects.
+	 * @param returnWhenInactive
+	 *            if <B>true</B>, the <B>start()</B> method returns when no
+	 *            Behavior is active.
+	 */
+	public Arbitrator(Behavior[] behaviorList, boolean returnWhenInactive) {
+		_behavior = behaviorList;
+		_returnWhenInactive = returnWhenInactive;
+		stop = false;
+		monitor = new Monitor();
+		monitor.setDaemon(true);
+	}
+
+	/**
+	 * Same as Arbitrator(behaviorList, false) Arbitrator start() never exits
+	 * 
+	 * @param behaviorList
+	 *            An array of Behavior objects.
+	 */
+	public Arbitrator(Behavior[] behaviorList) {
+		this(behaviorList, false);
+	}
+
+	/**
+	 * This method starts the arbitration of Behaviors and runs an endless loop.
+	 * <BR>
+	 * Note: Arbitrator does not run in a separate thread. The start() method
+	 * will never return unless <br>
+	 * 1. no action() method is running and <br>
+	 * 2. no behavior takeControl() returns <B> true </B> and <br>
+	 * 3. the <i>returnWhenInacative </i> flag is true,
+	 */
+	public void start() {
+		monitor.start();
+		while (_highestPriority == NONE) {
+			Thread.yield();// wait for some behavior to take contro
+		}
+		while (!stop) {
+			synchronized (monitor) {
+				if (_highestPriority != NONE) {
+					_active = _highestPriority;
+
+				} else if (_returnWhenInactive) {// no behavior wants to run
+					monitor.more = false;// 9 shut down monitor thread
+					return;
+				}
+			} // monitor released before action is called
+			if (_active != NONE) // _highestPrioirty could be NONE
+			{
+				_behavior[_active].action();
+				_active = NONE; // no active behavior at the moment
+			}
+			Thread.yield();
+		}
+	}
+
+	public void stop() {
+		this.monitor.more = false;
+		stop = true;
+	}
+
+	/**
+	 * Finds the highest priority behavior that returns <B>true </B> to
+	 * takeControl(); If this priority is higher than the active behavior, it
+	 * calls active.suppress(). If there is no active behavior, calls suppress()
+	 * on the most recently active behavior.
+	 */
+	private class Monitor extends Thread {
+
+		boolean more = true;
+		int maxPriority = _behavior.length - 1;
+
+		public void run() {
+			while (more) {
+				// FIND HIGHEST PRIORITY BEHAVIOR THAT WANTS CONTROL
+				synchronized (this) {
+					_highestPriority = NONE;
+					int active_behavior = _active; // BB modded
+					if (active_behavior == NONE)
+						active_behavior = 0; // BB modded
+					for (int i = maxPriority; i >= active_behavior; i--) // BB
+					{
+						if (_behavior[i].takeControl()) {
+							_highestPriority = i;
+							break;
+						}
+					}
+					int active = _active;// local copy: avoid out of bounds
+											// error in 134
+					if (active != NONE && _highestPriority > active) {
+						_behavior[active].suppress();
+					}
+				} // end synchronize block - main thread can run now
+				Thread.yield();
+			}
+		}
+	}
+}
+  
diff --git a/dsl/runtime/src/nl/ru/des/BasicBehaviour.java b/dsl/runtime/src/nl/ru/des/BasicBehaviour.java
index e55558d..8fd1fd3 100644
--- a/dsl/runtime/src/nl/ru/des/BasicBehaviour.java
+++ b/dsl/runtime/src/nl/ru/des/BasicBehaviour.java
@@ -8,7 +8,7 @@ public abstract class BasicBehaviour implements Behavior{
 	protected enum SuppressedState {
 		IDLE, IN_ACTION, SUPPRESSED;
 	}
-	protected SuppressedState suppressed;
+	private SuppressedState suppressed;
 	protected RegulatedMotor leftMotor, rightMotor, measMotor;
 	protected SensorCollector sensors;
 	protected long time;
@@ -28,16 +28,20 @@ public abstract class BasicBehaviour implements Behavior{
 		leftMotor.setAcceleration(Constants.acceleration);
 		rightMotor.stop(true);
 		leftMotor.stop(true);
-		suppressed = SuppressedState.IDLE;
+		setSuppressed(SuppressedState.IDLE);
+		long time = System.currentTimeMillis();
+		while(System.currentTimeMillis()-time > 250){
+			Thread.yield();
+		}
 	}
 
 	protected void measure(){
 		measMotor.backward();
-		while(suppressed == SuppressedState.IN_ACTION && !measMotor.isStalled()){
+		while(getSuppressed() == SuppressedState.IN_ACTION && !measMotor.isStalled()){
 			Thread.yield();
 		}
 		measMotor.forward();
-		while(suppressed == SuppressedState.IN_ACTION && !measMotor.isStalled()){
+		while(getSuppressed() == SuppressedState.IN_ACTION && !measMotor.isStalled()){
 			Thread.yield();
 		}
 		measMotor.stop(true);
@@ -50,7 +54,7 @@ public abstract class BasicBehaviour implements Behavior{
 		sensors.resetGyro();
 		rightMotor.backward();
 		leftMotor.forward();
-		while(suppressed == SuppressedState.IN_ACTION && Math.abs(sensors.gyro()) < angle){
+		while(getSuppressed() == SuppressedState.IN_ACTION && Math.abs(sensors.gyro()) < angle){
 			Thread.yield();
 		}
 		LCDPrinter.print(Float.toString(sensors.gyro()));
@@ -62,20 +66,28 @@ public abstract class BasicBehaviour implements Behavior{
 		sensors.resetGyro();
 		leftMotor.backward();
 		rightMotor.forward();
-		while(suppressed == SuppressedState.IN_ACTION && Math.abs(sensors.gyro()) < angle){
+		while(getSuppressed() == SuppressedState.IN_ACTION && Math.abs(sensors.gyro()) < angle){
 			Thread.yield();
 		}
 		LCDPrinter.print(Float.toString(sensors.gyro()));
 	}
 	
+	protected synchronized void setSuppressed(SuppressedState sup){
+		suppressed = sup;
+	}
+	
+	protected SuppressedState getSuppressed(){
+		return suppressed;
+	}
+	
 	@Override
 	public void action() {
-		suppressed = SuppressedState.IN_ACTION;
+		setSuppressed(SuppressedState.IN_ACTION);
 	}
 
 	@Override
 	public void suppress() {
-		suppressed = SuppressedState.SUPPRESSED;
+		setSuppressed(SuppressedState.SUPPRESSED);
 	}
 	
 	@Override public boolean takeControl(){
diff --git a/dsl/runtime/src/nl/ru/des/LCDPrinter.java b/dsl/runtime/src/nl/ru/des/LCDPrinter.java
index fdfcd60..c041f4d 100644
--- a/dsl/runtime/src/nl/ru/des/LCDPrinter.java
+++ b/dsl/runtime/src/nl/ru/des/LCDPrinter.java
@@ -53,7 +53,7 @@ public class LCDPrinter{
 		print(s, true);
 	}
 	
-	public static void print(String s, boolean nl){
+	public synchronized static void print(String s, boolean nl){
 		buffer.addLast(new Message(s, nl));
 	}
 	
diff --git a/dsl/runtime/src/nl/ru/des/Marster.java b/dsl/runtime/src/nl/ru/des/Marster.java
index 22ddb06..ee38341 100644
--- a/dsl/runtime/src/nl/ru/des/Marster.java
+++ b/dsl/runtime/src/nl/ru/des/Marster.java
@@ -16,8 +16,8 @@ import lejos.hardware.sensor.EV3UltrasonicSensor;
 import lejos.hardware.sensor.NXTLightSensor;
 import lejos.robotics.RegulatedMotor;
 import lejos.robotics.SampleProvider;
-import lejos.robotics.subsumption.Arbitrator;
 import nl.ru.des.sensors.BTController;
+import nl.ru.des.Arbitrator;
 import nl.ru.des.sensors.RemoteSensors;
 import nl.ru.des.sensors.SensorCollector;
 
@@ -84,6 +84,7 @@ public class Marster {
 				LCDPrinter.print("Start " + m.name + " mission...");
 				arb = new Arbitrator(m.behaviours);
 				arb.start();
+				LCDPrinter.print(m.name + " finished!!1one!");
 			}
 		}
 	}
diff --git a/dsl/xtend/src/robots/missions/generator/TaskDSLGenerator.xtend b/dsl/xtend/src/robots/missions/generator/TaskDSLGenerator.xtend
index 8e25b69..ff72ba4 100644
--- a/dsl/xtend/src/robots/missions/generator/TaskDSLGenerator.xtend
+++ b/dsl/xtend/src/robots/missions/generator/TaskDSLGenerator.xtend
@@ -73,7 +73,7 @@ public class «b.name»Behaviour extends BasicBehaviour {
 	}
 	«IF b.tc != null»
 	@Override public boolean takeControl(){
-		return suppressed == SuppressedState.IN_ACTION || «printExpression(b.tc)»;
+		return getSuppressed() == SuppressedState.IN_ACTION || «printExpression(b.tc)»;
 	}
 	«ENDIF»
 	
@@ -89,7 +89,7 @@ public class «b.name»Behaviour extends BasicBehaviour {
 				«a.whichMotor.d.toString()»Motor.«a.moveDir.d.toString()»();
 			«ELSEIF a.time != null»
 			time = System.currentTimeMillis();
-			while(suppressed != SuppressedState.SUPPRESSED«IF a.time.time > 0» && System.currentTimeMillis()-time>«a.time.time»«ENDIF»){
+			while(getSuppressed() != SuppressedState.SUPPRESSED«IF a.time.time > 0» && System.currentTimeMillis()-time>«a.time.time»«ENDIF»){
 				Thread.yield();	
 			}
 			«ELSEIF a.turnDir != null»