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

   1 package nl.ru.des.sensors;
   2
   3 import java.util.HashSet;
   4 import java.util.Set;
   5
   6 import lejos.hardware.Button;
   7 import lejos.hardware.sensor.EV3GyroSensor;
   8 import lejos.robotics.SampleProvider;
   9 import lejos.utility.Delay;
  10
  11 public class SensorCollector implements MessageHandler{
  12         public static final int DELAY = 200;
  13
  14         private float DANGER_DISTANCE_FRONT = 0.175f;
  15         private float DANGER_DISTANCE_BACK = 0.035f;
  16         private float DANGER_LIGHT = 0.40f;
  17
  18         private Set<String> variables;
  19
  20         //Local sensors
  21         private EV3GyroSensor gyroSensor;
  22         private SampleProvider ultra, leftLight, rightLight, gyro;
  23         private float[] ultraSamples, leftLightSamples, rightLightSamples, gyroSamples;
  24         private long ultraTime, leftLightTime, rightLightTime, gyroTime;
  25
  26         //Remote sensors
  27         private int color;
  28         private boolean leftTouch, rightTouch;
  29         private float frontUltra;
  30
  31         public SensorCollector(SampleProvider ultra,
  32                         SampleProvider leftLight,
  33                         SampleProvider rightLight,
  34                         EV3GyroSensor gyro){
  35                 this.ultra = ultra;
  36                 this.leftLight = leftLight;
  37                 this.rightLight = rightLight;
  38                 this.gyroSensor = gyro;
  39                 this.gyro = gyro.getAngleMode();
  40                 ultraSamples = new float[ultra.sampleSize()];
  41                 gyroSamples = new float[gyro.sampleSize()];
  42                 leftLightSamples = new float[leftLight.sampleSize()];
  43                 rightLightSamples = new float[rightLight.sampleSize()];
  44
  45                 ultraTime = System.currentTimeMillis();
  46                 leftLightTime = System.currentTimeMillis();
  47                 rightLightTime = System.currentTimeMillis();
  48                 gyroTime = System.currentTimeMillis();
  49
  50                 color = -1;
  51                 leftTouch = false;
  52                 rightTouch = false;
  53                 frontUltra = Float.MAX_VALUE;
  54                 variables = new HashSet<String>();
  55         }
  56
  57
  58         public void calibrate() {
  59                 System.out.println("Put left light on Blue");
  60                 Button.waitForAnyEvent();
  61                 leftLight();
  62                 DANGER_LIGHT = leftLightSamples[0];
  63                 System.out.println("Light limit: " + Float.toString(DANGER_LIGHT));
  64
  65                 Delay.msDelay(350);
  66                 System.out.println("Put left light on Black");
  67                 Button.waitForAnyEvent();
  68                 leftLight();
  69                 DANGER_LIGHT = (leftLightSamples[0]+DANGER_LIGHT)/2.0f;
  70                 System.out.println("Light limit: " + Float.toString(DANGER_LIGHT));
  71
  72                 Delay.msDelay(350);
  73                 System.out.println("Place back ultra safe");
  74                 Button.waitForAnyEvent();
  75                 backDistance();
  76                 DANGER_DISTANCE_BACK = ultraSamples[0] + 0.05f;
  77                 System.out.println("Back ultra limit: " + Float.toString(DANGER_DISTANCE_BACK));
  78
  79                 Delay.msDelay(350);
  80                 System.out.println("Place front ultra in danger");
  81                 Button.waitForAnyEvent();
  82                 DANGER_DISTANCE_FRONT = frontUltra;
  83                 System.out.println("Calibration done");
  84                 System.out.println("Front ultra limit: " + Float.toString(DANGER_DISTANCE_FRONT));
  85         }
  86
  87         //Local sensors
  88         public boolean backDistance(){
  89                 if(System.currentTimeMillis()-ultraTime>DELAY){
  90                         ultra.fetchSample(ultraSamples, 0);
  91                         ultraTime = System.currentTimeMillis();
  92                 }
  93                 return ultraSamples[0]>DANGER_DISTANCE_BACK;
  94         }
  95
  96         public boolean leftLight(){
  97                 if(System.currentTimeMillis()-leftLightTime>DELAY){
  98                         leftLight.fetchSample(leftLightSamples, 0);
  99                         leftLightTime = System.currentTimeMillis();
 100                 }
 101                 return leftLightSamples[0]>DANGER_LIGHT;
 102         }
 103
 104         public boolean rightLight(){
 105                 if(System.currentTimeMillis()-rightLightTime>DELAY){
 106                         rightLight.fetchSample(rightLightSamples, 0);
 107                         rightLightTime = System.currentTimeMillis();
 108                 }
 109                 return rightLightSamples[0]>DANGER_LIGHT;
 110         }
 111
 112         public float gyro(){
 113                 if(System.currentTimeMillis()-gyroTime>DELAY){
 114                         gyro.fetchSample(gyroSamples, 0);
 115                         gyroTime = System.currentTimeMillis();
 116                 }
 117                 return gyroSamples[0];
 118         }
 119
 120         public void resetGyro(){
 121                 gyroSensor.reset();
 122         }
 123
 124         //Remote sensors
 125         public void reset(){
 126                 variables.clear();
 127         }
 128
 129         public boolean collected(String var){
 130                 return variables.contains(var);
 131         }
 132
 133         public void saveVar(String var) {
 134                 variables.add(var);
 135         }
 136
 137         public int color(){
 138                 return color;
 139         }
 140
 141         public boolean leftTouch(){
 142                 return leftTouch;
 143         }
 144
 145         public boolean rightTouch(){
 146                 return rightTouch;
 147         }
 148
 149         public boolean frontDistance(){
 150                 return frontUltra<DANGER_DISTANCE_FRONT;
 151         }
 152
 153         @Override
 154         public void handleMessage(String m) {
 155                 String s = m.substring(1);
 156                 switch(RemoteSensors.RemoteSensorEnum.values()[Integer.valueOf(Character.toString(m.charAt(0)))]){
 157                 case COLOR:
 158                         color = Integer.valueOf(s);
 159                         break;
 160                 case LEFT:
 161                         leftTouch = Integer.valueOf(s)==1;
 162                         break;
 163                 case RIGHT:
 164                         rightTouch = Integer.valueOf(s)==1;
 165                         break;
 166                 case ULTRA:
 167                         frontUltra = Float.valueOf(s);
 168                         break;
 169                 }
 170         }
 171 }