Implemented measure motor

[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 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(){