final commit

[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 e55558d..945a709 100644 (file)
--- a/dsl/runtime/src/nl/ru/des/BasicBehaviour.java
+++ b/dsl/runtime/src/nl/ru/des/BasicBehaviour.java
@@ -5,10 +5,8 @@ import lejos.robotics.subsumption.Behavior;
 import nl.ru.des.sensors.SensorCollector;
 
 public abstract class BasicBehaviour implements Behavior{
-       protected enum SuppressedState {
-               IDLE, IN_ACTION, SUPPRESSED;
-       }
-       protected SuppressedState suppressed;
+       protected boolean suppressed;
+       protected boolean finished;
        protected RegulatedMotor leftMotor, rightMotor, measMotor;
        protected SensorCollector sensors;
        protected long time;
@@ -19,63 +17,149 @@ public abstract class BasicBehaviour implements Behavior{
                this.rightMotor = rightMotor;
                this.measMotor = measMotor;
                this.sensors = sensors;
+               suppressed = false;
+               finished = true;
        }
 
        protected void reset(){
+               finished = true;
                rightMotor.setSpeed(Constants.speed);
                rightMotor.setAcceleration(Constants.acceleration);
                leftMotor.setSpeed(Constants.speed);
                leftMotor.setAcceleration(Constants.acceleration);
                rightMotor.stop(true);
                leftMotor.stop(true);
-               suppressed = SuppressedState.IDLE;
+               measMotor.stop(true);
        }
 
-       protected void measure(){
+       protected void measureLake(){
+               long time = System.currentTimeMillis();
+               rightMotor.backward();
+               leftMotor.backward();
+               while(System.currentTimeMillis()-time < 250){
+                       Thread.yield();
+               }
+               
+               rightMotor.stop(true);
+               leftMotor.stop(true);
+               rightMotor.setSpeed(25);
+               leftMotor.setSpeed(25);
+               if(sensors.leftLight() && !sensors.rightLight()){
+                       leftTurn(10);
+               } else if (sensors.rightLight() && !sensors.leftLight()){
+                       rightTurn(10);
+               }
+               
+               rightMotor.stop(true);
+               leftMotor.stop(true);
+               
+               measMotor.rotate(-100, true);
+               while(!suppressed && !measMotor.isStalled() && measMotor.isMoving()){
+                       Thread.yield();
+               }
+               measMotor.rotate(100, true);
+               while(!suppressed && !measMotor.isStalled() && measMotor.isMoving()){
+                       Thread.yield();
+               }
+               measMotor.stop(true);
+               
+               reset();
+               rightMotor.backward();
+               leftMotor.backward();
+               while(System.currentTimeMillis()-time < 250){
+                       Thread.yield();
+               }
+               turnRandom(45, 60);
+       }
+       
+       protected void measureRock(){
+               long time = System.currentTimeMillis();
+               rightMotor.forward();
+               leftMotor.forward();
+               while(System.currentTimeMillis()-time<1000){
+                       Thread.yield();
+               }
+               
+               rightMotor.stop(true);
+               leftMotor.stop(true);
+               
                measMotor.backward();
-               while(suppressed == SuppressedState.IN_ACTION && !measMotor.isStalled()){
+               while(!suppressed && !measMotor.isStalled()){
                        Thread.yield();
                }
                measMotor.forward();
-               while(suppressed == SuppressedState.IN_ACTION && !measMotor.isStalled()){
+               while(!suppressed && !measMotor.isStalled()){
                        Thread.yield();
                }
                measMotor.stop(true);
-               rightTurn(90);
+               
+               rightMotor.backward();
+               leftMotor.backward();
+               while(System.currentTimeMillis()-time<1000){
+                       Thread.yield();
+               }
+               turnRandom(30, 45);
+       }
+       
+       protected void turnRandom(int from, int to){
+               int degrees = Marster.random.nextInt(to-from)+from;
+               if(Marster.random.nextBoolean()){
+                       rightTurn(degrees);
+               } else {
+                       leftTurn(degrees);
+               }
        }
        
        protected void rightTurn(int angle){
-               rightMotor.stop();
+               rightMotor.stop(true);
                leftMotor.stop();
                sensors.resetGyro();
                rightMotor.backward();
                leftMotor.forward();
-               while(suppressed == SuppressedState.IN_ACTION && Math.abs(sensors.gyro()) < angle){
+               while(!suppressed && Math.abs(sensors.gyro()) < angle){
                        Thread.yield();
                }
-               LCDPrinter.print(Float.toString(sensors.gyro()));
+               rightMotor.stop(true);
+               leftMotor.stop(true);
+               System.out.println(Float.toString(sensors.gyro()));
        }
        
        protected void leftTurn(int angle){
-               rightMotor.stop();
+               rightMotor.stop(true);
                leftMotor.stop();
                sensors.resetGyro();
                leftMotor.backward();
                rightMotor.forward();
-               while(suppressed == SuppressedState.IN_ACTION && Math.abs(sensors.gyro()) < angle){
+               while(!suppressed && Math.abs(sensors.gyro()) < angle){
                        Thread.yield();
                }
-               LCDPrinter.print(Float.toString(sensors.gyro()));
+               rightMotor.stop(true);
+               leftMotor.stop(true);
+               System.out.println(Float.toString(sensors.gyro()));
        }
        
        @Override
        public void action() {
-               suppressed = SuppressedState.IN_ACTION;
+               suppressed = false;
+               finished = false;
+               rightMotor.setSpeed(Constants.speed);
+               rightMotor.setAcceleration(Constants.acceleration);
+               leftMotor.setSpeed(Constants.speed);
+               leftMotor.setAcceleration(Constants.acceleration);
+               rightMotor.stop(true);
+               leftMotor.stop(true);
        }
 
        @Override
        public void suppress() {
-               suppressed = SuppressedState.SUPPRESSED;
+               suppressed = true;
+               finished = true;
+               rightMotor.setSpeed(Constants.speed);
+               rightMotor.setAcceleration(Constants.acceleration);
+               leftMotor.setSpeed(Constants.speed);
+               leftMotor.setAcceleration(Constants.acceleration);
+               rightMotor.stop(true);
+               leftMotor.stop(true);
        }
        
        @Override public boolean takeControl(){