started with new rover, bt still not worknig:(

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

diff --git a/dsl/runtime/src/nl/ru/des/SensorCollector.java b/dsl/runtime/src/nl/ru/des/SensorCollector.java
index 7695ee0..9a58ae4 100644 (file)
--- a/dsl/runtime/src/nl/ru/des/SensorCollector.java
+++ b/dsl/runtime/src/nl/ru/des/SensorCollector.java
@@ -2,60 +2,104 @@ package nl.ru.des;
 
 import lejos.robotics.SampleProvider;
 
-public class SensorCollector{
-       public static final int DELAY = 50;
+public class SensorCollector implements MessageHandler{
+       public static final int DELAY = 300;
        
-       private SampleProvider ultrasone, color, leftTouch, rightTouch;
-       private float[] ultrasoneSamples, colorSamples, leftTouchSamples, rightTouchSamples;
-       private long ultrasoneTime, colorTime, leftTouchTime, rightTouchTime;
-
-       public SensorCollector(SampleProvider ultrasone,
-                       SampleProvider color,
-                       SampleProvider leftTouch,
-                       SampleProvider rightTouch){
-               this.ultrasone = ultrasone;
-               this.color = color;
-               this.leftTouch = leftTouch;
-               this.rightTouch = rightTouch;
-               ultrasoneSamples = new float[ultrasone.sampleSize()];
-               colorSamples = new float[color.sampleSize()];
-               leftTouchSamples = new float[leftTouch.sampleSize()];
-               rightTouchSamples = new float[rightTouch.sampleSize()];
-               ultrasoneTime = System.currentTimeMillis();
-               colorTime = System.currentTimeMillis();
-               leftTouchTime = System.currentTimeMillis();
-               rightTouchTime = System.currentTimeMillis();
-       }
-       
-       public float distance(){
-               if(System.currentTimeMillis()-ultrasoneTime>DELAY){
-                       ultrasone.fetchSample(ultrasoneSamples, 0);
-                       ultrasoneTime = System.currentTimeMillis();
+       //Local sensors
+       private SampleProvider ultra, leftLight, rightLight, gyro;
+       private float[] ultraSamples, leftLightSamples, rightLightSamples, gyroSamples;
+       private long ultraTime, leftLightTime, rightLightTime, gyroTime;
+       
+       //Remote sensors
+       private int color;
+       private boolean leftTouch, rightTouch;
+       private float frontUltra;
+       
+       public SensorCollector(SampleProvider ultra,
+                       SampleProvider leftLight,
+                       SampleProvider rightLight,
+                       SampleProvider gyro){
+               this.ultra = ultra;
+               this.leftLight = leftLight;
+               this.rightLight = rightLight;
+               this.gyro = gyro;
+               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();
+       }
+       
+       //Local sensors
+       public float backDistance(){
+               if(System.currentTimeMillis()-ultraTime>DELAY){
+                       ultra.fetchSample(ultraSamples, 0);
+                       ultraTime = System.currentTimeMillis();
                }
-               return ultrasoneSamples[0];
+               return ultraSamples[0];
        }
        
-       public int color(){
-               if(System.currentTimeMillis()-colorTime>DELAY){
-                       color.fetchSample(colorSamples, 0);
-                       colorTime = System.currentTimeMillis();
+       public boolean leftLight(){
+               if(System.currentTimeMillis()-leftLightTime>DELAY){
+                       leftLight.fetchSample(leftLightSamples, 0);
+                       leftLightTime = System.currentTimeMillis();
                }
-               return (int)colorSamples[0];
+               return leftLightSamples[0]>0.5;
        }
        
-       public boolean leftTouch(){
-               if(System.currentTimeMillis()-leftTouchTime>DELAY){
-                       leftTouch.fetchSample(leftTouchSamples, 0);
-                       leftTouchTime = System.currentTimeMillis();
+       public boolean rightLight(){
+               if(System.currentTimeMillis()-rightLightTime>DELAY){
+                       rightLight.fetchSample(rightLightSamples, 0);
+                       rightLightTime = System.currentTimeMillis();
+               }
+               return rightLightSamples[0]>0.5;
+       }
+       
+       public float gyro(){
+               if(System.currentTimeMillis()-gyroTime>DELAY){
+                       gyro.fetchSample(gyroSamples, 0);
+                       gyroTime = System.currentTimeMillis();
                }
-               return leftTouchSamples[0]==1;
+               return gyroSamples[0];
+       }
+       
+       //Remote sensors
+       public int color(){
+               return color;
+       }
+       
+       public boolean leftTouch(){
+               return leftTouch;
        }
        
        public boolean rightTouch(){
-               if(System.currentTimeMillis()-rightTouchTime>DELAY){
-                       rightTouch.fetchSample(rightTouchSamples, 0);
-                       rightTouchTime = System.currentTimeMillis();
+               return rightTouch;
+       }
+       
+       public float frontDistance(){
+               return frontUltra;
+       }
+
+       @Override
+       public void handleMessage(String m) {
+               LCDPrinter.print(m);
+               String s = m.substring(1);
+               switch(RemoteSensors.RemoteSensorEnum.values()[Integer.valueOf(m.charAt(0))]){
+               case COLOR:
+                       color = Integer.valueOf(s);
+                       break;
+               case LEFT:
+                       leftTouch = Integer.valueOf(s)==1;
+                       break;
+               case RIGHT:
+                       rightTouch = Integer.valueOf(s)==1;
+                       break;
+               case ULTRA:
+                       frontUltra = Float.valueOf(s);
+                       break;
                }
-               return rightTouchSamples[0]==1;
        }
 }
\ No newline at end of file