dsl/runtime/src/nl/ru/des/sensors/SensorCollector.java

   1 package nl.ru.des.sensors;
   2
   3 import lejos.hardware.lcd.LCD;
   4 import lejos.robotics.SampleProvider;
   5 import lejos.utility.Delay;
   6
   7 public class SensorCollector implements MessageHandler{
   8         public static final int DELAY = 100;
   9
  10         private static final float DANGER_DISTANCE_FRONT = 0.15f;
  11         private static final float DANGER_DISTANCE_BACK = 0.035f;
  12         private static final float DANGER_LIGHT = 0.45f;
  13
  14         //Local sensors
  15         private SampleProvider ultra, leftLight, rightLight, gyro;
  16         private float[] ultraSamples, leftLightSamples, rightLightSamples, gyroSamples;
  17         private long ultraTime, leftLightTime, rightLightTime, gyroTime;
  18
  19         //Remote sensors
  20         private int color;
  21         private boolean leftTouch, rightTouch;
  22         private float frontUltra;
  23
  24         public SensorCollector(SampleProvider ultra,
  25                         SampleProvider leftLight,
  26                         SampleProvider rightLight,
  27                         SampleProvider gyro){
  28                 this.ultra = ultra;
  29                 this.leftLight = leftLight;
  30                 this.rightLight = rightLight;
  31                 this.gyro = gyro;
  32                 ultraSamples = new float[ultra.sampleSize()];
  33                 gyroSamples = new float[gyro.sampleSize()];
  34                 leftLightSamples = new float[leftLight.sampleSize()];
  35                 rightLightSamples = new float[rightLight.sampleSize()];
  36                 ultraTime = System.currentTimeMillis();
  37                 leftLightTime = System.currentTimeMillis();
  38                 rightLightTime = System.currentTimeMillis();
  39                 gyroTime = System.currentTimeMillis();
  40         }
  41
  42         public void debug(){
  43                 new Thread(){
  44                         @Override public void run(){
  45                                 while(true){
  46                                         ultra.fetchSample(ultraSamples, 0);
  47                                         leftLight.fetchSample(leftLightSamples, 0);
  48                                         rightLight.fetchSample(rightLightSamples, 0);
  49                                         LCD.drawString("back: " + Float.toString(ultraSamples[0]), 0, 1);
  50                                         LCD.drawString("front: " + Float.toString(frontUltra), 0, 2);
  51                                         LCD.drawString("left: " + Float.toString(leftLightSamples[0]), 0, 3);
  52                                         LCD.drawString("right: " + Float.toString(rightLightSamples[0]), 0, 4);
  53                                         Delay.msDelay(250);
  54                                 }
  55                         }
  56                 }.run();
  57         }
  58
  59         //Local sensors
  60         public boolean backDistance(){
  61                 if(System.currentTimeMillis()-ultraTime>DELAY){
  62                         ultra.fetchSample(ultraSamples, 0);
  63                         ultraTime = System.currentTimeMillis();
  64                 }
  65                 return ultraSamples[0]>DANGER_DISTANCE_BACK;
  66         }
  67
  68         public boolean leftLight(){
  69                 if(System.currentTimeMillis()-leftLightTime>DELAY){
  70                         leftLight.fetchSample(leftLightSamples, 0);
  71                         leftLightTime = System.currentTimeMillis();
  72                 }
  73                 return leftLightSamples[0]>DANGER_LIGHT;
  74         }
  75
  76         public boolean rightLight(){
  77                 if(System.currentTimeMillis()-rightLightTime>DELAY){
  78                         rightLight.fetchSample(rightLightSamples, 0);
  79                         rightLightTime = System.currentTimeMillis();
  80                 }
  81                 return rightLightSamples[0]>DANGER_LIGHT;
  82         }
  83
  84         public float gyro(){
  85                 if(System.currentTimeMillis()-gyroTime>DELAY){
  86                         gyro.fetchSample(gyroSamples, 0);
  87                         gyroTime = System.currentTimeMillis();
  88                 }
  89                 return gyroSamples[0];
  90         }
  91
  92         //Remote sensors
  93         public boolean collected(int[] colors){
  94                 return false;
  95         }
  96
  97         public int color(){
  98                 return color;
  99         }
 100
 101         public boolean leftTouch(){
 102                 return leftTouch;
 103         }
 104
 105         public boolean rightTouch(){
 106                 return rightTouch;
 107         }
 108
 109         public boolean frontDistance(){
 110                 return frontUltra<DANGER_DISTANCE_FRONT;
 111         }
 112
 113         @Override
 114         public void handleMessage(String m) {
 115                 String s = m.substring(1);
 116                 switch(RemoteSensors.RemoteSensorEnum.values()[Integer.valueOf(Character.toString(m.charAt(0)))]){
 117                 case COLOR:
 118                         color = Integer.valueOf(s);
 119                         break;
 120                 case LEFT:
 121                         leftTouch = Integer.valueOf(s)==1;
 122                         break;
 123                 case RIGHT:
 124                         rightTouch = Integer.valueOf(s)==1;
 125                         break;
 126                 case ULTRA:
 127                         frontUltra = Float.valueOf(s);
 128                         break;
 129                 }
 130         }
 131 }