Implemented finding park space

[des2015.git] / dsl / runtime / src / nl / ru / des / sensors / SensorCollector.java

diff --git a/dsl/runtime/src/nl/ru/des/sensors/SensorCollector.java b/dsl/runtime/src/nl/ru/des/sensors/SensorCollector.java
index 60652d8..db10870 100644 (file)
--- a/dsl/runtime/src/nl/ru/des/sensors/SensorCollector.java
+++ b/dsl/runtime/src/nl/ru/des/sensors/SensorCollector.java
@@ -1,12 +1,22 @@
 package nl.ru.des.sensors;
 
+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<String> variables;
        
        //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 +29,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 +43,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;
+               variables = new HashSet<String>();
        }
        
        //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 +65,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 +73,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 +84,23 @@ public class SensorCollector implements MessageHandler{
                return gyroSamples[0];
        }
        
+       public void resetGyro(){
+               gyroSensor.reset();
+       }
+       
        //Remote sensors
+       public void reset(){
+               variables.clear();
+       }
+       
+       public boolean collected(String var){
+               return variables.contains(var);
+       }
+
+       public void saveVar(String var) {
+               variables.add(var);
+       }
+       
        public int color(){
                return color;
        }
@@ -80,13 +113,12 @@ 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: