--- /dev/null
+package nl.ru.des;
+
+import java.io.DataInputStream;
+import java.io.DataOutputStream;
+import java.io.IOException;
+import java.util.Arrays;
+import java.util.HashSet;
+import java.util.List;
+import java.util.Set;
+
+import lejos.remote.nxt.BTConnector;
+import lejos.remote.nxt.NXTConnection;
+
+public abstract class BTMemory extends Thread{
+ private List<String> COLORSTOFIND = Arrays.asList(new String[]{"Blue", "Red", "Yellow"});
+ private Set<String> colors;
+ protected NXTConnection connection;
+ protected DataOutputStream dataOutput;
+ protected DataInputStream dataInput;
+
+ public BTMemory(){
+ colors = new HashSet<String>();
+ setup();
+ }
+
+ public abstract void setup();
+
+ public void addColor(String c){
+ colors.add(c);
+ }
+
+ public boolean finished(){
+ return colors.equals(new HashSet<String>(COLORSTOFIND));
+ }
+
+ @Override
+ public void run(){
+ try {
+ while(true){
+ if(dataInput.available()>0){
+ LCDPrinter.print(Character.toChars(dataInput.read()).toString());
+ }
+ }
+ } catch (IOException e) {
+ // TODO Auto-generated catch block
+ e.printStackTrace();
+ }
+ }
+
+ public static BTMemory getBTMemory(boolean master, final String rovername){
+ if(master){
+ return new BTMemory(){
+ @Override
+ public void setup() {
+ BTConnector btconnector = new BTConnector();
+ connection = btconnector.connect(rovername, NXTConnection.RAW);
+ dataInput = connection.openDataInputStream();
+ dataOutput = connection.openDataOutputStream();
+ try {
+ dataOutput.write(77);
+ } catch (IOException e) {
+ e.printStackTrace();
+ }
+ }
+ };
+ } else {
+ return new BTMemory(){
+ @Override
+ public void setup() {
+ BTConnector btconnector = new BTConnector();
+ btconnector.waitForConnection(60, NXTConnection.RAW);
+ dataInput = connection.openDataInputStream();
+ dataOutput = connection.openDataOutputStream();
+ }
+ };
+ }
+ }
+}
--- /dev/null
+package nl.ru.des;
+
+import lejos.hardware.Key;
+import lejos.hardware.KeyListener;
+
+class ButtonListener implements KeyListener {
+
+ @Override
+ public void keyPressed(Key k) {
+ LCDPrinter.print("Bye");
+ System.exit(0);
+ }
+
+ @Override
+ public void keyReleased(Key k) {}
+}
\ No newline at end of file
--- /dev/null
+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;
+ }
+}
--- /dev/null
+package nl.ru.des;
+
+import java.util.Deque;
+import java.util.Iterator;
+import java.util.LinkedList;
+
+import lejos.hardware.lcd.Font;
+import lejos.hardware.lcd.GraphicsLCD;
+import lejos.utility.Delay;
+
+public class LCDPrinter extends Thread {
+ public static final int PRINTDELAY = 50;
+
+ private static Deque<String> buffer = new LinkedList<String>();
+
+ private GraphicsLCD glcd;
+ private Font font;
+ private Deque<String> lcdbuffer;
+ private int charwidth;
+
+ public LCDPrinter(GraphicsLCD glcd, Font font) {
+ this.glcd = glcd;
+ this.font = font;
+ glcd.setFont(font);
+ lcdbuffer = new LinkedList<String>();
+ charwidth = glcd.getWidth()/font.width;
+ for(int i = 0; i<glcd.getHeight()/font.height; i++){
+ lcdbuffer.add("");
+ }
+ }
+
+ public static void print(String s){
+ buffer.addLast(s);
+ }
+
+ public void run() {
+ Iterator<String> it;
+ while (true) {
+ if (!buffer.isEmpty()) {
+ String c = buffer.remove();
+ lcdbuffer.removeLast();
+ if(c.length() > charwidth){
+ buffer.addFirst(c.substring(charwidth, c.length()));
+ c = c.substring(0, charwidth);
+ }
+ lcdbuffer.addFirst(c);
+ glcd.clear();
+ it = lcdbuffer.descendingIterator();
+ for(int i = 0; it.hasNext(); i++){
+ glcd.drawString(it.next(), 0, i*font.height, font.getBaselinePosition());
+ }
+ }
+ Delay.msDelay(PRINTDELAY);
+ }
+ }
+}
--- /dev/null
+package nl.ru.des;
+
+import lejos.hardware.ev3.EV3;
+import lejos.hardware.ev3.LocalEV3;
+import lejos.hardware.lcd.Font;
+import lejos.hardware.motor.EV3LargeRegulatedMotor;
+import lejos.hardware.port.MotorPort;
+import lejos.hardware.sensor.EV3ColorSensor;
+import lejos.robotics.subsumption.Arbitrator;
+import lejos.robotics.subsumption.Behavior;
+
+public class Main {
+ public static Arbitrator arbitrator;
+
+ public static void main(String[] args) {
+ EV3 brick = LocalEV3.get();
+
+ //BTMemory.getBTMemory(true, null);
+ BTMemory.getBTMemory(false, "Rover1");
+ LCDPrinter lcdprinter = new LCDPrinter(brick.getGraphicsLCD(), Font.getSmallFont());
+ LCDPrinter.print("Starting up systems");
+ lcdprinter.start();
+
+ LCDPrinter.print("Loading keylistener...");
+ brick.getKey("Escape").addKeyListener(new ButtonListener());
+
+ LCDPrinter.print("Loading motors...");
+ EV3LargeRegulatedMotor rightMotor = new EV3LargeRegulatedMotor(MotorPort.D);
+ EV3LargeRegulatedMotor leftMotor = new EV3LargeRegulatedMotor(MotorPort.A);
+ leftMotor.setSpeed(200);
+ rightMotor.setSpeed(200);
+ rightMotor.setAcceleration(1000);
+ leftMotor.setAcceleration(1000);
+
+// LCDPrinter.print("Loading touch sensors...");
+// EV3TouchSensor leftTouch = new EV3TouchSensor(brick.getPort("S1"));
+// EV3TouchSensor rightTouch = new EV3TouchSensor(brick.getPort("S4"));
+ LCDPrinter.print("Loading color sensor...");
+ EV3ColorSensor colorSensor = new EV3ColorSensor(brick.getPort("S2"));
+ ColorSensor colorSensorObject = new ColorSensor(colorSensor);
+
+// LCDPrinter.print("Loading ultrasone sensor...");
+// EV3UltrasonicSensor ultraSensor = new EV3UltrasonicSensor(brick.getPort("S3"));
+
+ LCDPrinter.print("Initializing behaviours...");
+ Behavior[] behaviorList = new Behavior[] {
+ new WandererBehaviour(colorSensorObject, leftMotor, rightMotor),
+ new StayInFieldBehaviour(colorSensorObject, leftMotor, rightMotor)
+ };
+
+
+ LCDPrinter.print("Initializing arbitrator...");
+ Main.arbitrator = new Arbitrator(behaviorList);
+ Main.arbitrator.start();
+ LCDPrinter.print("Takeoff!");
+ colorSensor.close();
+// ultraSensor.close();
+ }
+}
\ No newline at end of file
--- /dev/null
+package nl.ru.des;
+
+import lejos.hardware.motor.EV3LargeRegulatedMotor;
+import lejos.robotics.subsumption.Behavior;
+
+public class StayInFieldBehaviour implements Behavior {
+ public static final long BACKWARDSTIME = 250;
+ public static final long TURNTIME = 250;
+
+ private ColorSensor colorSensor;
+ private EV3LargeRegulatedMotor leftMotor, rightMotor;
+ private boolean suppressed;
+
+ public StayInFieldBehaviour(ColorSensor colorSensor, EV3LargeRegulatedMotor leftMotor, EV3LargeRegulatedMotor rightMotor) {
+ this.colorSensor = colorSensor;
+ this.leftMotor = leftMotor;
+ this.rightMotor = rightMotor;
+ }
+
+ @Override
+ public boolean takeControl() {
+ return colorSensor.getCurrentColor() == "Black";
+ }
+
+ @Override
+ public void action() {
+ suppressed = false;
+ int leftacc = leftMotor.getAcceleration();
+ int rightacc = rightMotor.getAcceleration();
+ leftMotor.setAcceleration(10000);
+ rightMotor.setAcceleration(10000);
+ leftMotor.backward();
+ rightMotor.backward();
+ long time = System.currentTimeMillis();
+ while(!suppressed && System.currentTimeMillis()-time > BACKWARDSTIME){
+ Thread.yield();
+ }
+
+ leftMotor.forward();
+ rightMotor.backward();
+ time = System.currentTimeMillis();
+ while(!suppressed && System.currentTimeMillis()-time > TURNTIME){
+ Thread.yield();
+ }
+ leftMotor.stop(true);
+ rightMotor.stop(true);
+ leftMotor.setAcceleration(leftacc);
+ rightMotor.setAcceleration(rightacc);
+ }
+
+ @Override
+ public void suppress() {
+ suppressed = true;
+ }
+}
\ No newline at end of file
--- /dev/null
+package nl.ru.des;
+
+import lejos.hardware.Sound;
+import lejos.hardware.motor.EV3LargeRegulatedMotor;
+import lejos.robotics.subsumption.Behavior;
+
+public class WandererBehaviour implements Behavior {
+ private EV3LargeRegulatedMotor leftMotor, rightMotor;
+ private ColorSensor colorSensor;
+ private boolean suppressed;
+
+ public WandererBehaviour(ColorSensor colorSensor, EV3LargeRegulatedMotor leftMotor, EV3LargeRegulatedMotor rightMotor) {
+ this.leftMotor = leftMotor;
+ this.rightMotor = rightMotor;
+ this.colorSensor = colorSensor;
+ }
+
+ @Override
+ public boolean takeControl() {
+ return true;
+ }
+
+ @Override
+ public void action() {
+ suppressed = false;
+ leftMotor.setSpeed(300);
+ rightMotor.setSpeed(300);
+ leftMotor.forward();
+ rightMotor.forward();
+ while(!suppressed){
+ String current = colorSensor.getCurrentColor();
+ if(current == "Yellow" || current == "Blue" || current == "Red"){
+ Sound.beep();
+ }
+ Thread.yield();
+ }
+ leftMotor.stop(true);
+ rightMotor.stop(true);
+ }
+
+ @Override
+ public void suppress() {
+ suppressed = true;
+ }
+
+}
repositories / des2015.git / commitdiff