Finishes the communication threads anc closes the Bluetooth data streams, + * then quits the application.
+ */ + private static void quit(){ + if(recvThread != null) recvThread.finish(); + if(sendThread != null) sendThread.finish(); + + if(bluetoothConnection != null){ + try{ + dataOutputStream.close(); + dataInputStream.close(); + }catch(IOException io){ + System.out.println(io.getMessage()); + } + bluetoothConnection.close(); + } + + System.exit(0); + } + + /** + *Application entry point.
+ *Resets the motors, calibrates the light sensor and starts the + * networking threads.
+ * + * @param args Command line arguments. + */ public static void main(String[] args){ - System.out.println("Hello, world!"); - Button.waitForAnyPress(); + // Set a listener to force quit if the ESCAPE button is pressed. + Button.ESCAPE.addButtonListener(new QuitButtonListener()); + + // Reset the rotation counts of the motors. + Motor.A.resetTachoCount(); + Motor.B.resetTachoCount(); + Motor.C.resetTachoCount(); + + // Start the light sensor and calibrate it. + LightSensor lightSensor = new LightSensor(SensorPort.S1); + lightSensor.setFloodlight(false); + + System.out.println("Point at dark\nand press ENTER"); + Button.ENTER.waitForPress(); + lightSensor.calibrateLow(); + System.out.println("--/--"); + + System.out.println("Point at light\nand press ENTER"); + Button.ENTER.waitForPress(); + lightSensor.calibrateHigh(); + System.out.println("--/--"); + + // Connect with a Bluetooth device in raw mode. Then get the connection + // streams. + bluetoothConnection = Bluetooth.waitForConnection(); + bluetoothConnection.setIOMode(NXTConnection.RAW); + dataOutputStream = bluetoothConnection.openDataOutputStream(); + dataInputStream = bluetoothConnection.openDataInputStream(); + + System.out.println("Connected"); + + // Start the networking threads and wait for them to finish. + sendThread = new SensorReportThread(dataOutputStream, lightSensor); + recvThread = new MotorControlThread(dataInputStream); + + recvThread.start(); + sendThread.start(); + + try{ + recvThread.join(); + sendThread.join(); + }catch(InterruptedException i){ } + + quit(); + } + + /** + *Force quit button listener.
+ */ + private static class QuitButtonListener implements ButtonListener{ + /** + * Force quit. + */ + @Override + public void buttonPressed(Button b){ + System.exit(0); + //quit(); + } + + /** + * Do nothing. + */ + @Override + public void buttonReleased(Button b){ } } } diff --git a/src/ve/ucv/ciens/ccg/nxtarbot/protocol/MotorMasks.java b/src/ve/ucv/ciens/ccg/nxtarbot/protocol/MotorMasks.java new file mode 100644 index 0000000..505f301 --- /dev/null +++ b/src/ve/ucv/ciens/ccg/nxtarbot/protocol/MotorMasks.java @@ -0,0 +1,32 @@ +/* + * Copyright (C) 2014 Miguel Angel Astor Romero + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package ve.ucv.ciens.ccg.nxtarbot.protocol; + +/** + *Bit masks used to code/decode the control instructions sent by NxtAR-cam to + * NxtAR-bot.
+ *Expansions 1-3 are currently unused.
+ */ +public abstract class MotorMasks { + public static final byte MOTOR_A = (byte)0x01; + public static final byte MOTOR_B = (byte)0x02; + public static final byte MOTOR_C = (byte)0x04; + public static final byte DIRECTION = (byte)0x08; + public static final byte RECENTER = (byte)0x10; + public static final byte EXPANSION_1 = (byte)0x20; + public static final byte EXPANSION_2 = (byte)0x20; + public static final byte EXPANSION_3 = (byte)0x20; +} diff --git a/src/ve/ucv/ciens/ccg/nxtarbot/threads/MotorControlThread.java b/src/ve/ucv/ciens/ccg/nxtarbot/threads/MotorControlThread.java new file mode 100644 index 0000000..a6e4503 --- /dev/null +++ b/src/ve/ucv/ciens/ccg/nxtarbot/threads/MotorControlThread.java @@ -0,0 +1,130 @@ +/* + * Copyright (C) 2014 Miguel Angel Astor Romero + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package ve.ucv.ciens.ccg.nxtarbot.threads; + +import java.io.DataInputStream; +import java.io.IOException; + +import lejos.nxt.BasicMotorPort; +import lejos.nxt.Battery; +import lejos.nxt.Motor; +import ve.ucv.ciens.ccg.nxtarbot.protocol.MotorMasks; + +/** + *Class to control the motors on the robot based on isntructions received via + * Bluetooth.
+ * + *The instructions are codified on a very simple protocol:
+ *
Marks this thread as ready to finish cleanly.
+ */ + public void finish(){ + done = true; + } + + /** + *Receive and process motor control instructions via Bluetooth.
+ */ + @Override + public void run(){ + boolean motorA, motorB, motorC, recenterMotorB; + int direction, rotation, tacho; + byte[] message = new byte[2]; + + while(!done){ + try{ + // Read the two bytes indicated by the protocol. + message[0] = inputStream.readByte(); + message[1] = inputStream.readByte(); + + // Decode the instruction parameters. + recenterMotorB = (message[0] & MotorMasks.RECENTER) > 0 ? true : false; + motorA = (message[0] & MotorMasks.MOTOR_A) > 0 ? true : false; + motorB = (message[0] & MotorMasks.MOTOR_B) > 0 ? true : false; + motorC = (message[0] & MotorMasks.MOTOR_C) > 0 ? true : false; + direction = (message[0] & MotorMasks.DIRECTION) > 0 ? BasicMotorPort.FORWARD : BasicMotorPort.BACKWARD; + + if(motorA){ + // Set motor A to run at specified speed. + Motor.A.setSpeed(message[1] * Battery.getVoltage()); + if(direction == BasicMotorPort.FORWARD) + Motor.A.forward(); + else if(direction == BasicMotorPort.BACKWARD) + Motor.A.backward(); + } + + if(motorB){ + // Set motor B to run at specified speed. + Motor.B.setSpeed(message[1] * Battery.getVoltage()); + if(direction == BasicMotorPort.FORWARD) + Motor.B.forward(); + else if(direction == BasicMotorPort.BACKWARD) + Motor.B.backward(); + } + + if(motorC){ + // Set motor C to run at specified speed. + Motor.C.setSpeed(message[1] * Battery.getVoltage()); + if(direction == BasicMotorPort.FORWARD) + Motor.C.forward(); + else if(direction == BasicMotorPort.BACKWARD) + Motor.C.backward(); + } + + if(recenterMotorB){ + System.out.println("RECENTER"); + // Return motor B to it's origin. + Motor.B.setSpeed(50 * Battery.getVoltage()); + tacho = Motor.B.getTachoCount() % 360; + rotation = -(tacho); + Motor.B.rotate(rotation, false); + } + + }catch(IOException io){ + // On disconnection terminate. + done = true; + } + } + } +} diff --git a/src/ve/ucv/ciens/ccg/nxtarbot/threads/SensorReportThread.java b/src/ve/ucv/ciens/ccg/nxtarbot/threads/SensorReportThread.java new file mode 100644 index 0000000..0920e2b --- /dev/null +++ b/src/ve/ucv/ciens/ccg/nxtarbot/threads/SensorReportThread.java @@ -0,0 +1,75 @@ +/* + * Copyright (C) 2014 Miguel Angel Astor Romero + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package ve.ucv.ciens.ccg.nxtarbot.threads; + +import java.io.DataOutputStream; +import java.io.IOException; + +import lejos.nxt.LightSensor; + +/** + *Class to report the values read from the different sensors through a + * Bluetooth stream.
+ */ +public class SensorReportThread extends Thread{ + private boolean done; + private LightSensor lightSensor; + private DataOutputStream outputStream; + + /** + *Create a new SensorReportThread.
+ * + * @param outputStream The stream to send the data to. + * @param lightSensor An initialized and calibrated light sensor. + */ + public SensorReportThread(DataOutputStream outputStream, LightSensor lightSensor) throws NullPointerException{ + if(outputStream == null) + throw new NullPointerException("Output stream is null."); + + if(lightSensor == null) + throw new NullPointerException("Sensor is null."); + + this.lightSensor = lightSensor; + done = false; + this.outputStream = outputStream; + } + + /** + *Marks this thread as ready to finish cleanly.
+ */ + public void finish(){ + done = true; + } + + /** + *Writes the values read from the light sensor to the output stream.
+ */ + @Override + public void run(){ + byte message = 0; + + while(!done){ + try{ + message = (byte)lightSensor.getLightValue(); + outputStream.writeByte(message); + outputStream.flush(); + }catch(IOException io){ + // On disconnection terminate. + done = true; + } + } + } +}