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

   1 package nl.ru.des.sensors;
   2
   3 import java.util.Arrays;
   4 import java.util.HashSet;
   5 import java.util.Set;
   6
   7 import lejos.hardware.sensor.EV3GyroSensor;
   8 import lejos.robotics.SampleProvider;
   9
  10 public class SensorCollector implements MessageHandler{
  11         public static final int DELAY = 200;
  12
  13         private static final float DANGER_DISTANCE_FRONT = 0.15f;
  14         private static final float DANGER_DISTANCE_BACK = 0.035f;
  15         private static final float DANGER_LIGHT = 0.45f;
  16
  17         private Set<Integer> collectedColors;
  18
  19         //Local sensors
  20         private EV3GyroSensor gyroSensor;
  21         private SampleProvider ultra, leftLight, rightLight, gyro;
  22         private float[] ultraSamples, leftLightSamples, rightLightSamples, gyroSamples;
  23         private long ultraTime, leftLightTime, rightLightTime, gyroTime;
  24
  25         //Remote sensors
  26         private int color;
  27         private boolean leftTouch, rightTouch;
  28         private float frontUltra;
  29
  30         public SensorCollector(SampleProvider ultra,
  31                         SampleProvider leftLight,
  32                         SampleProvider rightLight,
  33                         EV3GyroSensor gyro){
  34                 this.ultra = ultra;
  35                 this.leftLight = leftLight;
  36                 this.rightLight = rightLight;
  37                 this.gyroSensor = gyro;
  38                 this.gyro = gyro.getAngleMode();
  39                 ultraSamples = new float[ultra.sampleSize()];
  40                 gyroSamples = new float[gyro.sampleSize()];
  41                 leftLightSamples = new float[leftLight.sampleSize()];
  42                 rightLightSamples = new float[rightLight.sampleSize()];
  43                 ultraTime = System.currentTimeMillis();
  44                 leftLightTime = System.currentTimeMillis();
  45                 rightLightTime = System.currentTimeMillis();
  46                 gyroTime = System.currentTimeMillis();
  47
  48                 collectedColors = new HashSet<Integer>();
  49         }
  50
  51         //Local sensors
  52         public boolean backDistance(){
  53                 if(System.currentTimeMillis()-ultraTime>DELAY){
  54                         ultra.fetchSample(ultraSamples, 0);
  55                         ultraTime = System.currentTimeMillis();
  56                 }
  57                 return ultraSamples[0]>DANGER_DISTANCE_BACK;
  58         }
  59
  60         public boolean leftLight(){
  61                 if(System.currentTimeMillis()-leftLightTime>DELAY){
  62                         leftLight.fetchSample(leftLightSamples, 0);
  63                         leftLightTime = System.currentTimeMillis();
  64                 }
  65                 return leftLightSamples[0]>DANGER_LIGHT;
  66         }
  67
  68         public boolean rightLight(){
  69                 if(System.currentTimeMillis()-rightLightTime>DELAY){
  70                         rightLight.fetchSample(rightLightSamples, 0);
  71                         rightLightTime = System.currentTimeMillis();
  72                 }
  73                 return rightLightSamples[0]>DANGER_LIGHT;
  74         }
  75
  76         public float gyro(){
  77                 if(System.currentTimeMillis()-gyroTime>DELAY){
  78                         gyro.fetchSample(gyroSamples, 0);
  79                         gyroTime = System.currentTimeMillis();
  80                 }
  81                 return gyroSamples[0];
  82         }
  83
  84         public void resetGyro(){
  85                 gyroSensor.reset();
  86         }
  87
  88         //Remote sensors
  89         public void resetColors(){
  90                 collectedColors.clear();
  91         }
  92
  93         public boolean collected(int[] colors){
  94                 return collectedColors.containsAll(Arrays.asList(colors));
  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                         collectedColors.add(color);
 120                         break;
 121                 case LEFT:
 122                         leftTouch = Integer.valueOf(s)==1;
 123                         break;
 124                 case RIGHT:
 125                         rightTouch = Integer.valueOf(s)==1;
 126                         break;
 127                 case ULTRA:
 128                         frontUltra = Float.valueOf(s);
 129                         break;
 130                 }
 131         }
 132 }