Implemented measure motor

diff --git a/dsl/runtime/specs/wander.tdsl b/dsl/runtime/specs/wander.tdsl
index a51a064..7bc0ef5 100644 (file)
--- a/dsl/runtime/specs/wander.tdsl
+++ b/dsl/runtime/specs/wander.tdsl
@@ -1,29 +1,25 @@
 Acceleration 1000
-Speed 250
+Speed 150
 Behaviour Wander
        take control:
        action:
                left motor forward
                right motor forward
                wait forever
-
-Behaviour StayInFieldBoth
-       take control: (&& Light on left Light on right)
-       action:
-               left motor backward with speed 1000 acceleration 10000
-               right motor backward with speed 250 acceleration 10000
-               wait 1000 ms
 Behaviour StayInFieldL
        take control: Light on left
        action:
-               turn right 90 with speed 100 acceleration 10000
+               turn right 90
 Behaviour StayInFieldR
        take control: Light on right
        action:
-               turn left 90 with speed 100 acceleration 10000
+               turn left 90
 Behaviour StayInFieldB
        take control: Distance dangerous at back
        action:
                wait 750 ms
-        
-Mission stayinfield using Wander StayInFieldL StayInFieldR StayInFieldB StayInFieldBoth and stops when Color is Cyan
\ No newline at end of file
+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

diff --git a/dsl/runtime/src/nl/ru/des/BasicBehaviour.java b/dsl/runtime/src/nl/ru/des/BasicBehaviour.java
index 9b7742b..fb70588 100644 (file)
--- a/dsl/runtime/src/nl/ru/des/BasicBehaviour.java
+++ b/dsl/runtime/src/nl/ru/des/BasicBehaviour.java
@@ -5,7 +5,10 @@ import lejos.robotics.subsumption.Behavior;
 import nl.ru.des.sensors.SensorCollector;
 
 public abstract class BasicBehaviour implements Behavior{
-       protected int suppressed;
+       protected enum SuppressedState {
+               IDLE, IN_ACTION, SUPPRESSED;
+       }
+       protected SuppressedState suppressed;
        protected RegulatedMotor leftMotor, rightMotor, measMotor;
        protected SensorCollector sensors;
        protected long time;
@@ -25,44 +28,54 @@ public abstract class BasicBehaviour implements Behavior{
                leftMotor.setAcceleration(Constants.acceleration);
                rightMotor.stop(true);
                leftMotor.stop(true);
-               suppressed = 0;
+               suppressed = SuppressedState.IDLE;
        }
 
-       protected void rockMeasure(){
-               
-       }
-       
-       protected void lakeMeasure(){
-               
+       protected void measure(){
+               measMotor.backward();
+               while(!measMotor.isStalled()){
+                       Thread.yield();
+               }
+               measMotor.forward();
+               while(!measMotor.isStalled()){
+                       Thread.yield();
+               }
+               measMotor.stop(true);
+               rightTurn(90);
        }
        
        protected void rightTurn(int angle){
-               float init = sensors.gyro();
+               rightMotor.stop();
+               leftMotor.stop();
+               sensors.resetGyro();
                rightMotor.backward();
                leftMotor.forward();
-
-               while(suppressed != 2 && sensors.gyro()-90>init){
+               while(suppressed != SuppressedState.SUPPRESSED && Math.abs(sensors.gyro()) < angle){
                        Thread.yield();
                }
+               LCDPrinter.print(Float.toString(sensors.gyro()));
        }
        
        protected void leftTurn(int angle){
-               float init = sensors.gyro();
+               rightMotor.stop();
+               leftMotor.stop();
+               sensors.resetGyro();
                leftMotor.backward();
                rightMotor.forward();
-               while(suppressed != 2 && sensors.gyro()+90<init){
+               while(suppressed != SuppressedState.SUPPRESSED && Math.abs(sensors.gyro()) < angle){
                        Thread.yield();
                }
+               LCDPrinter.print(Float.toString(sensors.gyro()));
        }
        
        @Override
        public void action() {
-               suppressed = 1;
+               suppressed = SuppressedState.IN_ACTION;
        }
 
        @Override
        public void suppress() {
-               suppressed = 2;
+               suppressed = 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 ea39f52..fdfcd60 100644 (file)
--- a/dsl/runtime/src/nl/ru/des/LCDPrinter.java
+++ b/dsl/runtime/src/nl/ru/des/LCDPrinter.java
@@ -11,8 +11,18 @@ import lejos.utility.Delay;
 
 public class LCDPrinter{
        public static final int PRINTDELAY = 50;
+       
+       private static class Message{
+               public String msg;
+               public boolean nl;
+               
+               public Message(String msg, boolean nl){
+                       this.msg = msg;
+                       this.nl = nl;
+               }
+       }
 
-       private static Deque<String> buffer = new LinkedList<String>();
+       private static Deque<Message> buffer = new LinkedList<Message>();
 
        public static void startLCDPrinter(final TextLCD glcd) {
                new Thread(new Runnable(){
@@ -20,13 +30,17 @@ public class LCDPrinter{
                        public void run() {
                                while (true) {
                                        if (!buffer.isEmpty()) {
-                                               String c = buffer.remove();
-                                               if(c.length() > glcd.getTextWidth()){
-                                                       buffer.addFirst(c.substring(glcd.getTextWidth(), c.length()));
-                                                       c = c.substring(0, glcd.getTextWidth());
+                                               Message c = buffer.remove();
+                                               if(c.msg.length() > glcd.getTextWidth()){
+                                                       buffer.addFirst(new Message(c.msg.substring(glcd.getTextWidth(), c.msg.length()), c.nl));
+                                                       c.msg = c.msg.substring(0, glcd.getTextWidth());
+                                               }
+                                               if(c.nl){
+                                                       glcd.scroll();
+                                               } else {
+                                                       glcd.clear(glcd.getTextHeight()-1);
                                                }
-                                               glcd.scroll();
-                                               glcd.drawString(c, 0, glcd.getTextHeight()-1);
+                                               glcd.drawString(c.msg, 0, glcd.getTextHeight()-1);
                                        }
                                        Delay.msDelay(PRINTDELAY);
                                }
@@ -36,7 +50,11 @@ public class LCDPrinter{
        }
        
        public static void print(String s){
-               buffer.addLast(s);
+               print(s, true);
+       }
+       
+       public static void print(String s, boolean nl){
+               buffer.addLast(new Message(s, nl));
        }
        
        public static PrintStream getPrefixedPrintstream(final String prefix, final TextLCD glcd){

diff --git a/dsl/runtime/src/nl/ru/des/Marster.java b/dsl/runtime/src/nl/ru/des/Marster.java
index ad31306..22ddb06 100644 (file)
--- a/dsl/runtime/src/nl/ru/des/Marster.java
+++ b/dsl/runtime/src/nl/ru/des/Marster.java
@@ -62,11 +62,11 @@ public class Marster {
                        RegulatedMotor measMotor = new EV3MediumRegulatedMotor(MotorPort.C);
                        leftMotor.setSpeed(Constants.speed);
                        rightMotor.setSpeed(Constants.speed);
-                       measMotor.setSpeed(100);
+                       measMotor.setSpeed(50);
                        rightMotor.setAcceleration(Constants.acceleration);
                        leftMotor.setAcceleration(Constants.acceleration);
                        measMotor.setAcceleration(100);
-                       
+                                               
                        LCDPrinter.print("Loading touch sensors...");
                        SampleProvider leftLight = new NXTLightSensor(brick.getPort("S1")).getRedMode();
                        SampleProvider rightLight = new NXTLightSensor(brick.getPort("S2")).getRedMode();
@@ -75,9 +75,8 @@ public class Marster {
                        SampleProvider backUltra = new EV3UltrasonicSensor(brick.getPort("S3")).getDistanceMode();
                        
                        LCDPrinter.print("Loading gyro sensor...");
-                       SampleProvider gyro = new EV3GyroSensor(brick.getPort("S4")).getAngleAndRateMode();
+                       SensorCollector sc = new SensorCollector(backUltra, leftLight, rightLight, new EV3GyroSensor(brick.getPort("S4")));
 
-                       SensorCollector sc = new SensorCollector(backUltra, leftLight, rightLight, gyro);
                        BTController.startMaster(slave, sc);
                        LCDPrinter.print("Finished loading");
                        LinkedList<Mission> missions = Missions.getMissions(sc, rightMotor, leftMotor, measMotor);

diff --git a/dsl/runtime/src/nl/ru/des/sensors/RemoteSensors.java b/dsl/runtime/src/nl/ru/des/sensors/RemoteSensors.java
index 65bc067..642a5e6 100644 (file)
--- a/dsl/runtime/src/nl/ru/des/sensors/RemoteSensors.java
+++ b/dsl/runtime/src/nl/ru/des/sensors/RemoteSensors.java
@@ -7,7 +7,7 @@ import lejos.utility.Delay;
 import nl.ru.des.LCDPrinter;
 
 public class RemoteSensors{
-       public static final int DELAY = 100;
+       public static final int DELAY = 200;
        private SampleProvider left, right, ultra, color;
        private float[] leftSamples, rightSamples, ultraSamples, colorSamples;
        private float leftLatest, rightLatest, ultraLatest, colorLatest;

diff --git a/dsl/runtime/src/nl/ru/des/sensors/SensorCollector.java b/dsl/runtime/src/nl/ru/des/sensors/SensorCollector.java
index a9c2048..dcf2d87 100644 (file)
--- a/dsl/runtime/src/nl/ru/des/sensors/SensorCollector.java
+++ b/dsl/runtime/src/nl/ru/des/sensors/SensorCollector.java
@@ -1,17 +1,17 @@
 package nl.ru.des.sensors;
 
-import lejos.hardware.lcd.LCD;
+import lejos.hardware.sensor.EV3GyroSensor;
 import lejos.robotics.SampleProvider;
-import lejos.utility.Delay;
 
 public class SensorCollector implements MessageHandler{
-       public static final int DELAY = 100;
+       public static final int DELAY = 200;
 
        private static final float DANGER_DISTANCE_FRONT = 0.15f;
        private static final float DANGER_DISTANCE_BACK = 0.035f;
        private static final float DANGER_LIGHT = 0.45f;
        
        //Local sensors
+       private EV3GyroSensor gyroSensor;
        private SampleProvider ultra, leftLight, rightLight, gyro;
        private float[] ultraSamples, leftLightSamples, rightLightSamples, gyroSamples;
        private long ultraTime, leftLightTime, rightLightTime, gyroTime;
@@ -24,11 +24,12 @@ public class SensorCollector implements MessageHandler{
        public SensorCollector(SampleProvider ultra,
                        SampleProvider leftLight,
                        SampleProvider rightLight,
-                       SampleProvider gyro){
+                       EV3GyroSensor gyro){
                this.ultra = ultra;
                this.leftLight = leftLight;
                this.rightLight = rightLight;
-               this.gyro = gyro;
+               this.gyroSensor = gyro;
+               this.gyro = gyro.getAngleMode();
                ultraSamples = new float[ultra.sampleSize()];
                gyroSamples = new float[gyro.sampleSize()];
                leftLightSamples = new float[leftLight.sampleSize()];
@@ -72,6 +73,10 @@ public class SensorCollector implements MessageHandler{
                return gyroSamples[0];
        }
        
+       public void resetGyro(){
+               gyroSensor.reset();
+       }
+       
        //Remote sensors
        public boolean collected(int[] colors){
                return false;

diff --git a/dsl/xtend/src/robots/missions/TaskDSL.xtext b/dsl/xtend/src/robots/missions/TaskDSL.xtext
index 35ed5b0..afefa5d 100644 (file)
--- a/dsl/xtend/src/robots/missions/TaskDSL.xtext
+++ b/dsl/xtend/src/robots/missions/TaskDSL.xtext
@@ -30,7 +30,7 @@ Behaviour: 'Behaviour' name=ID
 Action:
        whichMotor=LeftRight 'motor' moveDir=Direction ('with speed' spd=INT 'acceleration' acc=INT)? |
        'turn' turnDir=LeftRight degrees=INT ('with speed' spd=INT 'acceleration' acc=INT)? |
-       'measure' measureWhat=RockLake |
+       {Action} 'measure'|
        'wait' time=Time;
 
 Time: time=INT 'ms' | {Time} 'forever';

diff --git a/dsl/xtend/src/robots/missions/generator/TaskDSLGenerator.xtend b/dsl/xtend/src/robots/missions/generator/TaskDSLGenerator.xtend
index b4c56ae..8e25b69 100644 (file)
--- a/dsl/xtend/src/robots/missions/generator/TaskDSLGenerator.xtend
+++ b/dsl/xtend/src/robots/missions/generator/TaskDSLGenerator.xtend
@@ -73,11 +73,12 @@ public class «b.name»Behaviour extends BasicBehaviour {
        }
        «IF b.tc != null»
        @Override public boolean takeControl(){
-               return suppressed == 1 || «printExpression(b.tc)»;
+               return suppressed == SuppressedState.IN_ACTION || «printExpression(b.tc)»;
        }
        «ENDIF»
        
        @Override public void action(){
+               LCDPrinter.print("Start: «b.name»");
                super.action();
                «FOR a : b.actions»
                        «IF a.whichMotor != null»
@@ -86,8 +87,11 @@ public class «b.name»Behaviour extends BasicBehaviour {
                                «a.whichMotor.d.toString()»Motor.setSpeed(«a.spd»);
                                «ENDIF»
                                «a.whichMotor.d.toString()»Motor.«a.moveDir.d.toString()»();
-                       «ELSEIF a.measureWhat != null»
-                       «a.measureWhat.d.toString()»Measure();
+                       «ELSEIF a.time != null»
+                       time = System.currentTimeMillis();
+                       while(suppressed != SuppressedState.SUPPRESSED«IF a.time.time > 0» && System.currentTimeMillis()-time>«a.time.time»«ENDIF»){
+                               Thread.yield(); 
+                       }
                        «ELSEIF a.turnDir != null»
                                «IF a.acc > 0»
                                        leftMotor.setAcceleration(«a.acc»);
@@ -97,12 +101,10 @@ public class «b.name»Behaviour extends BasicBehaviour {
                                «ENDIF»
                        «a.turnDir.d.toString()»Turn(«a.degrees»);
                        «ELSE»
-                       time = System.currentTimeMillis();
-                       while(suppressed != 2«IF a.time.time > 0» && System.currentTimeMillis()-time>«a.time.time»«ENDIF»){
-                               Thread.yield(); 
-                       }
+                       measure();
                        «ENDIF»
                «ENDFOR»
+               LCDPrinter.print("Stop: «b.name»");
                reset();
        }
 }