From 4b888fe34d8482541e763df31c4285de3e9f4219 Mon Sep 17 00:00:00 2001 From: Mart Lubbers Date: Thu, 5 Nov 2015 14:55:13 +0100 Subject: [PATCH] update mart --- mart/ev3/ex2/nl/ru/des/BTController.java | 74 +++++++++++++++++++ mart/ev3/ex2/nl/ru/des/ButtonListener.java | 2 +- .../nl/ru/des/{Main.java => MarsRover.java} | 42 ++++++----- .../behaviours/AvoidHighObjectBehaviour.java | 27 +++---- .../behaviours/AvoidLowObjectBehaviour.java | 16 ++-- .../ru/des/behaviours/ReactiveBehaviour.java | 33 +++++++++ .../des/behaviours/StayInFieldBehaviour.java | 20 ++--- .../ru/des/behaviours/WandererBehaviour.java | 30 +++----- .../ex2/nl/ru/des/bluetooth/ColorMemory.java | 2 +- .../ex2/nl/ru/des/sensors/ColorSensor.java | 14 ---- .../ex2/nl/ru/des/sensors/SuperSensor.java | 29 -------- .../ex2/nl/ru/des/sensors/TouchSensor.java | 15 ---- .../nl/ru/des/sensors/UltraSoneSensor.java | 27 ------- 13 files changed, 170 insertions(+), 161 deletions(-) create mode 100644 mart/ev3/ex2/nl/ru/des/BTController.java rename mart/ev3/ex2/nl/ru/des/{Main.java => MarsRover.java} (67%) create mode 100644 mart/ev3/ex2/nl/ru/des/behaviours/ReactiveBehaviour.java delete mode 100644 mart/ev3/ex2/nl/ru/des/sensors/ColorSensor.java delete mode 100644 mart/ev3/ex2/nl/ru/des/sensors/SuperSensor.java delete mode 100644 mart/ev3/ex2/nl/ru/des/sensors/TouchSensor.java delete mode 100644 mart/ev3/ex2/nl/ru/des/sensors/UltraSoneSensor.java diff --git a/mart/ev3/ex2/nl/ru/des/BTController.java b/mart/ev3/ex2/nl/ru/des/BTController.java new file mode 100644 index 0000000..9426e56 --- /dev/null +++ b/mart/ev3/ex2/nl/ru/des/BTController.java @@ -0,0 +1,74 @@ +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 messageBuffer; + + private BTController(MessageHandler sh, NXTConnection connection) { + this.sh = sh; + dataInput = connection.openDataInputStream(); + dataOutput = connection.openDataOutputStream(); + messageBuffer = new LinkedList(); + } + + @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)); + } +} diff --git a/mart/ev3/ex2/nl/ru/des/ButtonListener.java b/mart/ev3/ex2/nl/ru/des/ButtonListener.java index 29df1ac..8d9d28b 100644 --- a/mart/ev3/ex2/nl/ru/des/ButtonListener.java +++ b/mart/ev3/ex2/nl/ru/des/ButtonListener.java @@ -13,4 +13,4 @@ class ButtonListener implements KeyListener { @Override public void keyReleased(Key k) {} -} \ No newline at end of file +} diff --git a/mart/ev3/ex2/nl/ru/des/Main.java b/mart/ev3/ex2/nl/ru/des/MarsRover.java similarity index 67% rename from mart/ev3/ex2/nl/ru/des/Main.java rename to mart/ev3/ex2/nl/ru/des/MarsRover.java index 1be815b..16b97c6 100644 --- a/mart/ev3/ex2/nl/ru/des/Main.java +++ b/mart/ev3/ex2/nl/ru/des/MarsRover.java @@ -10,6 +10,9 @@ import lejos.hardware.port.MotorPort; 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; @@ -19,13 +22,11 @@ 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 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()); @@ -40,8 +41,8 @@ public class Main { 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"); @@ -49,34 +50,35 @@ public class Main { 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 diff --git a/mart/ev3/ex2/nl/ru/des/behaviours/AvoidHighObjectBehaviour.java b/mart/ev3/ex2/nl/ru/des/behaviours/AvoidHighObjectBehaviour.java index 67ebad5..e9395f8 100644 --- a/mart/ev3/ex2/nl/ru/des/behaviours/AvoidHighObjectBehaviour.java +++ b/mart/ev3/ex2/nl/ru/des/behaviours/AvoidHighObjectBehaviour.java @@ -1,32 +1,38 @@ 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(); @@ -36,9 +42,4 @@ public class AvoidHighObjectBehaviour implements Behavior{ leftMotor.stop(true); rightMotor.stop(true); } - - @Override - public void suppress() { - suppressed = true; - } } \ No newline at end of file diff --git a/mart/ev3/ex2/nl/ru/des/behaviours/AvoidLowObjectBehaviour.java b/mart/ev3/ex2/nl/ru/des/behaviours/AvoidLowObjectBehaviour.java index 0ce1704..1c47f4f 100644 --- a/mart/ev3/ex2/nl/ru/des/behaviours/AvoidLowObjectBehaviour.java +++ b/mart/ev3/ex2/nl/ru/des/behaviours/AvoidLowObjectBehaviour.java @@ -1,32 +1,34 @@ 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; } diff --git a/mart/ev3/ex2/nl/ru/des/behaviours/ReactiveBehaviour.java b/mart/ev3/ex2/nl/ru/des/behaviours/ReactiveBehaviour.java new file mode 100644 index 0000000..2f33eb6 --- /dev/null +++ b/mart/ev3/ex2/nl/ru/des/behaviours/ReactiveBehaviour.java @@ -0,0 +1,33 @@ +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; + } +} diff --git a/mart/ev3/ex2/nl/ru/des/behaviours/StayInFieldBehaviour.java b/mart/ev3/ex2/nl/ru/des/behaviours/StayInFieldBehaviour.java index 3d9e7f6..75e6d2a 100644 --- a/mart/ev3/ex2/nl/ru/des/behaviours/StayInFieldBehaviour.java +++ b/mart/ev3/ex2/nl/ru/des/behaviours/StayInFieldBehaviour.java @@ -2,32 +2,31 @@ package nl.ru.des.behaviours; 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); @@ -50,9 +49,4 @@ public class StayInFieldBehaviour implements Behavior { 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/behaviours/WandererBehaviour.java b/mart/ev3/ex2/nl/ru/des/behaviours/WandererBehaviour.java index ec16941..f66bb62 100644 --- a/mart/ev3/ex2/nl/ru/des/behaviours/WandererBehaviour.java +++ b/mart/ev3/ex2/nl/ru/des/behaviours/WandererBehaviour.java @@ -1,23 +1,19 @@ 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; } @@ -28,25 +24,17 @@ public class WandererBehaviour implements Behavior { @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 diff --git a/mart/ev3/ex2/nl/ru/des/bluetooth/ColorMemory.java b/mart/ev3/ex2/nl/ru/des/bluetooth/ColorMemory.java index 5e89606..ec9c575 100644 --- a/mart/ev3/ex2/nl/ru/des/bluetooth/ColorMemory.java +++ b/mart/ev3/ex2/nl/ru/des/bluetooth/ColorMemory.java @@ -9,7 +9,7 @@ import lejos.robotics.Color; import nl.ru.des.LCDPrinter; public class ColorMemory implements MessageHandler { - private List COLORSTOFIND = Arrays.asList(new Integer[] {Color.BLUE, Color.RED, Color.YELLOW}); + public static final List COLORSTOFIND = Arrays.asList(new Integer[] {Color.BLUE, Color.RED, Color.YELLOW}); private Set colors; public ColorMemory() { diff --git a/mart/ev3/ex2/nl/ru/des/sensors/ColorSensor.java b/mart/ev3/ex2/nl/ru/des/sensors/ColorSensor.java deleted file mode 100644 index 884e0ef..0000000 --- a/mart/ev3/ex2/nl/ru/des/sensors/ColorSensor.java +++ /dev/null @@ -1,14 +0,0 @@ -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]; - } -} diff --git a/mart/ev3/ex2/nl/ru/des/sensors/SuperSensor.java b/mart/ev3/ex2/nl/ru/des/sensors/SuperSensor.java deleted file mode 100644 index affddbe..0000000 --- a/mart/ev3/ex2/nl/ru/des/sensors/SuperSensor.java +++ /dev/null @@ -1,29 +0,0 @@ -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); - } - } -} diff --git a/mart/ev3/ex2/nl/ru/des/sensors/TouchSensor.java b/mart/ev3/ex2/nl/ru/des/sensors/TouchSensor.java deleted file mode 100644 index 864635b..0000000 --- a/mart/ev3/ex2/nl/ru/des/sensors/TouchSensor.java +++ /dev/null @@ -1,15 +0,0 @@ -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; - } -} diff --git a/mart/ev3/ex2/nl/ru/des/sensors/UltraSoneSensor.java b/mart/ev3/ex2/nl/ru/des/sensors/UltraSoneSensor.java deleted file mode 100644 index 0f0a10b..0000000 --- a/mart/ev3/ex2/nl/ru/des/sensors/UltraSoneSensor.java +++ /dev/null @@ -1,27 +0,0 @@ -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]