+++ /dev/null
-package nl.ru.des;
-
-import java.util.Arrays;
-import java.util.HashSet;
-import java.util.List;
-import java.util.Set;
-
-public class ColorMemory implements MessageHandler {
- private List<String> COLORSTOFIND = Arrays.asList(new String[] { "Blue", "Red", "Yellow" });
- private Set<String> colors;
-
- public ColorMemory(){
- colors = new HashSet<String>();
- }
-
- @Override
- public void handleMessage(String message) {
- colors.add(message);
- LCDPrinter.print(message + " found");
- }
-
- public boolean finished() {
- return colors.equals(new HashSet<String>(COLORSTOFIND));
- }
-}
+++ /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 lejos.hardware.Sound;
-import lejos.hardware.motor.EV3LargeRegulatedMotor;
-import lejos.robotics.subsumption.Behavior;
-
-public class CommBehaviour implements Behavior {
-
- private String message;
-
- @Override
- public boolean takeControl() {
- // TODO Auto-generated method stub
- return false;
- }
-
- @Override
- public void action() {
- // TODO Auto-generated method stub
- LCDPrinter.print(message);
- }
-
- @Override
- public void suppress() {
- // TODO Auto-generated method stub
-
- }
-
-
-}
package nl.ru.des;
+import java.io.IOException;
+import java.io.OutputStream;
+import java.io.PrintStream;
import java.util.Deque;
-import java.util.Iterator;
import java.util.LinkedList;
-import lejos.hardware.lcd.Font;
-import lejos.hardware.lcd.GraphicsLCD;
+import lejos.hardware.lcd.TextLCD;
import lejos.utility.Delay;
-public class LCDPrinter extends Thread {
+public class LCDPrinter{
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 startLCDPrinter(final TextLCD glcd) {
+ new Thread(new Runnable(){
+ @Override
+ public void run() {
+ while (true) {
+ if (!buffer.isEmpty()) {
+ String c = buffer.remove();
+ if(c.length() > glcd.getTextWidth()){
+ buffer.addFirst(c.substring(glcd.getTextWidth(), c.length()));
+ c = c.substring(0, glcd.getTextWidth());
+ }
+ glcd.scroll();
+ glcd.drawString(c, 0, glcd.getTextHeight()-1);
+ }
+ Delay.msDelay(PRINTDELAY);
+ }
+ }
+ }).start();
+ LCDPrinter.print("LCDThread started");
}
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());
+
+ public static PrintStream getPrefixedPrintstream(final String prefix, final TextLCD glcd){
+ return new PrintStream(new OutputStream(){
+ private char[] line = new char[glcd.getTextWidth()];
+ private int index = 0;
+
+ @Override
+ public void write(int b) throws IOException {
+ if(index == line.length-prefix.length() || b == '\n'){
+ LCDPrinter.print(prefix + String.valueOf(line, 0, index));
+ index = 0;
+ } else {
+ line[index++] = (char)b;
}
}
- Delay.msDelay(PRINTDELAY);
- }
+ });
}
}
package nl.ru.des;
+import lejos.hardware.Button;
import lejos.hardware.ev3.EV3;
import lejos.hardware.ev3.LocalEV3;
import lejos.hardware.lcd.Font;
+import lejos.hardware.lcd.TextLCD;
import lejos.hardware.motor.EV3LargeRegulatedMotor;
import lejos.hardware.port.MotorPort;
import lejos.hardware.sensor.EV3ColorSensor;
+import lejos.hardware.sensor.EV3TouchSensor;
+import lejos.hardware.sensor.EV3UltrasonicSensor;
import lejos.robotics.subsumption.Arbitrator;
import lejos.robotics.subsumption.Behavior;
+import nl.ru.des.behaviours.AvoidHighObjectBehaviour;
+import nl.ru.des.behaviours.AvoidLowObjectBehaviour;
+import nl.ru.des.behaviours.ShutdownBehaviour;
+import nl.ru.des.behaviours.StayInFieldBehaviour;
+import nl.ru.des.behaviours.WandererBehaviour;
+import nl.ru.des.bluetooth.BTController;
+import nl.ru.des.bluetooth.ColorMemory;
+import nl.ru.des.sensors.ColorSensor;
+import nl.ru.des.sensors.TouchSensor;
+import nl.ru.des.sensors.UltraSoneSensor;
public class Main {
public static Arbitrator arbitrator;
public static void main(String[] args) {
EV3 brick = LocalEV3.get();
- LCDPrinter lcdprinter = new LCDPrinter(brick.getGraphicsLCD(), Font.getSmallFont());
- lcdprinter.start();
- /*
- * Start the slave first. "Socket is 18" is always displayed, we can just ignore this
- */
+ TextLCD tlcd = brick.getTextLCD(Font.getSmallFont());
+ LCDPrinter.startLCDPrinter(tlcd);
+ System.setOut(LCDPrinter.getPrefixedPrintstream("out: ", tlcd));
+ System.setErr(LCDPrinter.getPrefixedPrintstream("err: ", tlcd));
+
ColorMemory mh = new ColorMemory();
- //BTController btController = BTController.getBTMaster("Rover2", mh); // Master
- BTController btController = BTController.getBTSlave(mh); // Slave
+ LCDPrinter.print("Loading keylistener...");
+ brick.getKey("Escape").addKeyListener(new ButtonListener());
+
+ BTController btController;
+ LCDPrinter.print("Trying to pair");
+ btController = BTController.getBTMaster("Rover1", mh); // Master
+ //btController = BTController.getBTSlave(mh); // Slave
btController.start();
- BTController.SendMessage("Hi");
LCDPrinter.print("Robots are connected");
-
- LCDPrinter.print("Starting up systems");
-
-
- LCDPrinter.print("Loading keylistener...");
- brick.getKey("Escape").addKeyListener(new ButtonListener());
LCDPrinter.print("Loading motors...");
EV3LargeRegulatedMotor rightMotor = new EV3LargeRegulatedMotor(MotorPort.D);
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 touch sensors...");
+ TouchSensor leftTouchSensor = new TouchSensor(new EV3TouchSensor(brick.getPort("S1")));
+ TouchSensor rightTouchSensor = new TouchSensor(new EV3TouchSensor(brick.getPort("S4")));
LCDPrinter.print("Loading color sensor...");
- EV3ColorSensor colorSensor = new EV3ColorSensor(brick.getPort("S2"));
- ColorSensor colorSensorObject = new ColorSensor(colorSensor);
+ ColorSensor colorSensorObject = new ColorSensor(new EV3ColorSensor(brick.getPort("S2")));
-// LCDPrinter.print("Loading ultrasone sensor...");
-// EV3UltrasonicSensor ultraSensor = new EV3UltrasonicSensor(brick.getPort("S3"));
+ LCDPrinter.print("Loading ultrasone sensor...");
+ UltraSoneSensor ultraSoneSensor = new UltraSoneSensor(new EV3UltrasonicSensor(brick.getPort("S3")));
LCDPrinter.print("Initializing behaviours...");
Behavior[] behaviorList = new Behavior[] {
new WandererBehaviour(colorSensorObject, leftMotor, rightMotor, mh),
- new StayInFieldBehaviour(colorSensorObject, leftMotor, rightMotor)
- //new CommBehaviour(leftMotor, rightMotor)
+ new StayInFieldBehaviour(colorSensorObject, leftMotor, rightMotor),
+ new AvoidLowObjectBehaviour(leftMotor, rightMotor, leftTouchSensor, rightTouchSensor),
+ new AvoidHighObjectBehaviour(leftMotor, rightMotor, ultraSoneSensor),
+ new ShutdownBehaviour(mh)
};
LCDPrinter.print("Initializing arbitrator...");
Main.arbitrator = new Arbitrator(behaviorList);
+ LCDPrinter.print("Press any button for takeoff!");
+ Button.waitForAnyPress();
Main.arbitrator.start();
- LCDPrinter.print("Takeoff!");
- colorSensor.close();
-// ultraSensor.close();
+ LCDPrinter.print("Finish");
}
}
\ No newline at end of file
--- /dev/null
+package nl.ru.des.behaviours;
+
+import lejos.hardware.motor.EV3LargeRegulatedMotor;
+import lejos.robotics.subsumption.Behavior;
+import nl.ru.des.sensors.UltraSoneSensor;
+
+public class AvoidHighObjectBehaviour implements Behavior{
+ private static final long TURNTIME = 250;
+
+ private UltraSoneSensor ultraSone;
+ private EV3LargeRegulatedMotor leftMotor;
+ private EV3LargeRegulatedMotor rightMotor;
+ private boolean suppressed;
+
+ public AvoidHighObjectBehaviour(EV3LargeRegulatedMotor leftMotor, EV3LargeRegulatedMotor rightMotor,
+ UltraSoneSensor ultraSoneSensor) {
+ ultraSone = ultraSoneSensor;
+ this.leftMotor = leftMotor;
+ this.rightMotor = rightMotor;
+ }
+
+ @Override
+ public boolean takeControl() {
+ return ultraSone.getCurrentStatus();
+ }
+
+ @Override
+ public void action() {
+ suppressed = false;
+ rightMotor.backward();
+ leftMotor.forward();
+ long time = System.currentTimeMillis();
+ while (!suppressed && System.currentTimeMillis() - time > TURNTIME) {
+ Thread.yield();
+ }
+ leftMotor.stop(true);
+ rightMotor.stop(true);
+ }
+
+ @Override
+ public void suppress() {
+ suppressed = true;
+ }
+}
\ No newline at end of file
--- /dev/null
+package nl.ru.des.behaviours;
+
+import lejos.hardware.motor.EV3LargeRegulatedMotor;
+import lejos.robotics.subsumption.Behavior;
+import nl.ru.des.sensors.TouchSensor;
+
+public class AvoidLowObjectBehaviour implements Behavior {
+ private static final long BACKWARDSTIME = 250;
+ private static final long TURNTIME = 250;
+ private TouchSensor rightTouch, leftTouch;
+ private boolean avoidDirection;
+ private boolean suppressed;
+ private EV3LargeRegulatedMotor leftMotor, rightMotor;
+
+ public AvoidLowObjectBehaviour(EV3LargeRegulatedMotor leftMotor, EV3LargeRegulatedMotor rightMotor,
+ TouchSensor leftTouchSensor, TouchSensor rightTouchSensor) {
+ rightTouch = rightTouchSensor;
+ leftTouch = leftTouchSensor;
+ this.leftMotor = leftMotor;
+ this.rightMotor = rightMotor;
+ }
+
+ @Override
+ public boolean takeControl() {
+ if(leftTouch.getCurrentStatus()){
+ avoidDirection = false;
+ return true;
+ }
+ if(rightTouch.getCurrentStatus()){
+ avoidDirection = true;
+ return true;
+ }
+ return false;
+ }
+
+ @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();
+ }
+
+ if(avoidDirection){
+ leftMotor.backward();
+ rightMotor.forward();
+ } else {
+ leftMotor.forward();
+ rightMotor.backward();
+ }
+ time = System.currentTimeMillis();
+ while (!suppressed && System.currentTimeMillis() - time > TURNTIME) {
+ Thread.yield();
+ }
+ leftMotor.stop(true);
+ leftMotor.stop(true);
+ leftMotor.setAcceleration(leftacc);
+ leftMotor.setAcceleration(rightacc);
+ }
+
+ @Override
+ public void suppress() {
+ suppressed = true;
+ }
+}
\ No newline at end of file
--- /dev/null
+package nl.ru.des.behaviours;
+
+import lejos.robotics.subsumption.Behavior;
+import nl.ru.des.LCDPrinter;
+import nl.ru.des.bluetooth.ColorMemory;
+
+public class ShutdownBehaviour implements Behavior {
+ private ColorMemory cm;
+ private boolean suppressed;
+
+ public ShutdownBehaviour(ColorMemory cm) {
+ this.cm = cm;
+ }
+
+ @Override
+ public boolean takeControl() {
+ return cm.finished();
+ }
+
+ @Override
+ public void action() {
+ suppressed = false;
+ LCDPrinter.print("finish");
+ while (!suppressed){
+ Thread.yield();
+ }
+ }
+
+ @Override
+ public void suppress() {
+ suppressed = true;
+ }
+}
-package nl.ru.des;
+package nl.ru.des.behaviours;
import lejos.hardware.motor.EV3LargeRegulatedMotor;
+import lejos.robotics.Color;
import lejos.robotics.subsumption.Behavior;
+import nl.ru.des.sensors.ColorSensor;
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) {
+
+ 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";
+ return colorSensor.getCurrentColor() == Color.BLACK;
}
@Override
leftMotor.backward();
rightMotor.backward();
long time = System.currentTimeMillis();
- while(!suppressed && System.currentTimeMillis()-time > BACKWARDSTIME){
+ while (!suppressed && System.currentTimeMillis() - time > BACKWARDSTIME) {
Thread.yield();
}
leftMotor.forward();
rightMotor.backward();
time = System.currentTimeMillis();
- while(!suppressed && System.currentTimeMillis()-time > TURNTIME){
+ while (!suppressed && System.currentTimeMillis() - time > TURNTIME) {
Thread.yield();
}
leftMotor.stop(true);
-package nl.ru.des;
+package nl.ru.des.behaviours;
import lejos.hardware.Sound;
import lejos.hardware.motor.EV3LargeRegulatedMotor;
+import lejos.robotics.Color;
import lejos.robotics.subsumption.Behavior;
+import nl.ru.des.bluetooth.ColorMemory;
+import nl.ru.des.sensors.ColorSensor;
public class WandererBehaviour implements Behavior {
private EV3LargeRegulatedMotor leftMotor, rightMotor;
leftMotor.forward();
rightMotor.forward();
while (!suppressed) {
- String current = colorSensor.getCurrentColor();
- if (current == "Yellow" || current == "Blue" || current == "Red") {
+ int current = colorSensor.getCurrentColor();
+ if (current == Color.YELLOW || current == Color.BLUE || current == Color.RED) {
Sound.beep();
- BTController.SendMessage(current);
+ cm.addColor(current);
}
Thread.yield();
}
public void suppress() {
suppressed = true;
}
-
}
-package nl.ru.des;
+package nl.ru.des.bluetooth;
import java.io.DataInputStream;
import java.io.DataOutputStream;
while (true) {
try {
int c = dataInput.readUnsignedByte();
- sb.appendCodePoint(c);
if (c == '\n') {
sh.handleMessage(sb.toString());
sb = new StringBuilder();
+ } else {
+ sb.appendCodePoint(c);
}
} catch (IOException e) {
e.printStackTrace();
}
}
- public static void SendMessage(String message) {
+ public static void sendMessage(String message) {
messageBuffer.add(message + "\n");
}
--- /dev/null
+package nl.ru.des.bluetooth;
+
+import java.util.Arrays;
+import java.util.HashSet;
+import java.util.List;
+import java.util.Set;
+
+import lejos.robotics.Color;
+import nl.ru.des.LCDPrinter;
+
+public class ColorMemory implements MessageHandler {
+ private List<Integer> COLORSTOFIND = Arrays.asList(new Integer[] {Color.BLUE, Color.RED, Color.YELLOW});
+ private Set<Integer> colors;
+
+ public ColorMemory() {
+ colors = new HashSet<Integer>();
+ }
+
+ @Override
+ public void handleMessage(String message) {
+ colors.add(Integer.valueOf(message));
+ LCDPrinter.print(message + " received");
+ LCDPrinter.print(colors.toString());
+ }
+
+ public boolean finished() {
+ return colors.containsAll(COLORSTOFIND);
+ }
+
+ public void addColor(int current) {
+ BTController.sendMessage(Integer.toString(current));
+ colors.add(current);
+ LCDPrinter.print(current + " found");
+ LCDPrinter.print(colors.toString());
+ }
+}
-package nl.ru.des;
+package nl.ru.des.bluetooth;
public interface MessageHandler {
public void handleMessage(String message);
--- /dev/null
+package nl.ru.des.sensors;
+
+import lejos.hardware.sensor.EV3ColorSensor;
+
+public class ColorSensor extends SuperSensor{
+ public ColorSensor(EV3ColorSensor colorSensor){
+ super(colorSensor.getColorIDMode());
+ }
+
+ public int getCurrentColor(){
+ fetchSample();
+ return (int) samples[0];
+ }
+}
--- /dev/null
+package nl.ru.des.sensors;
+
+import lejos.robotics.SampleProvider;
+
+public abstract class SuperSensor {
+ public static final long SAMPLETIME = 50;
+
+ private long lastSampleTaken;
+ private SampleProvider sampleProvider;
+
+ protected float[] samples;
+
+ public SuperSensor(SampleProvider sampleProvider){
+ this.sampleProvider = sampleProvider;
+ this.samples = new float[sampleProvider.sampleSize()];
+ this.lastSampleTaken = System.currentTimeMillis()-SAMPLETIME;
+ }
+
+ public void fetchSample(){
+ fetchSample(false);
+ }
+
+ public void fetchSample(boolean always){
+ if(always || System.currentTimeMillis()-lastSampleTaken > SAMPLETIME){
+ lastSampleTaken = System.currentTimeMillis();
+ sampleProvider.fetchSample(samples, 0);
+ }
+ }
+}
--- /dev/null
+package nl.ru.des.sensors;
+
+import lejos.hardware.sensor.EV3TouchSensor;
+
+public class TouchSensor extends SuperSensor {
+
+ public TouchSensor(EV3TouchSensor leftTouch) {
+ super(leftTouch.getTouchMode());
+ }
+
+ public boolean getCurrentStatus(){
+ fetchSample();
+ return samples[0] == 1;
+ }
+}
--- /dev/null
+package nl.ru.des.sensors;
+
+import lejos.hardware.Button;
+import lejos.hardware.sensor.EV3UltrasonicSensor;
+import nl.ru.des.LCDPrinter;
+
+public class UltraSoneSensor extends SuperSensor {
+ private float limit;
+
+ public UltraSoneSensor(EV3UltrasonicSensor ultraSone) {
+ super(ultraSone.getDistanceMode());
+ calibrate();
+ }
+
+ private void calibrate(){
+ LCDPrinter.print("Calibrate ultrasone, place object in the turn radius...");
+ Button.waitForAnyPress();
+ fetchSample(true);
+ limit = samples[0];
+ LCDPrinter.print("Limit: " + limit);
+ }
+
+ public boolean getCurrentStatus(){
+ fetchSample();
+ return samples[0]<limit;
+ }
+}
repositories / des2015.git / commitdiff