push

[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..29433c1 100644 (file)
--- a/dsl/runtime/src/nl/ru/des/sensors/SensorCollector.java
+++ b/dsl/runtime/src/nl/ru/des/sensors/SensorCollector.java
@@ -1,10 +1,15 @@
 package nl.ru.des.sensors;
 
+import lejos.hardware.lcd.LCD;
 import lejos.robotics.SampleProvider;
-import nl.ru.des.LCDPrinter;
+import lejos.utility.Delay;
 
 public class SensorCollector implements MessageHandler{
-       public static final int DELAY = 300;
+       public static final int DELAY = 100;
+
+       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;
        
        //Local sensors
        private SampleProvider ultra, leftLight, rightLight, gyro;
@@ -34,13 +39,30 @@ public class SensorCollector implements MessageHandler{
                gyroTime = System.currentTimeMillis();
        }
        
+       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();
+       }
+       
        //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 +70,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 +78,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(){
@@ -68,6 +90,10 @@ public class SensorCollector implements MessageHandler{
        }
        
        //Remote sensors
+       public boolean collected(int[] colors){
+               return false;
+       }
+       
        public int color(){
                return color;
        }
@@ -80,13 +106,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: