+package nl.ru.des;
+
+import java.util.Arrays;
+import java.util.HashMap;
+import java.util.Map;
+
+import lejos.hardware.Button;
+import lejos.hardware.sensor.EV3ColorSensor;
+import lejos.robotics.SampleProvider;
+
+public class ColorSensor {
+ public static final String[] COLORS = new String[]{
+ "Black",
+ "White",
+ "Yellow",
+ "Red",
+ "Blue"
+ };
+ public static final long SAMPLETIME = 50;
+
+ private float[] samples;
+ private SampleProvider sampleProvider;
+ private Map<String,Color> colors;
+ private long lastSampleTaken;
+
+ private class Color{
+ private float[] rgb;
+
+ public Color(float r, float g, float b){
+ this.rgb = new float[]{r, g, b};
+ }
+
+ public double distanceTo(Color c){
+ return Math.sqrt(
+ Math.pow(this.rgb[0]-c.rgb[0], 2)+
+ Math.pow(this.rgb[1]-c.rgb[1], 2)+
+ Math.pow(this.rgb[2]-c.rgb[2], 2)
+ );
+ }
+ }
+
+ public ColorSensor(EV3ColorSensor colorSensor){
+ this.sampleProvider = colorSensor.getRGBMode();
+ this.colors = new HashMap<String, ColorSensor.Color>();
+ this.samples = new float[sampleProvider.sampleSize()];
+ calibrate();
+ this.lastSampleTaken = System.currentTimeMillis();
+ }
+
+ public void calibrate(){
+ LCDPrinter.print("Color calibration initialized...");
+ for(String c : COLORS){
+ LCDPrinter.print("Put the sensor on " + c);
+ Button.waitForAnyPress();
+ sampleProvider.fetchSample(samples, 0);
+ colors.put(c, new Color(samples[0], samples[1], samples[2]));
+ LCDPrinter.print("Value:" + Arrays.toString(samples));
+ }
+ }
+
+ public String getCurrentColor(){
+ if(System.currentTimeMillis()-lastSampleTaken > SAMPLETIME){
+ lastSampleTaken = System.currentTimeMillis();
+ sampleProvider.fetchSample(samples, 0);
+ }
+ Color current = new Color(samples[0], samples[1], samples[2]);
+ String min = null;
+ double dist = Double.MAX_VALUE;
+ for(String c : colors.keySet()){
+ double newdist = current.distanceTo(colors.get(c));
+ if(newdist < dist){
+ min = c;
+ dist = newdist;
+ }
+ }
+ return min;
+ }
+}