-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;
- }
-}