From 0ebcbb3ce3841dc16e8220c0f21806637c11d0a0 Mon Sep 17 00:00:00 2001 From: unknown Date: Tue, 1 Apr 2014 15:43:59 -0430 Subject: [PATCH] Added light sensor calibration and motor control. --- src/ve/ucv/ciens/ccg/nxtarbot/NxtAR_bot.java | 75 +++++++++++---- .../MotorMasks.java} | 32 ++----- .../nxtarbot/threads/MotorControlThread.java | 92 +++++++++++++++++++ ...{CommSend.java => SensorReportThread.java} | 29 +++--- 4 files changed, 174 insertions(+), 54 deletions(-) rename src/ve/ucv/ciens/ccg/nxtarbot/{threads/CommRecv.java => protocol/MotorMasks.java} (54%) create mode 100644 src/ve/ucv/ciens/ccg/nxtarbot/threads/MotorControlThread.java rename src/ve/ucv/ciens/ccg/nxtarbot/threads/{CommSend.java => SensorReportThread.java} (66%) diff --git a/src/ve/ucv/ciens/ccg/nxtarbot/NxtAR_bot.java b/src/ve/ucv/ciens/ccg/nxtarbot/NxtAR_bot.java index 6568c2a..834c601 100644 --- a/src/ve/ucv/ciens/ccg/nxtarbot/NxtAR_bot.java +++ b/src/ve/ucv/ciens/ccg/nxtarbot/NxtAR_bot.java @@ -19,19 +19,58 @@ import java.io.DataInputStream; import java.io.DataOutputStream; import java.io.IOException; +import lejos.nxt.Button; +import lejos.nxt.ButtonListener; +import lejos.nxt.LightSensor; +import lejos.nxt.Motor; +import lejos.nxt.SensorPort; import lejos.nxt.comm.Bluetooth; import lejos.nxt.comm.NXTConnection; -import ve.ucv.ciens.ccg.nxtarbot.threads.CommRecv; -import ve.ucv.ciens.ccg.nxtarbot.threads.CommSend; +import ve.ucv.ciens.ccg.nxtarbot.threads.MotorControlThread; +import ve.ucv.ciens.ccg.nxtarbot.threads.SensorReportThread; public class NxtAR_bot{ private static DataOutputStream dataOutputStream; private static DataInputStream dataInputStream; private static NXTConnection bluetoothConnection; - private static CommRecv recv; - private static CommSend send; + private static MotorControlThread recvThread; + private static SensorReportThread sendThread; + + 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(); + } + } public static void main(String[] args){ + Motor.A.resetTachoCount(); + Motor.B.resetTachoCount(); + Motor.C.resetTachoCount(); + + LightSensor lightSensor = new LightSensor(SensorPort.S1); + lightSensor.setFloodlight(false); + + System.out.println("Point at dark and press ENTER"); + Button.ENTER.waitForPress(); + lightSensor.calibrateLow(); + System.out.println("--/--"); + + System.out.println("Point at light and press ENTER"); + Button.ENTER.waitForPress(); + lightSensor.calibrateHigh(); + System.out.println("--/--"); + + Button.ESCAPE.addButtonListener(new QuitButtonListener()); + bluetoothConnection = Bluetooth.waitForConnection(); bluetoothConnection.setIOMode(NXTConnection.RAW); dataOutputStream = bluetoothConnection.openDataOutputStream(); @@ -39,23 +78,27 @@ public class NxtAR_bot{ System.out.println("Connected"); - send = new CommSend(dataOutputStream); - recv = new CommRecv(dataInputStream); + sendThread = new SensorReportThread(dataOutputStream, lightSensor); + recvThread = new MotorControlThread(dataInputStream); - recv.start(); - send.start(); + recvThread.start(); + sendThread.start(); try{ - recv.join(); - send.join(); + recvThread.join(); + sendThread.join(); }catch(InterruptedException i){ } - try{ - dataOutputStream.close(); - dataInputStream.close(); - }catch(IOException io){ - System.out.println(io.getMessage()); + quit(); + } + + private static class QuitButtonListener implements ButtonListener{ + @Override + public void buttonPressed(Button b) { + quit(); } - bluetoothConnection.close(); + + @Override + public void buttonReleased(Button b){ } } } diff --git a/src/ve/ucv/ciens/ccg/nxtarbot/threads/CommRecv.java b/src/ve/ucv/ciens/ccg/nxtarbot/protocol/MotorMasks.java similarity index 54% rename from src/ve/ucv/ciens/ccg/nxtarbot/threads/CommRecv.java rename to src/ve/ucv/ciens/ccg/nxtarbot/protocol/MotorMasks.java index 7813df5..61c0b33 100644 --- a/src/ve/ucv/ciens/ccg/nxtarbot/threads/CommRecv.java +++ b/src/ve/ucv/ciens/ccg/nxtarbot/protocol/MotorMasks.java @@ -13,30 +13,12 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -package ve.ucv.ciens.ccg.nxtarbot.threads; +package ve.ucv.ciens.ccg.nxtarbot.protocol; -import java.io.DataInputStream; -import java.io.IOException; - -public class CommRecv extends Thread { - private boolean done; - private byte msg; - private DataInputStream iStream; - - public CommRecv(DataInputStream iStream){ - done = false; - this.iStream = iStream; - } - - @Override - public void run(){ - while(!done){ - try{ - msg = iStream.readByte(); - System.out.println("Byte: " + Byte.toString(msg)); - }catch(IOException io){ - done = true; - } - } - } +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)0xF0; } 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..c732522 --- /dev/null +++ b/src/ve/ucv/ciens/ccg/nxtarbot/threads/MotorControlThread.java @@ -0,0 +1,92 @@ +/* + * 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; + +public class MotorControlThread extends Thread{ + private boolean done; + private DataInputStream inputStream; + + public MotorControlThread(DataInputStream inputStream){ + done = false; + this.inputStream = inputStream; + } + + public void finish(){ + done = true; + } + + @Override + public void run(){ + boolean motorA, motorB, motorC, recenterMotorB; + int direction, rotation, tacho; + byte[] message = new byte[2]; + + while(!done){ + try{ + message[0] = inputStream.readByte(); + message[1] = inputStream.readByte(); + + 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){ + Motor.A.setSpeed(message[1] * Battery.getVoltage()); + if(direction == BasicMotorPort.FORWARD) + Motor.A.forward(); + else if(direction == BasicMotorPort.BACKWARD) + Motor.A.backward(); + } + + if(motorB){ + Motor.B.setSpeed(message[1] * Battery.getVoltage()); + if(direction == BasicMotorPort.FORWARD) + Motor.B.forward(); + else if(direction == BasicMotorPort.BACKWARD) + Motor.B.backward(); + } + + if(motorC){ + Motor.C.setSpeed(message[1] * Battery.getVoltage()); + if(direction == BasicMotorPort.FORWARD) + Motor.C.forward(); + else if(direction == BasicMotorPort.BACKWARD) + Motor.C.backward(); + } + + if(recenterMotorB){ + tacho = Motor.B.getTachoCount() % 360; + rotation = tacho > 180 ? 360 - tacho : -(tacho); + Motor.B.rotate(rotation); + Motor.B.resetTachoCount(); + } + + }catch(IOException io){ + done = true; + } + } + } +} diff --git a/src/ve/ucv/ciens/ccg/nxtarbot/threads/CommSend.java b/src/ve/ucv/ciens/ccg/nxtarbot/threads/SensorReportThread.java similarity index 66% rename from src/ve/ucv/ciens/ccg/nxtarbot/threads/CommSend.java rename to src/ve/ucv/ciens/ccg/nxtarbot/threads/SensorReportThread.java index 779ab07..c3e48c8 100644 --- a/src/ve/ucv/ciens/ccg/nxtarbot/threads/CommSend.java +++ b/src/ve/ucv/ciens/ccg/nxtarbot/threads/SensorReportThread.java @@ -19,29 +19,32 @@ import java.io.DataOutputStream; import java.io.IOException; import lejos.nxt.LightSensor; -import lejos.nxt.SensorPort; -public class CommSend extends Thread { +public class SensorReportThread extends Thread{ private boolean done; - private byte msg; - private LightSensor light; - private DataOutputStream oStream; + private LightSensor lightSensor; + private DataOutputStream outputStream; - public CommSend(DataOutputStream oStream){ - light = new LightSensor(SensorPort.S1); + public SensorReportThread(DataOutputStream outputStream, LightSensor lightSensor){ + this.lightSensor = lightSensor; done = false; - msg = 0; - this.oStream = oStream; + this.outputStream = outputStream; + } + + public void finish(){ + done = true; } @Override public void run(){ + byte message = 0; + while(!done){ try{ - if((msg = (byte)light.getLightValue()) < 40){ - oStream.writeByte(msg); - oStream.flush(); - } + message = (byte)lightSensor.getLightValue(); + outputStream.writeByte(message); + outputStream.flush(); + }catch(IOException io){ done = true; }