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.175f;
  14         private static final float DANGER_DISTANCE_BACK = 0.035f;
  15         private static final float DANGER_LIGHT = 0.40f;
  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                 color = -1;
  49                 leftTouch = false;
  50                 rightTouch = false;
  51                 frontUltra = Float.MAX_VALUE;
  52                 collectedColors = new HashSet<Integer>();
  53         }
  54
  55         //Local sensors
  56         public boolean backDistance(){
  57                 if(System.currentTimeMillis()-ultraTime>DELAY){
  58                         ultra.fetchSample(ultraSamples, 0);
  59                         ultraTime = System.currentTimeMillis();
  60                 }
  61                 return ultraSamples[0]>DANGER_DISTANCE_BACK;
  62         }
  63
  64         public boolean leftLight(){
  65                 if(System.currentTimeMillis()-leftLightTime>DELAY){
  66                         leftLight.fetchSample(leftLightSamples, 0);
  67                         leftLightTime = System.currentTimeMillis();
  68                 }
  69                 return leftLightSamples[0]>DANGER_LIGHT;
  70         }
  71
  72         public boolean rightLight(){
  73                 if(System.currentTimeMillis()-rightLightTime>DELAY){
  74                         rightLight.fetchSample(rightLightSamples, 0);
  75                         rightLightTime = System.currentTimeMillis();
  76                 }
  77                 return rightLightSamples[0]>DANGER_LIGHT;
  78         }
  79
  80         public float gyro(){
  81                 if(System.currentTimeMillis()-gyroTime>DELAY){
  82                         gyro.fetchSample(gyroSamples, 0);
  83                         gyroTime = System.currentTimeMillis();
  84                 }
  85                 return gyroSamples[0];
  86         }
  87
  88         public void resetGyro(){
  89                 gyroSensor.reset();
  90         }
  91
  92         //Remote sensors
  93         public void resetColors(){
  94                 collectedColors.clear();
  95         }
  96
  97         public boolean collected(int[] colors){
  98                 return collectedColors.containsAll(Arrays.asList(colors));
  99         }
 100
 101         public int color(){
 102                 return color;
 103         }
 104
 105         public boolean leftTouch(){
 106                 return leftTouch;
 107         }
 108
 109         public boolean rightTouch(){
 110                 return rightTouch;
 111         }
 112
 113         public boolean frontDistance(){
 114                 return frontUltra<DANGER_DISTANCE_FRONT;
 115         }
 116
 117         @Override
 118         public void handleMessage(String m) {
 119                 String s = m.substring(1);
 120                 switch(RemoteSensors.RemoteSensorEnum.values()[Integer.valueOf(Character.toString(m.charAt(0)))]){
 121                 case COLOR:
 122                         color = Integer.valueOf(s);
 123                         collectedColors.add(color);
 124                         break;
 125                 case LEFT:
 126                         leftTouch = Integer.valueOf(s)==1;
 127                         break;
 128                 case RIGHT:
 129                         rightTouch = Integer.valueOf(s)==1;
 130                         break;
 131                 case ULTRA:
 132                         frontUltra = Float.valueOf(s);
 133                         break;
 134                 }
 135         }
 136 }