From 461310f6749698875aa8275816df7ee011ffca3c Mon Sep 17 00:00:00 2001 From: Mart Lubbers Date: Wed, 21 Oct 2015 15:42:21 +0200 Subject: [PATCH] start with ex2 --- mart/ev3/ex2/nl/ru/des/BTMemory.java | 78 +++++++++++++++++++ mart/ev3/ex2/nl/ru/des/ButtonListener.java | 16 ++++ mart/ev3/ex2/nl/ru/des/ColorSensor.java | 78 +++++++++++++++++++ mart/ev3/ex2/nl/ru/des/LCDPrinter.java | 56 +++++++++++++ mart/ev3/ex2/nl/ru/des/Main.java | 59 ++++++++++++++ .../ex2/nl/ru/des/StayInFieldBehaviour.java | 55 +++++++++++++ mart/ev3/ex2/nl/ru/des/WandererBehaviour.java | 46 +++++++++++ 7 files changed, 388 insertions(+) create mode 100644 mart/ev3/ex2/nl/ru/des/BTMemory.java create mode 100644 mart/ev3/ex2/nl/ru/des/ButtonListener.java create mode 100644 mart/ev3/ex2/nl/ru/des/ColorSensor.java create mode 100644 mart/ev3/ex2/nl/ru/des/LCDPrinter.java create mode 100644 mart/ev3/ex2/nl/ru/des/Main.java create mode 100644 mart/ev3/ex2/nl/ru/des/StayInFieldBehaviour.java create mode 100644 mart/ev3/ex2/nl/ru/des/WandererBehaviour.java diff --git a/mart/ev3/ex2/nl/ru/des/BTMemory.java b/mart/ev3/ex2/nl/ru/des/BTMemory.java new file mode 100644 index 0000000..2321396 --- /dev/null +++ b/mart/ev3/ex2/nl/ru/des/BTMemory.java @@ -0,0 +1,78 @@ +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 COLORSTOFIND = Arrays.asList(new String[]{"Blue", "Red", "Yellow"}); + private Set colors; + protected NXTConnection connection; + protected DataOutputStream dataOutput; + protected DataInputStream dataInput; + + public BTMemory(){ + colors = new HashSet(); + setup(); + } + + public abstract void setup(); + + public void addColor(String c){ + colors.add(c); + } + + public boolean finished(){ + return colors.equals(new HashSet(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(); + } + }; + } + } +} diff --git a/mart/ev3/ex2/nl/ru/des/ButtonListener.java b/mart/ev3/ex2/nl/ru/des/ButtonListener.java new file mode 100644 index 0000000..29df1ac --- /dev/null +++ b/mart/ev3/ex2/nl/ru/des/ButtonListener.java @@ -0,0 +1,16 @@ +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 diff --git a/mart/ev3/ex2/nl/ru/des/ColorSensor.java b/mart/ev3/ex2/nl/ru/des/ColorSensor.java new file mode 100644 index 0000000..616b275 --- /dev/null +++ b/mart/ev3/ex2/nl/ru/des/ColorSensor.java @@ -0,0 +1,78 @@ +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 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(); + 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; + } +} diff --git a/mart/ev3/ex2/nl/ru/des/LCDPrinter.java b/mart/ev3/ex2/nl/ru/des/LCDPrinter.java new file mode 100644 index 0000000..104521a --- /dev/null +++ b/mart/ev3/ex2/nl/ru/des/LCDPrinter.java @@ -0,0 +1,56 @@ +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 buffer = new LinkedList(); + + private GraphicsLCD glcd; + private Font font; + private Deque lcdbuffer; + private int charwidth; + + public LCDPrinter(GraphicsLCD glcd, Font font) { + this.glcd = glcd; + this.font = font; + glcd.setFont(font); + lcdbuffer = new LinkedList(); + charwidth = glcd.getWidth()/font.width; + for(int i = 0; i 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); + } + } +} diff --git a/mart/ev3/ex2/nl/ru/des/Main.java b/mart/ev3/ex2/nl/ru/des/Main.java new file mode 100644 index 0000000..d03946c --- /dev/null +++ b/mart/ev3/ex2/nl/ru/des/Main.java @@ -0,0 +1,59 @@ +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 diff --git a/mart/ev3/ex2/nl/ru/des/StayInFieldBehaviour.java b/mart/ev3/ex2/nl/ru/des/StayInFieldBehaviour.java new file mode 100644 index 0000000..7534d3c --- /dev/null +++ b/mart/ev3/ex2/nl/ru/des/StayInFieldBehaviour.java @@ -0,0 +1,55 @@ +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 diff --git a/mart/ev3/ex2/nl/ru/des/WandererBehaviour.java b/mart/ev3/ex2/nl/ru/des/WandererBehaviour.java new file mode 100644 index 0000000..0b8a6fd --- /dev/null +++ b/mart/ev3/ex2/nl/ru/des/WandererBehaviour.java @@ -0,0 +1,46 @@ +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; + } + +} -- 2.20.1