From 725c0fbcb9c645aa42fe931d7ed28aac73468ef5 Mon Sep 17 00:00:00 2001 From: Natanael Adityasatria Date: Fri, 30 Oct 2015 20:18:33 +0100 Subject: [PATCH] Implemented communication protocol --- mart/ev3/ex2/nl/ru/des/BTMemory.java | 130 +++++++----------- mart/ev3/ex2/nl/ru/des/ColorMemory.java | 25 ++++ mart/ev3/ex2/nl/ru/des/Main.java | 18 ++- mart/ev3/ex2/nl/ru/des/MessageHandler.java | 5 + mart/ev3/ex2/nl/ru/des/WandererBehaviour.java | 14 +- 5 files changed, 99 insertions(+), 93 deletions(-) create mode 100644 mart/ev3/ex2/nl/ru/des/ColorMemory.java create mode 100644 mart/ev3/ex2/nl/ru/des/MessageHandler.java diff --git a/mart/ev3/ex2/nl/ru/des/BTMemory.java b/mart/ev3/ex2/nl/ru/des/BTMemory.java index e47ab0e..75fc60a 100644 --- a/mart/ev3/ex2/nl/ru/des/BTMemory.java +++ b/mart/ev3/ex2/nl/ru/des/BTMemory.java @@ -3,103 +3,71 @@ package nl.ru.des; import java.io.DataInputStream; import java.io.DataOutputStream; import java.io.IOException; -import java.io.OutputStream; -import java.io.PrintWriter; -import java.util.Arrays; -import java.util.HashSet; -import java.util.List; -import java.util.Set; +import java.util.LinkedList; +import java.util.Queue; -import lejos.hardware.Sound; -import lejos.hardware.lcd.LCD; import lejos.remote.nxt.BTConnector; import lejos.remote.nxt.NXTConnection; -import lejos.utility.Delay; -public abstract class BTMemory extends Thread { - private List COLORSTOFIND = Arrays.asList(new String[] { "Blue", "Red", "Yellow" }); - private Set colors; +public class BTController extends Thread { protected NXTConnection connection; - protected OutputStream dataOutput; + protected DataOutputStream dataOutput; protected DataInputStream dataInput; - protected static PrintWriter writer; + protected MessageHandler sh; + public static Queue messageBuffer; - 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)); + private BTController(MessageHandler sh, NXTConnection connection) { + this.dataInput = connection.openDataInputStream(); + this.dataOutput = connection.openDataOutputStream(); + this.sh = sh; + messageBuffer = new LinkedList(); } @Override public void run() { - try { - while (true) { - if (dataInput.available() > 0) { - //String msg = dataInput.readLine(); - LCDPrinter.print(Character.toChars(dataInput.read()).toString()); - } - } - } - catch (IOException e) { - // TODO Auto-generated catch block - e.printStackTrace(); - } - } - - public static void SendMessage(String message){ - writer.println(message); - writer.flush(); - } - - public static BTMemory getBTMemory(boolean master, final String rovername) { - if (master) { - return new BTMemory() { - @Override - public void setup() { - //LCD.drawString("Create connection", 0, 0); - BTConnector btconnector = new BTConnector(); - //LCD.drawString("BTConnector created", 0, 1); - connection = btconnector.connect(rovername, NXTConnection.RAW); - //LCD.drawString("Connection created", 0, 2); - dataInput = connection.openDataInputStream(); - dataOutput = connection.openOutputStream(); - writer = new PrintWriter(dataOutput); - //LCD.drawString("Input output created", 0, 3); + Thread receiver = new Thread() { + @Override + public void run() { + StringBuilder sb = new StringBuilder(); + while (true) { try { - //dataOutput.write(77); - writer.println("Hellow"); - writer.flush(); - //LCD.clear(); - //LCD.drawString("Message sent", 0, 4); - } catch (Exception e) { + int c = dataInput.readUnsignedByte(); + sb.appendCodePoint(c); + if (c == '\n') { + sh.handleMessage(sb.toString()); + sb = new StringBuilder(); + } + } catch (IOException e) { e.printStackTrace(); } } - }; - } else { - return new BTMemory() { - @Override - public void setup() { - //LCD.drawString("Create connection", 0, 0); - BTConnector btconnector = new BTConnector(); - //LCD.drawString("BTConnector created", 0, 1); - connection = btconnector.waitForConnection(60000, NXTConnection.RAW); - //LCD.drawString("Connection created", 0, 2); - dataInput = connection.openDataInputStream(); - dataOutput = connection.openOutputStream(); - writer = new PrintWriter(dataOutput); - //LCD.drawString("Input output created", 0, 3); + } + }; + 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 btconnector = new BTConnector(); + return new BTController(sh, btconnector.connect(slave, NXTConnection.RAW)); + } + + public static BTController getBTSlave(MessageHandler sh) { + BTConnector btconnector = new BTConnector(); + return new BTController(sh, btconnector.waitForConnection(60000, NXTConnection.RAW)); + } } \ No newline at end of file diff --git a/mart/ev3/ex2/nl/ru/des/ColorMemory.java b/mart/ev3/ex2/nl/ru/des/ColorMemory.java new file mode 100644 index 0000000..7e92f12 --- /dev/null +++ b/mart/ev3/ex2/nl/ru/des/ColorMemory.java @@ -0,0 +1,25 @@ +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 COLORSTOFIND = Arrays.asList(new String[] { "Blue", "Red", "Yellow" }); + private Set colors; + + public ColorMemory(){ + colors = new HashSet(); + } + + @Override + public void handleMessage(String message) { + colors.add(message); + LCDPrinter.print(message + " found"); + } + + public boolean finished() { + return colors.equals(new HashSet(COLORSTOFIND)); + } +} diff --git a/mart/ev3/ex2/nl/ru/des/Main.java b/mart/ev3/ex2/nl/ru/des/Main.java index 7c78e31..3a7e256 100644 --- a/mart/ev3/ex2/nl/ru/des/Main.java +++ b/mart/ev3/ex2/nl/ru/des/Main.java @@ -3,7 +3,6 @@ package nl.ru.des; import lejos.hardware.ev3.EV3; import lejos.hardware.ev3.LocalEV3; import lejos.hardware.lcd.Font; -import lejos.hardware.lcd.LCD; import lejos.hardware.motor.EV3LargeRegulatedMotor; import lejos.hardware.port.MotorPort; import lejos.hardware.sensor.EV3ColorSensor; @@ -16,16 +15,21 @@ public class Main { 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 */ - //BTMemory.getBTMemory(false, "Rover2"); - BTMemory.getBTMemory(true, "Rover1"); + ColorMemory mh = new ColorMemory(); + + //BTController btController = BTController.getBTMaster("Rover2", mh); // Master + BTController btController = BTController.getBTSlave(mh); // Slave + btController.start(); + BTController.SendMessage("Hi"); + LCDPrinter.print("Robots are connected"); LCDPrinter.print("Starting up systems"); - lcdprinter.start(); + LCDPrinter.print("Loading keylistener..."); brick.getKey("Escape").addKeyListener(new ButtonListener()); @@ -50,11 +54,11 @@ public class Main { LCDPrinter.print("Initializing behaviours..."); Behavior[] behaviorList = new Behavior[] { - new WandererBehaviour(colorSensorObject, leftMotor, rightMotor), + new WandererBehaviour(colorSensorObject, leftMotor, rightMotor, mh), new StayInFieldBehaviour(colorSensorObject, leftMotor, rightMotor) + //new CommBehaviour(leftMotor, rightMotor) }; - LCDPrinter.print("Initializing arbitrator..."); Main.arbitrator = new Arbitrator(behaviorList); Main.arbitrator.start(); diff --git a/mart/ev3/ex2/nl/ru/des/MessageHandler.java b/mart/ev3/ex2/nl/ru/des/MessageHandler.java new file mode 100644 index 0000000..3f4abb6 --- /dev/null +++ b/mart/ev3/ex2/nl/ru/des/MessageHandler.java @@ -0,0 +1,5 @@ +package nl.ru.des; + +public interface MessageHandler { + public void handleMessage(String message); +} \ 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 index 0b8a6fd..a0c8bcc 100644 --- a/mart/ev3/ex2/nl/ru/des/WandererBehaviour.java +++ b/mart/ev3/ex2/nl/ru/des/WandererBehaviour.java @@ -8,16 +8,19 @@ public class WandererBehaviour implements Behavior { private EV3LargeRegulatedMotor leftMotor, rightMotor; private ColorSensor colorSensor; private boolean suppressed; - - public WandererBehaviour(ColorSensor colorSensor, EV3LargeRegulatedMotor leftMotor, EV3LargeRegulatedMotor rightMotor) { + private ColorMemory cm; + + public WandererBehaviour(ColorSensor colorSensor, EV3LargeRegulatedMotor leftMotor, + EV3LargeRegulatedMotor rightMotor, ColorMemory cm) { this.leftMotor = leftMotor; this.rightMotor = rightMotor; this.colorSensor = colorSensor; + this.cm = cm; } @Override public boolean takeControl() { - return true; + return !cm.finished(); } @Override @@ -27,10 +30,11 @@ public class WandererBehaviour implements Behavior { rightMotor.setSpeed(300); leftMotor.forward(); rightMotor.forward(); - while(!suppressed){ + while (!suppressed) { String current = colorSensor.getCurrentColor(); - if(current == "Yellow" || current == "Blue" || current == "Red"){ + if (current == "Yellow" || current == "Blue" || current == "Red") { Sound.beep(); + BTController.SendMessage(current); } Thread.yield(); } -- 2.20.1