mart/ev3/ex2/nl/ru/des/ColorSensor.java

   1 package nl.ru.des;
   2
   3 import java.util.Arrays;
   4 import java.util.HashMap;
   5 import java.util.Map;
   6
   7 import lejos.hardware.Button;
   8 import lejos.hardware.sensor.EV3ColorSensor;
   9 import lejos.robotics.SampleProvider;
  10
  11 public class ColorSensor {
  12         public static final String[] COLORS = new String[]{
  13                         "Black",
  14                         "White",
  15                         "Yellow",
  16                         "Red",
  17                         "Blue"
  18         };
  19         public static final long SAMPLETIME = 50;
  20
  21         private float[] samples;
  22         private SampleProvider sampleProvider;
  23         private Map<String,Color> colors;
  24         private long lastSampleTaken;
  25
  26         private class Color{
  27                 private float[] rgb;
  28
  29                 public Color(float r, float g, float b){
  30                         this.rgb = new float[]{r, g, b};
  31                 }
  32
  33                 public double distanceTo(Color c){
  34                         return Math.sqrt(
  35                                 Math.pow(this.rgb[0]-c.rgb[0], 2)+
  36                                 Math.pow(this.rgb[1]-c.rgb[1], 2)+
  37                                 Math.pow(this.rgb[2]-c.rgb[2], 2)
  38                                 );
  39                 }
  40         }
  41
  42         public ColorSensor(EV3ColorSensor colorSensor){
  43                 this.sampleProvider = colorSensor.getRGBMode();
  44                 this.colors = new HashMap<String, ColorSensor.Color>();
  45                 this.samples = new float[sampleProvider.sampleSize()];
  46                 calibrate();
  47                 this.lastSampleTaken = System.currentTimeMillis();
  48         }
  49
  50         public void calibrate(){
  51                 LCDPrinter.print("Color calibration initialized...");
  52                 for(String c : COLORS){
  53                         LCDPrinter.print("Put the sensor on " + c);
  54                         Button.waitForAnyPress();
  55                         sampleProvider.fetchSample(samples, 0);
  56                         colors.put(c, new Color(samples[0], samples[1], samples[2]));
  57                         LCDPrinter.print("Value:" + Arrays.toString(samples));
  58                 }
  59         }
  60
  61         public String getCurrentColor(){
  62                 if(System.currentTimeMillis()-lastSampleTaken > SAMPLETIME){
  63                         lastSampleTaken = System.currentTimeMillis();
  64                         sampleProvider.fetchSample(samples, 0);
  65                 }
  66                 Color current = new Color(samples[0], samples[1], samples[2]);
  67                 String min = null;
  68                 double dist = Double.MAX_VALUE;
  69                 for(String c : colors.keySet()){
  70                         double newdist = current.distanceTo(colors.get(c));
  71                         if(newdist < dist){
  72                                 min = c;
  73                                 dist = newdist;
  74                         }
  75                 }
  76                 return min;
  77         }
  78 }