From 8880a95aedaf51c028818910b64f85472d96c14e Mon Sep 17 00:00:00 2001
From: Natanael Adityasatria <nata.adit@gmail.com>
Date: Thu, 7 Jan 2016 15:04:46 +0100
Subject: [PATCH] Implemented finding park space

---
 dsl/runtime/specs/wander.tdsl                 | 35 +++++++++++++++++--
 dsl/runtime/src/nl/ru/des/BasicBehaviour.java | 28 +++++++++++++--
 dsl/runtime/src/nl/ru/des/Marster.java        | 26 +++++++-------
 3 files changed, 71 insertions(+), 18 deletions(-)

diff --git a/dsl/runtime/specs/wander.tdsl b/dsl/runtime/specs/wander.tdsl
index d592a25..0f764d3 100644
--- a/dsl/runtime/specs/wander.tdsl
+++ b/dsl/runtime/specs/wander.tdsl
@@ -98,13 +98,42 @@ Behaviour AvoidHigh
 	take control: Distance dangerous at front
 	action: 
 		turn randomly 10 to 11 degrees
-		
+
+Behaviour MeasureRock
+	take control: (&& Distance dangerous at front not flag set RockMeasured)
+	action:
+		measure Rock
+		set flag RockMeasured
+
+Behaviour FindParkingL
+	take control: Light on left
+	action:
+		turn left exactly 10 degrees with speed 20 acceleration 1000
+
+Behaviour FindParkingR
+	take control: Light on right
+	action:
+		turn right exactly 10 degrees with speed 20 acceleration 1000
+
+Behaviour FindParkingSpace
+	take control: (&& Light on right Light on left)
+	action:
+		right motor backward with speed 80 acceleration 1000
+		left motor backward with speed 80 acceleration 1000
+		wait 1000 ms
+		turn right exactly 180 degrees
+		set flag Parked
+
 //Mission pushRock
 //	using Drive LocateR LocateL Push StayInFieldB StayInFieldL StayInFieldR and stops when Never
 //Mission avoidHighRocks
 //	using Drive AvoidHigh BumpR BumpL StayInFieldB StayInFieldL StayInFieldR and stops when Never
-Mission measureLakes
-	using Drive MeasureRedLake MeasureGreenLake MeasureBlueLake and stops when (&& flag set GreenMeasured flag set BlueMeasured flag set RedMeasured)
+Mission MeasureRocks
+	using Drive StayInFieldB StayInFieldL StayInFieldR MeasureRock and stops when flag set RockMeasured
+Mission FindParkingSpaceInTheCorner
+	using Wander StayInFieldB FindParkingL FindParkingR FindParkingSpace and stops when flag set Parked
+//Mission measureLakes
+//	using Drive MeasureRedLake MeasureGreenLake MeasureBlueLake and stops when (&& flag set GreenMeasured flag set BlueMeasured flag set RedMeasured)
 //Mission findBlueLakeWhileAvoidingRocks
 //	using Wander BumpR BumpL StayInFieldB StayInFieldL StayInFieldR and stops when Color is Blue
 //Mission findAllLakesAndMeasureThem
diff --git a/dsl/runtime/src/nl/ru/des/BasicBehaviour.java b/dsl/runtime/src/nl/ru/des/BasicBehaviour.java
index 2de5ab3..44fcbe2 100644
--- a/dsl/runtime/src/nl/ru/des/BasicBehaviour.java
+++ b/dsl/runtime/src/nl/ru/des/BasicBehaviour.java
@@ -61,8 +61,32 @@ public abstract class BasicBehaviour implements Behavior{
 			Thread.yield();
 		}
 		measMotor.stop(true);
-		reset();
-		rightTurn(45);
+		
+		turnRandom(30, 45);
+	}
+	
+	protected void measureRock(){
+		long time = System.currentTimeMillis();
+		rightMotor.forward();
+		leftMotor.forward();
+		while(System.currentTimeMillis()-time<1500){
+			Thread.yield();
+		}
+		
+		rightMotor.stop(true);
+		leftMotor.stop(true);
+		
+		measMotor.backward();
+		while(!suppressed && !measMotor.isStalled()){
+			Thread.yield();
+		}
+		measMotor.forward();
+		while(!suppressed && !measMotor.isStalled()){
+			Thread.yield();
+		}
+		measMotor.stop(true);
+		
+		turnRandom(30, 45);
 	}
 	
 	protected void turnRandom(int from, int to){
diff --git a/dsl/runtime/src/nl/ru/des/Marster.java b/dsl/runtime/src/nl/ru/des/Marster.java
index 0a48bc1..83bc184 100644
--- a/dsl/runtime/src/nl/ru/des/Marster.java
+++ b/dsl/runtime/src/nl/ru/des/Marster.java
@@ -58,18 +58,18 @@ public class Marster {
 			BTController.startSlave();
 			rs.start(BTController.buf);
 		} else {
-			try {
-				InputStream inp = Marster.class.getResourceAsStream("nl/ru/des/sound/rick.wav");
-				FileOutputStream out = new FileOutputStream("rick.wav");
-				byte buffer[] = new byte[2048];
-				while(inp.read(buffer)>0){
-					out.write(buffer);
-				}
-				inp.close();
-				out.close();
-			} catch (IOException e) {
-				e.printStackTrace();
-			}
+//			try {
+//				InputStream inp = Marster.class.getResourceAsStream("nl/ru/des/sound/rick.wav");
+//				FileOutputStream out = new FileOutputStream("rick.wav");
+//				byte buffer[] = new byte[2048];
+//				while(inp.read(buffer)>0){
+//					out.write(buffer);
+//				}
+//				inp.close();
+//				out.close();
+//			} catch (IOException e) {
+//				e.printStackTrace();
+//			}
 			String slave = brick.getName().equalsIgnoreCase("Rover5") ? "Rover6" : "Rover8";
 			LCDPrinter.print("Starting as as master...");
 			LCDPrinter.print("My name is " + brick.getName());
@@ -106,7 +106,7 @@ public class Marster {
 				arb.start();
 				LCDPrinter.print(m.name + " finished!!1one!");
 			}
-			Sound.playSample(new File("rick.wav"));
+//			Sound.playSample(new File("rick.wav"));
 		}
 	}
 }
\ No newline at end of file
-- 
2.20.1