diff --git a/src/ve/ucv/ciens/ccg/nxtcam/network/LCPThread.java b/src/ve/ucv/ciens/ccg/nxtcam/network/LCPThread.java index e96ebc9..bfcaf56 100644 --- a/src/ve/ucv/ciens/ccg/nxtcam/network/LCPThread.java +++ b/src/ve/ucv/ciens/ccg/nxtcam/network/LCPThread.java @@ -19,7 +19,7 @@ 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.network.protocols.MotorMasks; import ve.ucv.ciens.ccg.nxtcam.robotcontrol.MotorEventQueue; import ve.ucv.ciens.ccg.nxtcam.utils.Logger; @@ -47,6 +47,7 @@ public class LCPThread extends Thread{ public void run(){ long then, now, delta; MotorEvent event; + byte[] msg = new byte[2]; sensorReport.start(); motorControl.start(); @@ -75,11 +76,17 @@ public class LCPThread extends Thread{ event = queue.getNextEvent(); try{ - btComm.writeMessage( - LegoCommunicationProtocol.setOutputState( - event.getMotor() == motor_t.MOTOR_A ? LegoCommunicationProtocol.PORT_0 : (event.getMotor() == motor_t.MOTOR_B ? LegoCommunicationProtocol.PORT_1 : LegoCommunicationProtocol.PORT_2), - event.getPower()) - ); + // Set the motor bit. + msg[0] = (event.getMotor() == motor_t.MOTOR_A) ? MotorMasks.MOTOR_A: ((event.getMotor() == motor_t.MOTOR_B) ? MotorMasks.MOTOR_B: MotorMasks.MOTOR_C); + // Set the direction bit. + if(event.getPower() > 0) msg[0] |= MotorMasks.DIRECTION; + // TODO: Set the recenter bits. + + // Set the power byte. + msg[1] = (byte)Math.abs(event.getPower()); + + // Send the message. + btComm.writeMessage(msg); Logger.log_i(TAG, CLASS_NAME + ".run() :: Message sent to the robot."); try{ sleep(40); }catch(InterruptedException ie){ } @@ -89,7 +96,7 @@ public class LCPThread extends Thread{ } if(reportSensors){ - + } }else{ Logger.log_e(TAG, CLASS_NAME + ".run() :: The robot disconnected or was never available."); diff --git a/src/ve/ucv/ciens/ccg/nxtcam/network/protocols/MotorMasks.java b/src/ve/ucv/ciens/ccg/nxtcam/network/protocols/MotorMasks.java new file mode 100644 index 0000000..1c0bd61 --- /dev/null +++ b/src/ve/ucv/ciens/ccg/nxtcam/network/protocols/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.nxtcam.network.protocols; + +/** + *

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; +}