Merge branch 'master' of github.com:dopefishh/des2015

[des2015.git] / dsl / runtime / src / nl / ru / des / BasicBehaviour.java

diff --git a/dsl/runtime/src/nl/ru/des/BasicBehaviour.java b/dsl/runtime/src/nl/ru/des/BasicBehaviour.java
index 5095222..8fd1fd3 100644 (file)
--- a/dsl/runtime/src/nl/ru/des/BasicBehaviour.java
+++ b/dsl/runtime/src/nl/ru/des/BasicBehaviour.java
@@ -1,22 +1,26 @@
 package nl.ru.des;
 
-import lejos.hardware.motor.EV3LargeRegulatedMotor;
+import lejos.robotics.RegulatedMotor;
 import lejos.robotics.subsumption.Behavior;
 import nl.ru.des.sensors.SensorCollector;
 
 public abstract class BasicBehaviour implements Behavior{
-       protected boolean suppressed;
-       protected EV3LargeRegulatedMotor leftMotor, rightMotor;
+       protected enum SuppressedState {
+               IDLE, IN_ACTION, SUPPRESSED;
+       }
+       private SuppressedState suppressed;
+       protected RegulatedMotor leftMotor, rightMotor, measMotor;
        protected SensorCollector sensors;
        protected long time;
        
-       public BasicBehaviour(SensorCollector sensors, EV3LargeRegulatedMotor leftMotor,
-                       EV3LargeRegulatedMotor rightMotor){
+       public BasicBehaviour(SensorCollector sensors, RegulatedMotor leftMotor,
+                       RegulatedMotor rightMotor, RegulatedMotor measMotor){
                this.leftMotor = leftMotor;
                this.rightMotor = rightMotor;
+               this.measMotor = measMotor;
                this.sensors = sensors;
        }
-       
+
        protected void reset(){
                rightMotor.setSpeed(Constants.speed);
                rightMotor.setAcceleration(Constants.acceleration);
@@ -24,20 +28,69 @@ public abstract class BasicBehaviour implements Behavior{
                leftMotor.setAcceleration(Constants.acceleration);
                rightMotor.stop(true);
                leftMotor.stop(true);
+               setSuppressed(SuppressedState.IDLE);
+               long time = System.currentTimeMillis();
+               while(System.currentTimeMillis()-time > 250){
+                       Thread.yield();
+               }
+       }
+
+       protected void measure(){
+               measMotor.backward();
+               while(getSuppressed() == SuppressedState.IN_ACTION && !measMotor.isStalled()){
+                       Thread.yield();
+               }
+               measMotor.forward();
+               while(getSuppressed() == SuppressedState.IN_ACTION && !measMotor.isStalled()){
+                       Thread.yield();
+               }
+               measMotor.stop(true);
+               rightTurn(90);
+       }
+       
+       protected void rightTurn(int angle){
+               rightMotor.stop();
+               leftMotor.stop();
+               sensors.resetGyro();
+               rightMotor.backward();
+               leftMotor.forward();
+               while(getSuppressed() == SuppressedState.IN_ACTION && Math.abs(sensors.gyro()) < angle){
+                       Thread.yield();
+               }
+               LCDPrinter.print(Float.toString(sensors.gyro()));
+       }
+       
+       protected void leftTurn(int angle){
+               rightMotor.stop();
+               leftMotor.stop();
+               sensors.resetGyro();
+               leftMotor.backward();
+               rightMotor.forward();
+               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 = false;
+               setSuppressed(SuppressedState.IN_ACTION);
        }
 
        @Override
        public void suppress() {
-               suppressed = true;
+               setSuppressed(SuppressedState.SUPPRESSED);
        }
        
-       @Override
-       public boolean takeControl() {
+       @Override public boolean takeControl(){
                return true;
        }
 }