From 89c744181e54ae0683b4a072b2e3fed55704ce94 Mon Sep 17 00:00:00 2001 From: unknown Date: Mon, 10 Feb 2014 16:36:32 -0430 Subject: [PATCH] LCPThread sends BT messages. --- src/ve/ucv/ciens/ccg/nxtcam/CamActivity.java | 16 +++--- .../ciens/ccg/nxtcam/network/LCPThread.java | 56 ++++++++++++++++--- .../nxtcam/network/MotorControlThread.java | 4 ++ .../protocols/LegoCommunicationProtocol.java | 17 +++++- 4 files changed, 76 insertions(+), 17 deletions(-) diff --git a/src/ve/ucv/ciens/ccg/nxtcam/CamActivity.java b/src/ve/ucv/ciens/ccg/nxtcam/CamActivity.java index 6d28e8a..58404ea 100644 --- a/src/ve/ucv/ciens/ccg/nxtcam/CamActivity.java +++ b/src/ve/ucv/ciens/ccg/nxtcam/CamActivity.java @@ -16,9 +16,8 @@ package ve.ucv.ciens.ccg.nxtcam; import ve.ucv.ciens.ccg.nxtcam.camera.CameraPreview; -import ve.ucv.ciens.ccg.nxtcam.network.MotorControlThread; -import ve.ucv.ciens.ccg.nxtcam.network.VideoStreamingThread; import ve.ucv.ciens.ccg.nxtcam.network.LCPThread; +import ve.ucv.ciens.ccg.nxtcam.network.VideoStreamingThread; import ve.ucv.ciens.ccg.nxtcam.utils.Logger; import ve.ucv.ciens.ccg.nxtcam.utils.ProjectConstants; import android.app.Activity; @@ -42,7 +41,7 @@ public class CamActivity extends Activity{ private CameraSetupTask camSetupTask; private VideoStreamingThread imThread; private LCPThread botThread; - private MotorControlThread motorThread; + //private MotorControlThread motorThread; private String serverIp; /******************* @@ -59,8 +58,11 @@ public class CamActivity extends Activity{ imThread = new VideoStreamingThread(serverIp); imThread.start(); - motorThread = new MotorControlThread(serverIp); - motorThread.start(); + botThread = new LCPThread(serverIp); + botThread.start(); + + /*motorThread = new MotorControlThread(serverIp); + motorThread.start();*/ } @Override @@ -116,8 +118,8 @@ public class CamActivity extends Activity{ imThread.finish(); imThread = null; - motorThread.finish(); - motorThread = null; + botThread.finish(); + botThread = null; } @Override diff --git a/src/ve/ucv/ciens/ccg/nxtcam/network/LCPThread.java b/src/ve/ucv/ciens/ccg/nxtcam/network/LCPThread.java index 36aebcf..887f146 100644 --- a/src/ve/ucv/ciens/ccg/nxtcam/network/LCPThread.java +++ b/src/ve/ucv/ciens/ccg/nxtcam/network/LCPThread.java @@ -15,6 +15,12 @@ */ package ve.ucv.ciens.ccg.nxtcam.network; +import java.io.IOException; + +import ve.ucv.ciens.ccg.networkdata.MotorEvent; +import ve.ucv.ciens.ccg.networkdata.MotorEvent.motor_t; +import ve.ucv.ciens.ccg.nxtcam.network.protocols.LegoCommunicationProtocol; +import ve.ucv.ciens.ccg.nxtcam.robotcontrol.MotorEventQueue; import ve.ucv.ciens.ccg.nxtcam.utils.Logger; public class LCPThread extends Thread{ @@ -27,29 +33,63 @@ public class LCPThread extends Thread{ private MotorControlThread motorControl; private SensorReportThread sensorReport; + private MotorEventQueue queue; + public LCPThread(String serverIp){ super("Robot Control Main Thread"); btComm = BTCommunicator.getInstance(); done = false; motorControl = new MotorControlThread(serverIp); sensorReport = new SensorReportThread(serverIp); + queue = MotorEventQueue.getInstance(); } public void run(){ - if(!motorControl.connectToServer()){ - Logger.log_e(TAG, CLASS_NAME + ".run() :: Thread motorControl could not connect to the server."); - return; + long then, now, delta; + MotorEvent event; + + //sensorReport.start(); + motorControl.start(); + + then = System.currentTimeMillis(); + + while(!motorControl.isConnected()){ + now = System.currentTimeMillis(); + delta = now - then; + if(delta > 5000L){ + Logger.log_e(TAG, CLASS_NAME + ".run() :: Thread motorControl could not connect to the server."); + return; + } } - if(!(reportSensors = sensorReport.connectToServer())){ + /*if(!(reportSensors = sensorReport.connectToServer())){ Logger.log_e(TAG, CLASS_NAME + ".run() :: Thread sensorReport could not connect to the server."); Logger.log_e(TAG, CLASS_NAME + ".run() :: Sensor data will not be reported to server app."); - } + }*/ while(!done){ if(btComm.isBTEnabled() && btComm.isConnected()){ - Logger.log_d(TAG, CLASS_NAME + ".run() :: Connected."); - if(reportSensors) - Logger.log_d(TAG, CLASS_NAME + ".run() :: Sensor data can be reported."); + //Logger.log_d(TAG, CLASS_NAME + ".run() :: Connected."); + + event = queue.getNextEvent(); + + try{ + btComm.writeMessage( + LegoCommunicationProtocol.setOutputState( + event.getMotor() == motor_t.MOTOR_A ? LegoCommunicationProtocol.PORT_0 : LegoCommunicationProtocol.PORT_2, + event.getPower()) + ); + Logger.log_i(TAG, CLASS_NAME + ".run() :: Message sent to the robot."); + + }catch(IOException io){ + Logger.log_e(TAG, CLASS_NAME + ".run() :: IOException sending message to the robot."); + } + + /*if(reportSensors){ + //Logger.log_d(TAG, CLASS_NAME + ".run() :: Sensor data can be reported."); + }*/ + }else{ + Logger.log_e(TAG, CLASS_NAME + ".run() :: The robot disconnected or was never available."); + break; } } } diff --git a/src/ve/ucv/ciens/ccg/nxtcam/network/MotorControlThread.java b/src/ve/ucv/ciens/ccg/nxtcam/network/MotorControlThread.java index 12dcafb..9ed7cec 100644 --- a/src/ve/ucv/ciens/ccg/nxtcam/network/MotorControlThread.java +++ b/src/ve/ucv/ciens/ccg/nxtcam/network/MotorControlThread.java @@ -131,4 +131,8 @@ public class MotorControlThread extends Thread { return null; } } + + public boolean isConnected(){ + return connected; + } } diff --git a/src/ve/ucv/ciens/ccg/nxtcam/network/protocols/LegoCommunicationProtocol.java b/src/ve/ucv/ciens/ccg/nxtcam/network/protocols/LegoCommunicationProtocol.java index 404c63a..3f6b56e 100644 --- a/src/ve/ucv/ciens/ccg/nxtcam/network/protocols/LegoCommunicationProtocol.java +++ b/src/ve/ucv/ciens/ccg/nxtcam/network/protocols/LegoCommunicationProtocol.java @@ -16,6 +16,9 @@ package ve.ucv.ciens.ccg.nxtcam.network.protocols; import java.security.InvalidParameterException; +import java.util.Arrays; + +import ve.ucv.ciens.ccg.nxtcam.utils.Logger; public abstract class LegoCommunicationProtocol{ /** @@ -148,9 +151,9 @@ public abstract class LegoCommunicationProtocol{ if(turn_ratio < -100 || turn_ratio > 100){ throw new InvalidParameterException("Turn ratio out of range."); } - if(mode_byte != MOTORON && mode_byte != BRAKE && mode_byte != REGULATED){ + /*if(mode_byte != MOTORON && mode_byte != BRAKE && mode_byte != REGULATED){ throw new InvalidParameterException("Invalid mode byte."); - } + }*/ if(regulation_mode != REGULATION_MODE_IDLE && regulation_mode != REGULATION_MODE_MOTOR_SPEED && regulation_mode != REGULATION_MODE_MOTOR_SYNC){ throw new InvalidParameterException("Invalid regulation mode."); } @@ -170,9 +173,19 @@ public abstract class LegoCommunicationProtocol{ message[9] = run_state; message[10] = message[11] = message[12] = message[13] = message[14] = 0x00; + Logger.log_d("LCP", LegoCommunicationProtocol.class.getSimpleName() + "setOutputState(...) :: " + Arrays.toString(message)); + return message; } + public static byte[] setOutputState(byte output_port, byte power){ + if(power == (byte)0){ + return setOutputState(output_port, power, (byte)0x00, REGULATION_MODE_IDLE, (byte)0x00, MOTOR_RUN_STATE_IDLE); + }else{ + return setOutputState(output_port, power, (byte)(MOTORON + BRAKE), REGULATION_MODE_MOTOR_SPEED, (byte)0x00, MOTOR_RUN_STATE_RUNNING); + } + } + /** * Motor configuration request pdu. Page 8 of appendix 2. *