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