X-Git-Url: https://git.martlubbers.net/?a=blobdiff_plain;f=dsl%2Fruntime%2Fsrc%2Fnl%2Fru%2Fdes%2FBasicBehaviour.java;h=945a70986b945a268298deb91de7bb3cb9d86cb4;hb=3e5a839d3f540c49d5b903bfebdd8913e7deef9a;hp=e55558d459010b83fd7f53cf77ef9e4ff312dd49;hpb=9e9aab249373630a777c0407f08e605b50a24d2e;p=des2015.git

diff --git a/dsl/runtime/src/nl/ru/des/BasicBehaviour.java b/dsl/runtime/src/nl/ru/des/BasicBehaviour.java
index e55558d..945a709 100644
--- 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(){