final commit

[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 29433c1..64a26b4 100644 (file)
--- a/dsl/runtime/src/nl/ru/des/sensors/SensorCollector.java
+++ b/dsl/runtime/src/nl/ru/des/sensors/SensorCollector.java
@@ -1,17 +1,24 @@
 package nl.ru.des.sensors;
 
-import lejos.hardware.lcd.LCD;
+import java.util.HashSet;
+import java.util.Set;
+
+import lejos.hardware.Button;
+import lejos.hardware.sensor.EV3GyroSensor;
 import lejos.robotics.SampleProvider;
 import lejos.utility.Delay;
 
 public class SensorCollector implements MessageHandler{
-       public static final int DELAY = 100;
+       public static final int DELAY = 200;
 
-       private static final float DANGER_DISTANCE_FRONT = 0.15f;
-       private static final float DANGER_DISTANCE_BACK = 0.035f;
-       private static final float DANGER_LIGHT = 0.45f;
+       private float DANGER_DISTANCE_FRONT = 0.175f;
+       private float DANGER_DISTANCE_BACK = 0.035f;
+       private 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;
@@ -24,36 +31,57 @@ 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()];
                rightLightSamples = new float[rightLight.sampleSize()];
+
                ultraTime = System.currentTimeMillis();
                leftLightTime = System.currentTimeMillis();
                rightLightTime = System.currentTimeMillis();
                gyroTime = System.currentTimeMillis();
+               
+               color = -1;
+               leftTouch = false;
+               rightTouch = false;
+               frontUltra = Float.MAX_VALUE;
+               variables = new HashSet<String>();
        }
        
-       public void debug(){
-               new Thread(){
-                       @Override public void run(){
-                               while(true){
-                                       ultra.fetchSample(ultraSamples, 0);
-                                       leftLight.fetchSample(leftLightSamples, 0);
-                                       rightLight.fetchSample(rightLightSamples, 0);
-                                       LCD.drawString("back: " + Float.toString(ultraSamples[0]), 0, 1);
-                                       LCD.drawString("front: " + Float.toString(frontUltra), 0, 2);
-                                       LCD.drawString("left: " + Float.toString(leftLightSamples[0]), 0, 3);
-                                       LCD.drawString("right: " + Float.toString(rightLightSamples[0]), 0, 4);
-                                       Delay.msDelay(250);
-                               }
-                       }
-               }.run();
+
+       public void calibrate() {
+               System.out.println("Put left light on Blue");
+               Button.waitForAnyEvent();
+               leftLight();
+               DANGER_LIGHT = leftLightSamples[0];
+               System.out.println("Light limit: " + Float.toString(DANGER_LIGHT));
+
+               Delay.msDelay(350);
+               System.out.println("Put left light on Black");
+               Button.waitForAnyEvent();
+               leftLight();
+               DANGER_LIGHT = (leftLightSamples[0]+DANGER_LIGHT)/2.0f;
+               System.out.println("Light limit: " + Float.toString(DANGER_LIGHT));
+               
+               Delay.msDelay(350);             
+               System.out.println("Place back ultra safe");
+               Button.waitForAnyEvent();
+               backDistance();
+               DANGER_DISTANCE_BACK = ultraSamples[0] + 0.05f;
+               System.out.println("Back ultra limit: " + Float.toString(DANGER_DISTANCE_BACK));
+
+               Delay.msDelay(350);
+               System.out.println("Place front ultra in danger");
+               Button.waitForAnyEvent();
+               DANGER_DISTANCE_FRONT = frontUltra;
+               System.out.println("Calibration done");
+               System.out.println("Front ultra limit: " + Float.toString(DANGER_DISTANCE_FRONT));
        }
        
        //Local sensors
@@ -89,9 +117,21 @@ public class SensorCollector implements MessageHandler{
                return gyroSamples[0];
        }
        
+       public void resetGyro(){
+               gyroSensor.reset();
+       }
+       
        //Remote sensors
-       public boolean collected(int[] colors){
-               return false;
+       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(){