From: Natanael Adityasatria Date: Mon, 14 Dec 2015 11:27:50 +0000 (+0100) Subject: Implemented measure motor X-Git-Url: https://git.martlubbers.net/?a=commitdiff_plain;h=3a12cfb82749c7ca2cec2d31da1b4d62ffe9e7e2;p=des2015.git Implemented measure motor --- diff --git a/dsl/runtime/specs/wander.tdsl b/dsl/runtime/specs/wander.tdsl index a51a064..7bc0ef5 100644 --- 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 --- 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 buffer = new LinkedList(); + private static Deque buffer = new LinkedList(); 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 --- 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 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 --- 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 --- 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 --- 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 --- 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(); } }