--- /dev/null
+package nl.ru.des;
+
+import java.io.DataInputStream;
+import java.io.DataOutputStream;
+import java.io.IOException;
+import java.util.LinkedList;
+import java.util.Queue;
+
+import lejos.remote.nxt.BTConnector;
+import lejos.remote.nxt.NXTConnection;
+import nl.ru.des.bluetooth.MessageHandler;
+
+public class BTController extends Thread {
+ protected NXTConnection connection;
+ protected DataOutputStream dataOutput;
+ protected DataInputStream dataInput;
+ protected MessageHandler sh;
+ public static Queue<String> messageBuffer;
+
+ private BTController(MessageHandler sh, NXTConnection connection) {
+ this.sh = sh;
+ dataInput = connection.openDataInputStream();
+ dataOutput = connection.openDataOutputStream();
+ messageBuffer = new LinkedList<String>();
+ }
+
+ @Override
+ public void run() {
+ Thread receiver = new Thread() {
+ @Override
+ public void run() {
+ StringBuilder sb = new StringBuilder();
+ while (true) {
+ try {
+ int c = dataInput.readUnsignedByte();
+ sb.appendCodePoint(c);
+ if (c == '\n') {
+ sh.handleMessage(sb.toString());
+ sb = new StringBuilder();
+ }
+ } catch (IOException e) {
+ e.printStackTrace();
+ }
+ }
+ }
+ };
+ receiver.start();
+ while (true) {
+ if (!messageBuffer.isEmpty()) {
+ try {
+ dataOutput.write(messageBuffer.poll().getBytes());
+ dataOutput.flush();
+ } catch (IOException e) {
+ e.printStackTrace();
+ }
+ }
+ Thread.yield();
+ }
+ }
+
+ public static void SendMessage(String message) {
+ messageBuffer.add(message + "\n");
+ }
+
+ public static BTController getBTMaster(final String slave, final MessageHandler sh) {
+ BTConnector bt = new BTConnector();
+ return new BTController(sh, bt.connect(slave, NXTConnection.RAW));
+ }
+
+ public static BTController getBTSlave(MessageHandler sh) {
+ BTConnector bt = new BTConnector();
+ return new BTController(sh, bt.waitForConnection(60000, NXTConnection.RAW));
+ }
+}
@Override
public void keyReleased(Key k) {}
-}
\ No newline at end of file
+}
import lejos.hardware.sensor.EV3ColorSensor;
import lejos.hardware.sensor.EV3TouchSensor;
import lejos.hardware.sensor.EV3UltrasonicSensor;
+import lejos.robotics.SampleProvider;
+import lejos.robotics.filter.ConcatenationFilter;
+import lejos.robotics.filter.MeanFilter;
import lejos.robotics.subsumption.Arbitrator;
import lejos.robotics.subsumption.Behavior;
import nl.ru.des.behaviours.AvoidHighObjectBehaviour;
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 class MarsRover {
+ public static final float SAMPLERATE = 100;
+
+ @SuppressWarnings("resource")
public static void main(String[] args) {
EV3 brick = LocalEV3.get();
TextLCD tlcd = brick.getTextLCD(Font.getSmallFont());
BTController btController;
LCDPrinter.print("Trying to pair");
- btController = BTController.getBTMaster("Rover1", mh); // Master
- //btController = BTController.getBTSlave(mh); // Slave
+ //btController = BTController.getBTMaster("Rover1", mh); // Master
+ btController = BTController.getBTSlave(mh); // Slave
btController.start();
LCDPrinter.print("Robots are connected");
LCDPrinter.print("Loading motors...");
EV3LargeRegulatedMotor rightMotor = new EV3LargeRegulatedMotor(MotorPort.D);
EV3LargeRegulatedMotor leftMotor = new EV3LargeRegulatedMotor(MotorPort.A);
- leftMotor.setSpeed(200);
- rightMotor.setSpeed(200);
+ leftMotor.setSpeed(300);
+ rightMotor.setSpeed(300);
rightMotor.setAcceleration(1000);
leftMotor.setAcceleration(1000);
LCDPrinter.print("Loading touch sensors...");
- TouchSensor leftTouchSensor = new TouchSensor(new EV3TouchSensor(brick.getPort("S1")));
- TouchSensor rightTouchSensor = new TouchSensor(new EV3TouchSensor(brick.getPort("S4")));
+ SampleProvider touch = new ConcatenationFilter(
+ new EV3TouchSensor(brick.getPort("S1")).getTouchMode(),
+ new EV3TouchSensor(brick.getPort("S4")).getTouchMode());
LCDPrinter.print("Loading color sensor...");
- ColorSensor colorSensorObject = new ColorSensor(new EV3ColorSensor(brick.getPort("S2")));
+ SampleProvider color = new EV3ColorSensor(brick.getPort("S2")).getColorIDMode();
LCDPrinter.print("Loading ultrasone sensor...");
- UltraSoneSensor ultraSoneSensor = new UltraSoneSensor(new EV3UltrasonicSensor(brick.getPort("S3")));
+ SampleProvider ultraSonic = new MeanFilter(new EV3UltrasonicSensor(brick.getPort("S3")).getDistanceMode(), 3);
LCDPrinter.print("Initializing behaviours...");
Behavior[] behaviorList = new Behavior[] {
- new WandererBehaviour(colorSensorObject, leftMotor, rightMotor, mh),
- new StayInFieldBehaviour(colorSensorObject, leftMotor, rightMotor),
- new AvoidLowObjectBehaviour(leftMotor, rightMotor, leftTouchSensor, rightTouchSensor),
- new AvoidHighObjectBehaviour(leftMotor, rightMotor, ultraSoneSensor),
+ new WandererBehaviour(color, leftMotor, rightMotor, mh),
+ new StayInFieldBehaviour(color, leftMotor, rightMotor),
+ new AvoidLowObjectBehaviour(leftMotor, rightMotor, touch),
+ new AvoidHighObjectBehaviour(leftMotor, rightMotor, ultraSonic),
new ShutdownBehaviour(mh)
};
LCDPrinter.print("Initializing arbitrator...");
- Main.arbitrator = new Arbitrator(behaviorList);
+ Arbitrator arbitrator = new Arbitrator(behaviorList);
LCDPrinter.print("Press any button for takeoff!");
Button.waitForAnyPress();
- Main.arbitrator.start();
+ arbitrator.start();
LCDPrinter.print("Finish");
}
}
\ No newline at end of file
package nl.ru.des.behaviours;
+import lejos.hardware.Button;
import lejos.hardware.motor.EV3LargeRegulatedMotor;
+import lejos.robotics.SampleProvider;
import lejos.robotics.subsumption.Behavior;
-import nl.ru.des.sensors.UltraSoneSensor;
+import nl.ru.des.LCDPrinter;
-public class AvoidHighObjectBehaviour implements Behavior{
+public class AvoidHighObjectBehaviour extends ReactiveBehaviour implements Behavior {
private static final long TURNTIME = 250;
- private UltraSoneSensor ultraSone;
private EV3LargeRegulatedMotor leftMotor;
private EV3LargeRegulatedMotor rightMotor;
- private boolean suppressed;
+ private float limit;
public AvoidHighObjectBehaviour(EV3LargeRegulatedMotor leftMotor, EV3LargeRegulatedMotor rightMotor,
- UltraSoneSensor ultraSoneSensor) {
- ultraSone = ultraSoneSensor;
+ SampleProvider ultraSone) {
+ super(ultraSone);
this.leftMotor = leftMotor;
this.rightMotor = rightMotor;
+ LCDPrinter.print("Place object in turn radius and press a key");
+ Button.waitForAnyPress();
+ fetchSample();
+ limit = samples[0];
}
@Override
public boolean takeControl() {
- return ultraSone.getCurrentStatus();
+ fetchSample();
+ return samples[0] < limit;
}
@Override
public void action() {
- suppressed = false;
+ super.action();
rightMotor.backward();
leftMotor.forward();
long time = System.currentTimeMillis();
leftMotor.stop(true);
rightMotor.stop(true);
}
-
- @Override
- public void suppress() {
- suppressed = true;
- }
}
\ No newline at end of file
package nl.ru.des.behaviours;
import lejos.hardware.motor.EV3LargeRegulatedMotor;
+import lejos.robotics.SampleProvider;
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 SampleProvider touch;
+ private float[] samples;
private boolean avoidDirection;
private boolean suppressed;
private EV3LargeRegulatedMotor leftMotor, rightMotor;
public AvoidLowObjectBehaviour(EV3LargeRegulatedMotor leftMotor, EV3LargeRegulatedMotor rightMotor,
- TouchSensor leftTouchSensor, TouchSensor rightTouchSensor) {
- rightTouch = rightTouchSensor;
- leftTouch = leftTouchSensor;
+ SampleProvider touch) {
+ this.touch = touch;
+ this.samples = new float[touch.sampleSize()];
this.leftMotor = leftMotor;
this.rightMotor = rightMotor;
}
@Override
public boolean takeControl() {
- if(leftTouch.getCurrentStatus()){
+ touch.fetchSample(samples, 0);
+ if(samples[0]>0){
avoidDirection = false;
return true;
}
- if(rightTouch.getCurrentStatus()){
+ if(samples[1]>0){
avoidDirection = true;
return true;
}
--- /dev/null
+package nl.ru.des.behaviours;
+
+import lejos.robotics.SampleProvider;
+import lejos.robotics.subsumption.Behavior;
+
+public abstract class ReactiveBehaviour implements Behavior {
+ private SampleProvider sampleProvider;
+ private long lastSampleTaken;
+ protected float[] samples;
+ protected boolean suppressed;
+
+ public ReactiveBehaviour(SampleProvider sampleProvider) {
+ this.sampleProvider = sampleProvider;
+ this.samples = new float[sampleProvider.sampleSize()];
+ this.lastSampleTaken = System.currentTimeMillis();
+ }
+
+ public void fetchSample(){
+ if(System.currentTimeMillis()-lastSampleTaken > 100){
+ sampleProvider.fetchSample(samples, 0);
+ }
+ }
+
+ @Override
+ public void action() {
+ suppressed = true;
+ }
+
+ @Override
+ public void suppress() {
+ suppressed = false;
+ }
+}
import lejos.hardware.motor.EV3LargeRegulatedMotor;
import lejos.robotics.Color;
+import lejos.robotics.SampleProvider;
import lejos.robotics.subsumption.Behavior;
-import nl.ru.des.sensors.ColorSensor;
-public class StayInFieldBehaviour implements Behavior {
+public class StayInFieldBehaviour extends ReactiveBehaviour 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,
+ public StayInFieldBehaviour(SampleProvider color, EV3LargeRegulatedMotor leftMotor,
EV3LargeRegulatedMotor rightMotor) {
- this.colorSensor = colorSensor;
+ super(color);
this.leftMotor = leftMotor;
this.rightMotor = rightMotor;
}
@Override
public boolean takeControl() {
- return colorSensor.getCurrentColor() == Color.BLACK;
+ super.fetchSample();
+ return samples[0] == Color.BLACK;
}
@Override
public void action() {
- suppressed = false;
+ super.action();
int leftacc = leftMotor.getAcceleration();
int rightacc = rightMotor.getAcceleration();
leftMotor.setAcceleration(10000);
leftMotor.setAcceleration(leftacc);
rightMotor.setAcceleration(rightacc);
}
-
- @Override
- public void suppress() {
- suppressed = true;
- }
}
\ No newline at end of file
package nl.ru.des.behaviours;
-import lejos.hardware.Sound;
import lejos.hardware.motor.EV3LargeRegulatedMotor;
-import lejos.robotics.Color;
+import lejos.robotics.SampleProvider;
import lejos.robotics.subsumption.Behavior;
import nl.ru.des.bluetooth.ColorMemory;
-import nl.ru.des.sensors.ColorSensor;
-public class WandererBehaviour implements Behavior {
+public class WandererBehaviour extends ReactiveBehaviour implements Behavior {
private EV3LargeRegulatedMotor leftMotor, rightMotor;
- private ColorSensor colorSensor;
- private boolean suppressed;
private ColorMemory cm;
- public WandererBehaviour(ColorSensor colorSensor, EV3LargeRegulatedMotor leftMotor,
+ public WandererBehaviour(SampleProvider color, EV3LargeRegulatedMotor leftMotor,
EV3LargeRegulatedMotor rightMotor, ColorMemory cm) {
+ super(color);
this.leftMotor = leftMotor;
this.rightMotor = rightMotor;
- this.colorSensor = colorSensor;
this.cm = cm;
}
@Override
public void action() {
- suppressed = false;
- leftMotor.setSpeed(300);
- rightMotor.setSpeed(300);
+ super.action();
leftMotor.forward();
rightMotor.forward();
while (!suppressed) {
- int current = colorSensor.getCurrentColor();
- if (current == Color.YELLOW || current == Color.BLUE || current == Color.RED) {
- Sound.beep();
- cm.addColor(current);
+ super.fetchSample();
+ if (ColorMemory.COLORSTOFIND.contains((int)samples[0])) {
+ cm.addColor((int)samples[0]);
}
Thread.yield();
}
leftMotor.stop(true);
rightMotor.stop(true);
}
-
- @Override
- public void suppress() {
- suppressed = true;
- }
-}
+}
\ No newline at end of file
import nl.ru.des.LCDPrinter;
public class ColorMemory implements MessageHandler {
- private List<Integer> COLORSTOFIND = Arrays.asList(new Integer[] {Color.BLUE, Color.RED, Color.YELLOW});
+ public static final List<Integer> COLORSTOFIND = Arrays.asList(new Integer[] {Color.BLUE, Color.RED, Color.YELLOW});
private Set<Integer> colors;
public ColorMemory() {
+++ /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