X-Git-Url: https://git.martlubbers.net/?a=blobdiff_plain;f=dsl%2Fruntime%2Fsrc%2Fnl%2Fru%2Fdes%2Fsensors%2FSensorCollector.java;h=4597d92805c2220b9969860df654b7225f060d40;hb=095cdf6fc0cbad75c6057a401cbab23c18931a3d;hp=60652d8224daabb24aff3fb8084ffd82a618f399;hpb=476c651adb42cd9be8758e4f6ef2fe9ee2519fd5;p=des2015.git

diff --git a/dsl/runtime/src/nl/ru/des/sensors/SensorCollector.java b/dsl/runtime/src/nl/ru/des/sensors/SensorCollector.java
index 60652d8..4597d92 100644
--- a/dsl/runtime/src/nl/ru/des/sensors/SensorCollector.java
+++ b/dsl/runtime/src/nl/ru/des/sensors/SensorCollector.java
@@ -1,12 +1,23 @@
 package nl.ru.des.sensors;
 
+import java.util.Arrays;
+import java.util.HashSet;
+import java.util.Set;
+
+import lejos.hardware.sensor.EV3GyroSensor;
 import lejos.robotics.SampleProvider;
-import nl.ru.des.LCDPrinter;
 
 public class SensorCollector implements MessageHandler{
-	public static final int DELAY = 300;
+	public static final int DELAY = 200;
+
+	private static final float DANGER_DISTANCE_FRONT = 0.175f;
+	private static final float DANGER_DISTANCE_BACK = 0.035f;
+	private static final float DANGER_LIGHT = 0.40f;
+	
+	private Set<Integer> collectedColors;
 	
 	//Local sensors
+	private EV3GyroSensor gyroSensor;
 	private SampleProvider ultra, leftLight, rightLight, gyro;
 	private float[] ultraSamples, leftLightSamples, rightLightSamples, gyroSamples;
 	private long ultraTime, leftLightTime, rightLightTime, gyroTime;
@@ -19,11 +30,12 @@ public class SensorCollector implements MessageHandler{
 	public SensorCollector(SampleProvider ultra,
 			SampleProvider leftLight,
 			SampleProvider rightLight,
-			SampleProvider gyro){
+			EV3GyroSensor gyro){
 		this.ultra = ultra;
 		this.leftLight = leftLight;
 		this.rightLight = rightLight;
-		this.gyro = gyro;
+		this.gyroSensor = gyro;
+		this.gyro = gyro.getAngleMode();
 		ultraSamples = new float[ultra.sampleSize()];
 		gyroSamples = new float[gyro.sampleSize()];
 		leftLightSamples = new float[leftLight.sampleSize()];
@@ -32,15 +44,21 @@ public class SensorCollector implements MessageHandler{
 		leftLightTime = System.currentTimeMillis();
 		rightLightTime = System.currentTimeMillis();
 		gyroTime = System.currentTimeMillis();
+		
+		color = -1;
+		leftTouch = false;
+		rightTouch = false;
+		frontUltra = Float.MAX_VALUE;
+		collectedColors = new HashSet<Integer>();
 	}
 	
 	//Local sensors
-	public float backDistance(){
+	public boolean backDistance(){
 		if(System.currentTimeMillis()-ultraTime>DELAY){
 			ultra.fetchSample(ultraSamples, 0);
 			ultraTime = System.currentTimeMillis();
 		}
-		return ultraSamples[0];
+		return ultraSamples[0]>DANGER_DISTANCE_BACK;
 	}
 	
 	public boolean leftLight(){
@@ -48,7 +66,7 @@ public class SensorCollector implements MessageHandler{
 			leftLight.fetchSample(leftLightSamples, 0);
 			leftLightTime = System.currentTimeMillis();
 		}
-		return leftLightSamples[0]>0.5;
+		return leftLightSamples[0]>DANGER_LIGHT;
 	}
 	
 	public boolean rightLight(){
@@ -56,7 +74,7 @@ public class SensorCollector implements MessageHandler{
 			rightLight.fetchSample(rightLightSamples, 0);
 			rightLightTime = System.currentTimeMillis();
 		}
-		return rightLightSamples[0]>0.5;
+		return rightLightSamples[0]>DANGER_LIGHT;
 	}
 	
 	public float gyro(){
@@ -67,7 +85,19 @@ public class SensorCollector implements MessageHandler{
 		return gyroSamples[0];
 	}
 	
+	public void resetGyro(){
+		gyroSensor.reset();
+	}
+	
 	//Remote sensors
+	public void resetColors(){
+		collectedColors.clear();
+	}
+	
+	public boolean collected(int[] colors){
+		return collectedColors.containsAll(Arrays.asList(colors));
+	}
+	
 	public int color(){
 		return color;
 	}
@@ -80,17 +110,17 @@ public class SensorCollector implements MessageHandler{
 		return rightTouch;
 	}
 	
-	public float frontDistance(){
-		return frontUltra;
+	public boolean frontDistance(){
+		return frontUltra<DANGER_DISTANCE_FRONT;
 	}
 
 	@Override
 	public void handleMessage(String m) {
-		LCDPrinter.print("Sensor: " + Character.toString(m.charAt(0)) + " with value: " + m.substring(1));
 		String s = m.substring(1);
 		switch(RemoteSensors.RemoteSensorEnum.values()[Integer.valueOf(Character.toString(m.charAt(0)))]){
 		case COLOR:
 			color = Integer.valueOf(s);
+			collectedColors.add(color);
 			break;
 		case LEFT:
 			leftTouch = Integer.valueOf(s)==1;