From 5c3ac9b71334cd0e5f2f1d82137aaea85d8219a9 Mon Sep 17 00:00:00 2001 From: grog Date: Fri, 24 Nov 2023 08:48:29 -0800 Subject: [PATCH] Filter out warning when connecting --- src/main/java/org/myrobotlab/arduino/Msg.java | 2672 +++++++++-------- .../myrobotlab/arduino/virtual/MrlComm.java | 5 + .../java/org/myrobotlab/service/Arduino.java | 18 +- .../service/interfaces/MrlCommPublisher.java | 4 +- .../resource/Arduino/MrlComm/MrlNeopixel.cpp | 2 + .../Arduino/generate/Msg.java.template | 11 +- .../myrobotlab/arduino/MrlCommDirectTest.java | 6 + .../service/VirtualArduinoTest.java | 6 + 8 files changed, 1386 insertions(+), 1338 deletions(-) diff --git a/src/main/java/org/myrobotlab/arduino/Msg.java b/src/main/java/org/myrobotlab/arduino/Msg.java index e828124ab5..c850ec1d95 100644 --- a/src/main/java/org/myrobotlab/arduino/Msg.java +++ b/src/main/java/org/myrobotlab/arduino/Msg.java @@ -4,6 +4,7 @@ import java.io.ByteArrayOutputStream; import java.io.OutputStream; import java.io.IOException; +import org.myrobotlab.service.Serial; import java.nio.ByteBuffer; import java.nio.ByteOrder; import java.util.Arrays; @@ -11,6 +12,7 @@ import org.myrobotlab.logging.Level; +import org.myrobotlab.arduino.virtual.MrlComm; import org.myrobotlab.string.StringUtil; /** @@ -44,11 +46,12 @@ b16 int int (short) 2 bytes -32,768 to 32,767 import org.myrobotlab.logging.LoggerFactory; import org.myrobotlab.logging.LoggingFactory; +import org.myrobotlab.service.VirtualArduino; import java.io.FileOutputStream; +import java.util.Arrays; import org.myrobotlab.service.interfaces.MrlCommPublisher; import org.myrobotlab.service.Runtime; -import org.myrobotlab.service.Serial; import org.myrobotlab.service.Servo; import org.myrobotlab.service.interfaces.SerialDevice; import org.slf4j.Logger; @@ -97,154 +100,154 @@ public static class AckLock { transient StringBuilder rxBuffer = new StringBuilder(); transient StringBuilder txBuffer = new StringBuilder(); - public static final int DEVICE_TYPE_UNKNOWN = 0; - public static final int DEVICE_TYPE_ARDUINO = 1; - public static final int DEVICE_TYPE_ULTRASONICSENSOR = 2; - public static final int DEVICE_TYPE_STEPPER = 3; - public static final int DEVICE_TYPE_MOTOR = 4; - public static final int DEVICE_TYPE_SERVO = 5; - public static final int DEVICE_TYPE_SERIAL = 6; - public static final int DEVICE_TYPE_I2C = 7; - public static final int DEVICE_TYPE_NEOPIXEL = 8; - public static final int DEVICE_TYPE_ENCODER = 9; + public static final int DEVICE_TYPE_UNKNOWN = 0; + public static final int DEVICE_TYPE_ARDUINO = 1; + public static final int DEVICE_TYPE_ULTRASONICSENSOR = 2; + public static final int DEVICE_TYPE_STEPPER = 3; + public static final int DEVICE_TYPE_MOTOR = 4; + public static final int DEVICE_TYPE_SERVO = 5; + public static final int DEVICE_TYPE_SERIAL = 6; + public static final int DEVICE_TYPE_I2C = 7; + public static final int DEVICE_TYPE_NEOPIXEL = 8; + public static final int DEVICE_TYPE_ENCODER = 9; - // < publishMRLCommError/str errorMsg - public final static int PUBLISH_MRLCOMM_ERROR = 1; - // > getBoardInfo - public final static int GET_BOARD_INFO = 2; - // < publishBoardInfo/version/boardType/b16 microsPerLoop/b16 sram/activePins/[] deviceSummary - public final static int PUBLISH_BOARD_INFO = 3; - // > enablePin/address/type/b16 rate - public final static int ENABLE_PIN = 4; - // > setDebug/bool enabled - public final static int SET_DEBUG = 5; - // > setSerialRate/b32 rate - public final static int SET_SERIAL_RATE = 6; - // > softReset - public final static int SOFT_RESET = 7; - // > enableAck/bool enabled - public final static int ENABLE_ACK = 8; - // < publishAck/function - public final static int PUBLISH_ACK = 9; - // > echo/f32 myFloat/myByte/f32 secondFloat - public final static int ECHO = 10; - // < publishEcho/f32 myFloat/myByte/f32 secondFloat - public final static int PUBLISH_ECHO = 11; - // > customMsg/[] msg - public final static int CUSTOM_MSG = 12; - // < publishCustomMsg/[] msg - public final static int PUBLISH_CUSTOM_MSG = 13; - // > deviceDetach/deviceId - public final static int DEVICE_DETACH = 14; - // > i2cBusAttach/deviceId/i2cBus - public final static int I2C_BUS_ATTACH = 15; - // > i2cRead/deviceId/deviceAddress/size - public final static int I2C_READ = 16; - // > i2cWrite/deviceId/deviceAddress/[] data - public final static int I2C_WRITE = 17; - // > i2cWriteRead/deviceId/deviceAddress/readSize/writeValue - public final static int I2C_WRITE_READ = 18; - // < publishI2cData/deviceId/[] data - public final static int PUBLISH_I2C_DATA = 19; - // > neoPixelAttach/deviceId/pin/b16 numPixels/depth - public final static int NEO_PIXEL_ATTACH = 20; - // > neoPixelSetAnimation/deviceId/animation/red/green/blue/white/b32 wait_ms - public final static int NEO_PIXEL_SET_ANIMATION = 21; - // > neoPixelWriteMatrix/deviceId/[] buffer - public final static int NEO_PIXEL_WRITE_MATRIX = 22; - // > neoPixelFill/deviceId/b16 address/b16 count/red/green/blue/white - public final static int NEO_PIXEL_FILL = 23; - // > neoPixelSetBrightness/deviceId/brightness - public final static int NEO_PIXEL_SET_BRIGHTNESS = 24; - // > neoPixelClear/deviceId - public final static int NEO_PIXEL_CLEAR = 25; - // > analogWrite/pin/value - public final static int ANALOG_WRITE = 26; - // > digitalWrite/pin/value - public final static int DIGITAL_WRITE = 27; - // > disablePin/pin - public final static int DISABLE_PIN = 28; - // > disablePins - public final static int DISABLE_PINS = 29; - // > pinMode/pin/mode - public final static int PIN_MODE = 30; - // < publishDebug/str debugMsg - public final static int PUBLISH_DEBUG = 31; - // < publishPinArray/[] data - public final static int PUBLISH_PIN_ARRAY = 32; - // > setTrigger/pin/triggerValue - public final static int SET_TRIGGER = 33; - // > setDebounce/pin/delay - public final static int SET_DEBOUNCE = 34; - // > servoAttach/deviceId/pin/b16 initPos/b16 initVelocity/str name - public final static int SERVO_ATTACH = 35; - // > servoAttachPin/deviceId/pin - public final static int SERVO_ATTACH_PIN = 36; - // > servoDetachPin/deviceId - public final static int SERVO_DETACH_PIN = 37; - // > servoSetVelocity/deviceId/b16 velocity - public final static int SERVO_SET_VELOCITY = 38; - // > servoSweepStart/deviceId/min/max/step - public final static int SERVO_SWEEP_START = 39; - // > servoSweepStop/deviceId - public final static int SERVO_SWEEP_STOP = 40; - // > servoMoveToMicroseconds/deviceId/b16 target - public final static int SERVO_MOVE_TO_MICROSECONDS = 41; - // > servoSetAcceleration/deviceId/b16 acceleration - public final static int SERVO_SET_ACCELERATION = 42; - // < publishServoEvent/deviceId/eventType/b16 currentPos/b16 targetPos - public final static int PUBLISH_SERVO_EVENT = 43; - // > serialAttach/deviceId/relayPin - public final static int SERIAL_ATTACH = 44; - // > serialRelay/deviceId/[] data - public final static int SERIAL_RELAY = 45; - // < publishSerialData/deviceId/[] data - public final static int PUBLISH_SERIAL_DATA = 46; - // > ultrasonicSensorAttach/deviceId/triggerPin/echoPin - public final static int ULTRASONIC_SENSOR_ATTACH = 47; - // > ultrasonicSensorStartRanging/deviceId - public final static int ULTRASONIC_SENSOR_START_RANGING = 48; - // > ultrasonicSensorStopRanging/deviceId - public final static int ULTRASONIC_SENSOR_STOP_RANGING = 49; - // < publishUltrasonicSensorData/deviceId/b16 echoTime - public final static int PUBLISH_ULTRASONIC_SENSOR_DATA = 50; - // > setAref/b16 type - public final static int SET_AREF = 51; - // > motorAttach/deviceId/type/[] pins - public final static int MOTOR_ATTACH = 52; - // > motorMove/deviceId/pwr - public final static int MOTOR_MOVE = 53; - // > motorMoveTo/deviceId/pos - public final static int MOTOR_MOVE_TO = 54; - // > encoderAttach/deviceId/type/pin - public final static int ENCODER_ATTACH = 55; - // > setZeroPoint/deviceId - public final static int SET_ZERO_POINT = 56; - // < publishEncoderData/deviceId/b16 position - public final static int PUBLISH_ENCODER_DATA = 57; - // < publishMrlCommBegin/version - public final static int PUBLISH_MRL_COMM_BEGIN = 58; - // > servoStop/deviceId - public final static int SERVO_STOP = 59; + // < publishMRLCommError/str errorMsg + public final static int PUBLISH_MRLCOMM_ERROR = 1; + // > getBoardInfo + public final static int GET_BOARD_INFO = 2; + // < publishBoardInfo/version/boardType/b16 microsPerLoop/b16 sram/activePins/[] deviceSummary + public final static int PUBLISH_BOARD_INFO = 3; + // > enablePin/address/type/b16 rate + public final static int ENABLE_PIN = 4; + // > setDebug/bool enabled + public final static int SET_DEBUG = 5; + // > setSerialRate/b32 rate + public final static int SET_SERIAL_RATE = 6; + // > softReset + public final static int SOFT_RESET = 7; + // > enableAck/bool enabled + public final static int ENABLE_ACK = 8; + // < publishAck/function + public final static int PUBLISH_ACK = 9; + // > echo/f32 myFloat/myByte/f32 secondFloat + public final static int ECHO = 10; + // < publishEcho/f32 myFloat/myByte/f32 secondFloat + public final static int PUBLISH_ECHO = 11; + // > customMsg/[] msg + public final static int CUSTOM_MSG = 12; + // < publishCustomMsg/[] msg + public final static int PUBLISH_CUSTOM_MSG = 13; + // > deviceDetach/deviceId + public final static int DEVICE_DETACH = 14; + // > i2cBusAttach/deviceId/i2cBus + public final static int I2C_BUS_ATTACH = 15; + // > i2cRead/deviceId/deviceAddress/size + public final static int I2C_READ = 16; + // > i2cWrite/deviceId/deviceAddress/[] data + public final static int I2C_WRITE = 17; + // > i2cWriteRead/deviceId/deviceAddress/readSize/writeValue + public final static int I2C_WRITE_READ = 18; + // < publishI2cData/deviceId/[] data + public final static int PUBLISH_I2C_DATA = 19; + // > neoPixelAttach/deviceId/pin/b16 numPixels/depth + public final static int NEO_PIXEL_ATTACH = 20; + // > neoPixelSetAnimation/deviceId/animation/red/green/blue/white/b32 wait_ms + public final static int NEO_PIXEL_SET_ANIMATION = 21; + // > neoPixelWriteMatrix/deviceId/[] buffer + public final static int NEO_PIXEL_WRITE_MATRIX = 22; + // > neoPixelFill/deviceId/b16 address/b16 count/red/green/blue/white + public final static int NEO_PIXEL_FILL = 23; + // > neoPixelSetBrightness/deviceId/brightness + public final static int NEO_PIXEL_SET_BRIGHTNESS = 24; + // > neoPixelClear/deviceId + public final static int NEO_PIXEL_CLEAR = 25; + // > analogWrite/pin/value + public final static int ANALOG_WRITE = 26; + // > digitalWrite/pin/value + public final static int DIGITAL_WRITE = 27; + // > disablePin/pin + public final static int DISABLE_PIN = 28; + // > disablePins + public final static int DISABLE_PINS = 29; + // > pinMode/pin/mode + public final static int PIN_MODE = 30; + // < publishDebug/str debugMsg + public final static int PUBLISH_DEBUG = 31; + // < publishPinArray/[] data + public final static int PUBLISH_PIN_ARRAY = 32; + // > setTrigger/pin/triggerValue + public final static int SET_TRIGGER = 33; + // > setDebounce/pin/delay + public final static int SET_DEBOUNCE = 34; + // > servoAttach/deviceId/pin/b16 initPos/b16 initVelocity/str name + public final static int SERVO_ATTACH = 35; + // > servoAttachPin/deviceId/pin + public final static int SERVO_ATTACH_PIN = 36; + // > servoDetachPin/deviceId + public final static int SERVO_DETACH_PIN = 37; + // > servoSetVelocity/deviceId/b16 velocity + public final static int SERVO_SET_VELOCITY = 38; + // > servoSweepStart/deviceId/min/max/step + public final static int SERVO_SWEEP_START = 39; + // > servoSweepStop/deviceId + public final static int SERVO_SWEEP_STOP = 40; + // > servoMoveToMicroseconds/deviceId/b16 target + public final static int SERVO_MOVE_TO_MICROSECONDS = 41; + // > servoSetAcceleration/deviceId/b16 acceleration + public final static int SERVO_SET_ACCELERATION = 42; + // < publishServoEvent/deviceId/eventType/b16 currentPos/b16 targetPos + public final static int PUBLISH_SERVO_EVENT = 43; + // > serialAttach/deviceId/relayPin + public final static int SERIAL_ATTACH = 44; + // > serialRelay/deviceId/[] data + public final static int SERIAL_RELAY = 45; + // < publishSerialData/deviceId/[] data + public final static int PUBLISH_SERIAL_DATA = 46; + // > ultrasonicSensorAttach/deviceId/triggerPin/echoPin + public final static int ULTRASONIC_SENSOR_ATTACH = 47; + // > ultrasonicSensorStartRanging/deviceId + public final static int ULTRASONIC_SENSOR_START_RANGING = 48; + // > ultrasonicSensorStopRanging/deviceId + public final static int ULTRASONIC_SENSOR_STOP_RANGING = 49; + // < publishUltrasonicSensorData/deviceId/b16 echoTime + public final static int PUBLISH_ULTRASONIC_SENSOR_DATA = 50; + // > setAref/b16 type + public final static int SET_AREF = 51; + // > motorAttach/deviceId/type/[] pins + public final static int MOTOR_ATTACH = 52; + // > motorMove/deviceId/pwr + public final static int MOTOR_MOVE = 53; + // > motorMoveTo/deviceId/pos + public final static int MOTOR_MOVE_TO = 54; + // > encoderAttach/deviceId/type/pin + public final static int ENCODER_ATTACH = 55; + // > setZeroPoint/deviceId + public final static int SET_ZERO_POINT = 56; + // < publishEncoderData/deviceId/b16 position + public final static int PUBLISH_ENCODER_DATA = 57; + // < publishMrlCommBegin/version + public final static int PUBLISH_MRL_COMM_BEGIN = 58; + // > servoStop/deviceId + public final static int SERVO_STOP = 59; /** * These methods will be invoked from the Msg class as callbacks from MrlComm. */ - // public void publishMRLCommError(String errorMsg/*str*/){} - // public void publishBoardInfo(Integer version/*byte*/, Integer boardType/*byte*/, Integer microsPerLoop/*b16*/, Integer sram/*b16*/, Integer activePins/*byte*/, int[] deviceSummary/*[]*/){} - // public void publishAck(Integer function/*byte*/){} - // public void publishEcho(Float myFloat/*f32*/, Integer myByte/*byte*/, Float secondFloat/*f32*/){} - // public void publishCustomMsg(int[] msg/*[]*/){} - // public void publishI2cData(Integer deviceId/*byte*/, int[] data/*[]*/){} - // public void publishDebug(String debugMsg/*str*/){} - // public void publishPinArray(int[] data/*[]*/){} - // public void publishServoEvent(Integer deviceId/*byte*/, Integer eventType/*byte*/, Integer currentPos/*b16*/, Integer targetPos/*b16*/){} - // public void publishSerialData(Integer deviceId/*byte*/, int[] data/*[]*/){} - // public void publishUltrasonicSensorData(Integer deviceId/*byte*/, Integer echoTime/*b16*/){} - // public void publishEncoderData(Integer deviceId/*byte*/, Integer position/*b16*/){} - // public void publishMrlCommBegin(Integer version/*byte*/){} + // public void publishMRLCommError(String errorMsg/*str*/){} + // public void publishBoardInfo(Integer version/*byte*/, Integer boardType/*byte*/, Integer microsPerLoop/*b16*/, Integer sram/*b16*/, Integer activePins/*byte*/, int[] deviceSummary/*[]*/){} + // public void publishAck(Integer function/*byte*/){} + // public void publishEcho(Float myFloat/*f32*/, Integer myByte/*byte*/, Float secondFloat/*f32*/){} + // public void publishCustomMsg(int[] msg/*[]*/){} + // public void publishI2cData(Integer deviceId/*byte*/, int[] data/*[]*/){} + // public void publishDebug(String debugMsg/*str*/){} + // public void publishPinArray(int[] data/*[]*/){} + // public void publishServoEvent(Integer deviceId/*byte*/, Integer eventType/*byte*/, Integer currentPos/*b16*/, Integer targetPos/*b16*/){} + // public void publishSerialData(Integer deviceId/*byte*/, int[] data/*[]*/){} + // public void publishUltrasonicSensorData(Integer deviceId/*byte*/, Integer echoTime/*b16*/){} + // public void publishEncoderData(Integer deviceId/*byte*/, Integer position/*b16*/){} + // public void publishMrlCommBegin(Integer version/*byte*/){} @@ -291,361 +294,361 @@ public void processCommand(int[] ioCmd) { this.clearToSend = true; } switch (method) { - case PUBLISH_MRLCOMM_ERROR: { - String errorMsg = str(ioCmd, startPos+2, ioCmd[startPos+1]); - startPos += 1 + ioCmd[startPos+1]; - if(invoke){ - arduino.invoke("publishMRLCommError", errorMsg); - } else { - arduino.publishMRLCommError( errorMsg); - } - if(record != null){ - rxBuffer.append("< publishMRLCommError"); - rxBuffer.append("/"); - rxBuffer.append(errorMsg); - rxBuffer.append("\n"); - try{ - record.write(rxBuffer.toString().getBytes()); - rxBuffer.setLength(0); - } catch (IOException e) { - log.warn("failed recording bytes.", e); - } - } - - break; - } - case PUBLISH_BOARD_INFO: { - Integer version = ioCmd[startPos+1]; // bu8 - startPos += 1; - Integer boardType = ioCmd[startPos+1]; // bu8 - startPos += 1; - Integer microsPerLoop = b16(ioCmd, startPos+1); - startPos += 2; //b16 - Integer sram = b16(ioCmd, startPos+1); - startPos += 2; //b16 - Integer activePins = ioCmd[startPos+1]; // bu8 - startPos += 1; - int[] deviceSummary = subArray(ioCmd, startPos+2, ioCmd[startPos+1]); - startPos += 1 + ioCmd[startPos+1]; - if(invoke){ - arduino.invoke("publishBoardInfo", version, boardType, microsPerLoop, sram, activePins, deviceSummary); - } else { - arduino.publishBoardInfo( version, boardType, microsPerLoop, sram, activePins, deviceSummary); - } - if(record != null){ - rxBuffer.append("< publishBoardInfo"); - rxBuffer.append("/"); - rxBuffer.append(version); - rxBuffer.append("/"); - rxBuffer.append(boardType); - rxBuffer.append("/"); - rxBuffer.append(microsPerLoop); - rxBuffer.append("/"); - rxBuffer.append(sram); - rxBuffer.append("/"); - rxBuffer.append(activePins); - rxBuffer.append("/"); - rxBuffer.append(Arrays.toString(deviceSummary)); - rxBuffer.append("\n"); - try{ - record.write(rxBuffer.toString().getBytes()); - rxBuffer.setLength(0); - } catch (IOException e) { - log.warn("failed recording bytes.", e); - } - } - - break; - } - case PUBLISH_ACK: { - Integer function = ioCmd[startPos+1]; // bu8 - startPos += 1; - if(invoke){ - arduino.invoke("publishAck", function); - } else { - arduino.publishAck( function); - } - if(record != null){ - rxBuffer.append("< publishAck"); - rxBuffer.append("/"); - rxBuffer.append(function); - rxBuffer.append("\n"); - try{ - record.write(rxBuffer.toString().getBytes()); - rxBuffer.setLength(0); - } catch (IOException e) { - log.warn("failed recording bytes.", e); - } - } - - break; - } - case PUBLISH_ECHO: { - Float myFloat = f32(ioCmd, startPos+1); - startPos += 4; //f32 - Integer myByte = ioCmd[startPos+1]; // bu8 - startPos += 1; - Float secondFloat = f32(ioCmd, startPos+1); - startPos += 4; //f32 - if(invoke){ - arduino.invoke("publishEcho", myFloat, myByte, secondFloat); - } else { - arduino.publishEcho( myFloat, myByte, secondFloat); - } - if(record != null){ - rxBuffer.append("< publishEcho"); - rxBuffer.append("/"); - rxBuffer.append(myFloat); - rxBuffer.append("/"); - rxBuffer.append(myByte); - rxBuffer.append("/"); - rxBuffer.append(secondFloat); - rxBuffer.append("\n"); - try{ - record.write(rxBuffer.toString().getBytes()); - rxBuffer.setLength(0); - } catch (IOException e) { - log.warn("failed recording bytes.", e); - } - } - - break; - } - case PUBLISH_CUSTOM_MSG: { - int[] msg = subArray(ioCmd, startPos+2, ioCmd[startPos+1]); - startPos += 1 + ioCmd[startPos+1]; - if(invoke){ - arduino.invoke("publishCustomMsg", msg); - } else { - arduino.publishCustomMsg( msg); - } - if(record != null){ - rxBuffer.append("< publishCustomMsg"); - rxBuffer.append("/"); - rxBuffer.append(Arrays.toString(msg)); - rxBuffer.append("\n"); - try{ - record.write(rxBuffer.toString().getBytes()); - rxBuffer.setLength(0); - } catch (IOException e) { - log.warn("failed recording bytes.", e); - } - } - - break; - } - case PUBLISH_I2C_DATA: { - Integer deviceId = ioCmd[startPos+1]; // bu8 - startPos += 1; - int[] data = subArray(ioCmd, startPos+2, ioCmd[startPos+1]); - startPos += 1 + ioCmd[startPos+1]; - if(invoke){ - arduino.invoke("publishI2cData", deviceId, data); - } else { - arduino.publishI2cData( deviceId, data); - } - if(record != null){ - rxBuffer.append("< publishI2cData"); - rxBuffer.append("/"); - rxBuffer.append(deviceId); - rxBuffer.append("/"); - rxBuffer.append(Arrays.toString(data)); - rxBuffer.append("\n"); - try{ - record.write(rxBuffer.toString().getBytes()); - rxBuffer.setLength(0); - } catch (IOException e) { - log.warn("failed recording bytes.", e); - } - } - - break; - } - case PUBLISH_DEBUG: { - String debugMsg = str(ioCmd, startPos+2, ioCmd[startPos+1]); - startPos += 1 + ioCmd[startPos+1]; - if(invoke){ - arduino.invoke("publishDebug", debugMsg); - } else { - arduino.publishDebug( debugMsg); - } - if(record != null){ - rxBuffer.append("< publishDebug"); - rxBuffer.append("/"); - rxBuffer.append(debugMsg); - rxBuffer.append("\n"); - try{ - record.write(rxBuffer.toString().getBytes()); - rxBuffer.setLength(0); - } catch (IOException e) { - log.warn("failed recording bytes.", e); - } - } - - break; - } - case PUBLISH_PIN_ARRAY: { - int[] data = subArray(ioCmd, startPos+2, ioCmd[startPos+1]); - startPos += 1 + ioCmd[startPos+1]; - if(invoke){ - arduino.invoke("publishPinArray", data); - } else { - arduino.publishPinArray( data); - } - if(record != null){ - rxBuffer.append("< publishPinArray"); - rxBuffer.append("/"); - rxBuffer.append(Arrays.toString(data)); - rxBuffer.append("\n"); - try{ - record.write(rxBuffer.toString().getBytes()); - rxBuffer.setLength(0); - } catch (IOException e) { - log.warn("failed recording bytes.", e); - } - } - - break; - } - case PUBLISH_SERVO_EVENT: { - Integer deviceId = ioCmd[startPos+1]; // bu8 - startPos += 1; - Integer eventType = ioCmd[startPos+1]; // bu8 - startPos += 1; - Integer currentPos = b16(ioCmd, startPos+1); - startPos += 2; //b16 - Integer targetPos = b16(ioCmd, startPos+1); - startPos += 2; //b16 - if(invoke){ - arduino.invoke("publishServoEvent", deviceId, eventType, currentPos, targetPos); - } else { - arduino.publishServoEvent( deviceId, eventType, currentPos, targetPos); - } - if(record != null){ - rxBuffer.append("< publishServoEvent"); - rxBuffer.append("/"); - rxBuffer.append(deviceId); - rxBuffer.append("/"); - rxBuffer.append(eventType); - rxBuffer.append("/"); - rxBuffer.append(currentPos); - rxBuffer.append("/"); - rxBuffer.append(targetPos); - rxBuffer.append("\n"); - try{ - record.write(rxBuffer.toString().getBytes()); - rxBuffer.setLength(0); - } catch (IOException e) { - log.warn("failed recording bytes.", e); - } - } - - break; - } - case PUBLISH_SERIAL_DATA: { - Integer deviceId = ioCmd[startPos+1]; // bu8 - startPos += 1; - int[] data = subArray(ioCmd, startPos+2, ioCmd[startPos+1]); - startPos += 1 + ioCmd[startPos+1]; - if(invoke){ - arduino.invoke("publishSerialData", deviceId, data); - } else { - arduino.publishSerialData( deviceId, data); - } - if(record != null){ - rxBuffer.append("< publishSerialData"); - rxBuffer.append("/"); - rxBuffer.append(deviceId); - rxBuffer.append("/"); - rxBuffer.append(Arrays.toString(data)); - rxBuffer.append("\n"); - try{ - record.write(rxBuffer.toString().getBytes()); - rxBuffer.setLength(0); - } catch (IOException e) { - log.warn("failed recording bytes.", e); - } - } - - break; - } - case PUBLISH_ULTRASONIC_SENSOR_DATA: { - Integer deviceId = ioCmd[startPos+1]; // bu8 - startPos += 1; - Integer echoTime = b16(ioCmd, startPos+1); - startPos += 2; //b16 - if(invoke){ - arduino.invoke("publishUltrasonicSensorData", deviceId, echoTime); - } else { - arduino.publishUltrasonicSensorData( deviceId, echoTime); - } - if(record != null){ - rxBuffer.append("< publishUltrasonicSensorData"); - rxBuffer.append("/"); - rxBuffer.append(deviceId); - rxBuffer.append("/"); - rxBuffer.append(echoTime); - rxBuffer.append("\n"); - try{ - record.write(rxBuffer.toString().getBytes()); - rxBuffer.setLength(0); - } catch (IOException e) { - log.warn("failed recording bytes.", e); - } - } - - break; - } - case PUBLISH_ENCODER_DATA: { - Integer deviceId = ioCmd[startPos+1]; // bu8 - startPos += 1; - Integer position = b16(ioCmd, startPos+1); - startPos += 2; //b16 - if(invoke){ - arduino.invoke("publishEncoderData", deviceId, position); - } else { - arduino.publishEncoderData( deviceId, position); - } - if(record != null){ - rxBuffer.append("< publishEncoderData"); - rxBuffer.append("/"); - rxBuffer.append(deviceId); - rxBuffer.append("/"); - rxBuffer.append(position); - rxBuffer.append("\n"); - try{ - record.write(rxBuffer.toString().getBytes()); - rxBuffer.setLength(0); - } catch (IOException e) { - log.warn("failed recording bytes.", e); - } - } - - break; - } - case PUBLISH_MRL_COMM_BEGIN: { - Integer version = ioCmd[startPos+1]; // bu8 - startPos += 1; - if(invoke){ - arduino.invoke("publishMrlCommBegin", version); - } else { - arduino.publishMrlCommBegin( version); - } - if(record != null){ - rxBuffer.append("< publishMrlCommBegin"); - rxBuffer.append("/"); - rxBuffer.append(version); - rxBuffer.append("\n"); - try{ - record.write(rxBuffer.toString().getBytes()); - rxBuffer.setLength(0); - } catch (IOException e) { - log.warn("failed recording bytes.", e); - } - } - - break; - } + case PUBLISH_MRLCOMM_ERROR: { + String errorMsg = str(ioCmd, startPos+2, ioCmd[startPos+1]); + startPos += 1 + ioCmd[startPos+1]; + if(invoke){ + arduino.invoke("publishMRLCommError", errorMsg); + } else { + arduino.publishMRLCommError( errorMsg); + } + if(record != null){ + rxBuffer.append("< publishMRLCommError"); + rxBuffer.append("/"); + rxBuffer.append(errorMsg); + rxBuffer.append("\n"); + try{ + record.write(rxBuffer.toString().getBytes()); + rxBuffer.setLength(0); + } catch (IOException e) { + log.warn("failed recording bytes.", e); + } + } + + break; + } + case PUBLISH_BOARD_INFO: { + Integer version = ioCmd[startPos+1]; // bu8 + startPos += 1; + Integer boardType = ioCmd[startPos+1]; // bu8 + startPos += 1; + Integer microsPerLoop = b16(ioCmd, startPos+1); + startPos += 2; //b16 + Integer sram = b16(ioCmd, startPos+1); + startPos += 2; //b16 + Integer activePins = ioCmd[startPos+1]; // bu8 + startPos += 1; + int[] deviceSummary = subArray(ioCmd, startPos+2, ioCmd[startPos+1]); + startPos += 1 + ioCmd[startPos+1]; + if(invoke){ + arduino.invoke("publishBoardInfo", version, boardType, microsPerLoop, sram, activePins, deviceSummary); + } else { + arduino.publishBoardInfo( version, boardType, microsPerLoop, sram, activePins, deviceSummary); + } + if(record != null){ + rxBuffer.append("< publishBoardInfo"); + rxBuffer.append("/"); + rxBuffer.append(version); + rxBuffer.append("/"); + rxBuffer.append(boardType); + rxBuffer.append("/"); + rxBuffer.append(microsPerLoop); + rxBuffer.append("/"); + rxBuffer.append(sram); + rxBuffer.append("/"); + rxBuffer.append(activePins); + rxBuffer.append("/"); + rxBuffer.append(Arrays.toString(deviceSummary)); + rxBuffer.append("\n"); + try{ + record.write(rxBuffer.toString().getBytes()); + rxBuffer.setLength(0); + } catch (IOException e) { + log.warn("failed recording bytes.", e); + } + } + + break; + } + case PUBLISH_ACK: { + Integer function = ioCmd[startPos+1]; // bu8 + startPos += 1; + if(invoke){ + arduino.invoke("publishAck", function); + } else { + arduino.publishAck( function); + } + if(record != null){ + rxBuffer.append("< publishAck"); + rxBuffer.append("/"); + rxBuffer.append(function); + rxBuffer.append("\n"); + try{ + record.write(rxBuffer.toString().getBytes()); + rxBuffer.setLength(0); + } catch (IOException e) { + log.warn("failed recording bytes.", e); + } + } + + break; + } + case PUBLISH_ECHO: { + Float myFloat = f32(ioCmd, startPos+1); + startPos += 4; //f32 + Integer myByte = ioCmd[startPos+1]; // bu8 + startPos += 1; + Float secondFloat = f32(ioCmd, startPos+1); + startPos += 4; //f32 + if(invoke){ + arduino.invoke("publishEcho", myFloat, myByte, secondFloat); + } else { + arduino.publishEcho( myFloat, myByte, secondFloat); + } + if(record != null){ + rxBuffer.append("< publishEcho"); + rxBuffer.append("/"); + rxBuffer.append(myFloat); + rxBuffer.append("/"); + rxBuffer.append(myByte); + rxBuffer.append("/"); + rxBuffer.append(secondFloat); + rxBuffer.append("\n"); + try{ + record.write(rxBuffer.toString().getBytes()); + rxBuffer.setLength(0); + } catch (IOException e) { + log.warn("failed recording bytes.", e); + } + } + + break; + } + case PUBLISH_CUSTOM_MSG: { + int[] msg = subArray(ioCmd, startPos+2, ioCmd[startPos+1]); + startPos += 1 + ioCmd[startPos+1]; + if(invoke){ + arduino.invoke("publishCustomMsg", msg); + } else { + arduino.publishCustomMsg( msg); + } + if(record != null){ + rxBuffer.append("< publishCustomMsg"); + rxBuffer.append("/"); + rxBuffer.append(Arrays.toString(msg)); + rxBuffer.append("\n"); + try{ + record.write(rxBuffer.toString().getBytes()); + rxBuffer.setLength(0); + } catch (IOException e) { + log.warn("failed recording bytes.", e); + } + } + + break; + } + case PUBLISH_I2C_DATA: { + Integer deviceId = ioCmd[startPos+1]; // bu8 + startPos += 1; + int[] data = subArray(ioCmd, startPos+2, ioCmd[startPos+1]); + startPos += 1 + ioCmd[startPos+1]; + if(invoke){ + arduino.invoke("publishI2cData", deviceId, data); + } else { + arduino.publishI2cData( deviceId, data); + } + if(record != null){ + rxBuffer.append("< publishI2cData"); + rxBuffer.append("/"); + rxBuffer.append(deviceId); + rxBuffer.append("/"); + rxBuffer.append(Arrays.toString(data)); + rxBuffer.append("\n"); + try{ + record.write(rxBuffer.toString().getBytes()); + rxBuffer.setLength(0); + } catch (IOException e) { + log.warn("failed recording bytes.", e); + } + } + + break; + } + case PUBLISH_DEBUG: { + String debugMsg = str(ioCmd, startPos+2, ioCmd[startPos+1]); + startPos += 1 + ioCmd[startPos+1]; + if(invoke){ + arduino.invoke("publishDebug", debugMsg); + } else { + arduino.publishDebug( debugMsg); + } + if(record != null){ + rxBuffer.append("< publishDebug"); + rxBuffer.append("/"); + rxBuffer.append(debugMsg); + rxBuffer.append("\n"); + try{ + record.write(rxBuffer.toString().getBytes()); + rxBuffer.setLength(0); + } catch (IOException e) { + log.warn("failed recording bytes.", e); + } + } + + break; + } + case PUBLISH_PIN_ARRAY: { + int[] data = subArray(ioCmd, startPos+2, ioCmd[startPos+1]); + startPos += 1 + ioCmd[startPos+1]; + if(invoke){ + arduino.invoke("publishPinArray", data); + } else { + arduino.publishPinArray( data); + } + if(record != null){ + rxBuffer.append("< publishPinArray"); + rxBuffer.append("/"); + rxBuffer.append(Arrays.toString(data)); + rxBuffer.append("\n"); + try{ + record.write(rxBuffer.toString().getBytes()); + rxBuffer.setLength(0); + } catch (IOException e) { + log.warn("failed recording bytes.", e); + } + } + + break; + } + case PUBLISH_SERVO_EVENT: { + Integer deviceId = ioCmd[startPos+1]; // bu8 + startPos += 1; + Integer eventType = ioCmd[startPos+1]; // bu8 + startPos += 1; + Integer currentPos = b16(ioCmd, startPos+1); + startPos += 2; //b16 + Integer targetPos = b16(ioCmd, startPos+1); + startPos += 2; //b16 + if(invoke){ + arduino.invoke("publishServoEvent", deviceId, eventType, currentPos, targetPos); + } else { + arduino.publishServoEvent( deviceId, eventType, currentPos, targetPos); + } + if(record != null){ + rxBuffer.append("< publishServoEvent"); + rxBuffer.append("/"); + rxBuffer.append(deviceId); + rxBuffer.append("/"); + rxBuffer.append(eventType); + rxBuffer.append("/"); + rxBuffer.append(currentPos); + rxBuffer.append("/"); + rxBuffer.append(targetPos); + rxBuffer.append("\n"); + try{ + record.write(rxBuffer.toString().getBytes()); + rxBuffer.setLength(0); + } catch (IOException e) { + log.warn("failed recording bytes.", e); + } + } + + break; + } + case PUBLISH_SERIAL_DATA: { + Integer deviceId = ioCmd[startPos+1]; // bu8 + startPos += 1; + int[] data = subArray(ioCmd, startPos+2, ioCmd[startPos+1]); + startPos += 1 + ioCmd[startPos+1]; + if(invoke){ + arduino.invoke("publishSerialData", deviceId, data); + } else { + arduino.publishSerialData( deviceId, data); + } + if(record != null){ + rxBuffer.append("< publishSerialData"); + rxBuffer.append("/"); + rxBuffer.append(deviceId); + rxBuffer.append("/"); + rxBuffer.append(Arrays.toString(data)); + rxBuffer.append("\n"); + try{ + record.write(rxBuffer.toString().getBytes()); + rxBuffer.setLength(0); + } catch (IOException e) { + log.warn("failed recording bytes.", e); + } + } + + break; + } + case PUBLISH_ULTRASONIC_SENSOR_DATA: { + Integer deviceId = ioCmd[startPos+1]; // bu8 + startPos += 1; + Integer echoTime = b16(ioCmd, startPos+1); + startPos += 2; //b16 + if(invoke){ + arduino.invoke("publishUltrasonicSensorData", deviceId, echoTime); + } else { + arduino.publishUltrasonicSensorData( deviceId, echoTime); + } + if(record != null){ + rxBuffer.append("< publishUltrasonicSensorData"); + rxBuffer.append("/"); + rxBuffer.append(deviceId); + rxBuffer.append("/"); + rxBuffer.append(echoTime); + rxBuffer.append("\n"); + try{ + record.write(rxBuffer.toString().getBytes()); + rxBuffer.setLength(0); + } catch (IOException e) { + log.warn("failed recording bytes.", e); + } + } + + break; + } + case PUBLISH_ENCODER_DATA: { + Integer deviceId = ioCmd[startPos+1]; // bu8 + startPos += 1; + Integer position = b16(ioCmd, startPos+1); + startPos += 2; //b16 + if(invoke){ + arduino.invoke("publishEncoderData", deviceId, position); + } else { + arduino.publishEncoderData( deviceId, position); + } + if(record != null){ + rxBuffer.append("< publishEncoderData"); + rxBuffer.append("/"); + rxBuffer.append(deviceId); + rxBuffer.append("/"); + rxBuffer.append(position); + rxBuffer.append("\n"); + try{ + record.write(rxBuffer.toString().getBytes()); + rxBuffer.setLength(0); + } catch (IOException e) { + log.warn("failed recording bytes.", e); + } + } + + break; + } + case PUBLISH_MRL_COMM_BEGIN: { + Integer version = ioCmd[startPos+1]; // bu8 + startPos += 1; + if(invoke){ + arduino.invoke("publishMrlCommBegin", version); + } else { + arduino.publishMrlCommBegin( version); + } + if(record != null){ + rxBuffer.append("< publishMrlCommBegin"); + rxBuffer.append("/"); + rxBuffer.append(version); + rxBuffer.append("\n"); + try{ + record.write(rxBuffer.toString().getBytes()); + rxBuffer.setLength(0); + } catch (IOException e) { + log.warn("failed recording bytes.", e); + } + } + + break; + } } } @@ -661,18 +664,18 @@ public synchronized byte[] getBoardInfo() { try { appendMessage(baos, MAGIC_NUMBER); appendMessage(baos, 1); // size - appendMessage(baos, GET_BOARD_INFO); // msgType = 2 + appendMessage(baos, GET_BOARD_INFO); // msgType = 2 byte[] message = sendMessage(baos); if (ackEnabled){ waitForAck(); } - if(record != null){ - txBuffer.append("> getBoardInfo"); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } + if(record != null){ + txBuffer.append("> getBoardInfo"); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } return message; } catch (Exception e) { @@ -689,27 +692,27 @@ public synchronized byte[] enablePin(Integer address/*byte*/, Integer type/*byte try { appendMessage(baos, MAGIC_NUMBER); appendMessage(baos, 1 + 1 + 1 + 2); // size - appendMessage(baos, ENABLE_PIN); // msgType = 4 - appendMessage(baos, address); - appendMessage(baos, type); - appendMessageb16(baos, rate); + appendMessage(baos, ENABLE_PIN); // msgType = 4 + appendMessage(baos, address); + appendMessage(baos, type); + appendMessageb16(baos, rate); byte[] message = sendMessage(baos); if (ackEnabled){ waitForAck(); } - if(record != null){ - txBuffer.append("> enablePin"); - txBuffer.append("/"); - txBuffer.append(address); - txBuffer.append("/"); - txBuffer.append(type); - txBuffer.append("/"); - txBuffer.append(rate); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } + if(record != null){ + txBuffer.append("> enablePin"); + txBuffer.append("/"); + txBuffer.append(address); + txBuffer.append("/"); + txBuffer.append(type); + txBuffer.append("/"); + txBuffer.append(rate); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } return message; } catch (Exception e) { @@ -726,21 +729,21 @@ public synchronized byte[] setDebug(Boolean enabled/*bool*/) { try { appendMessage(baos, MAGIC_NUMBER); appendMessage(baos, 1 + 1); // size - appendMessage(baos, SET_DEBUG); // msgType = 5 - appendMessagebool(baos, enabled); + appendMessage(baos, SET_DEBUG); // msgType = 5 + appendMessagebool(baos, enabled); byte[] message = sendMessage(baos); if (ackEnabled){ waitForAck(); } - if(record != null){ - txBuffer.append("> setDebug"); - txBuffer.append("/"); - txBuffer.append(enabled); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } + if(record != null){ + txBuffer.append("> setDebug"); + txBuffer.append("/"); + txBuffer.append(enabled); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } return message; } catch (Exception e) { @@ -757,21 +760,21 @@ public synchronized byte[] setSerialRate(Integer rate/*b32*/) { try { appendMessage(baos, MAGIC_NUMBER); appendMessage(baos, 1 + 4); // size - appendMessage(baos, SET_SERIAL_RATE); // msgType = 6 - appendMessageb32(baos, rate); + appendMessage(baos, SET_SERIAL_RATE); // msgType = 6 + appendMessageb32(baos, rate); byte[] message = sendMessage(baos); if (ackEnabled){ waitForAck(); } - if(record != null){ - txBuffer.append("> setSerialRate"); - txBuffer.append("/"); - txBuffer.append(rate); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } + if(record != null){ + txBuffer.append("> setSerialRate"); + txBuffer.append("/"); + txBuffer.append(rate); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } return message; } catch (Exception e) { @@ -788,18 +791,18 @@ public synchronized byte[] softReset() { try { appendMessage(baos, MAGIC_NUMBER); appendMessage(baos, 1); // size - appendMessage(baos, SOFT_RESET); // msgType = 7 + appendMessage(baos, SOFT_RESET); // msgType = 7 byte[] message = sendMessage(baos); if (ackEnabled){ waitForAck(); } - if(record != null){ - txBuffer.append("> softReset"); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } + if(record != null){ + txBuffer.append("> softReset"); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } return message; } catch (Exception e) { @@ -816,21 +819,21 @@ public synchronized byte[] enableAck(Boolean enabled/*bool*/) { try { appendMessage(baos, MAGIC_NUMBER); appendMessage(baos, 1 + 1); // size - appendMessage(baos, ENABLE_ACK); // msgType = 8 - appendMessagebool(baos, enabled); + appendMessage(baos, ENABLE_ACK); // msgType = 8 + appendMessagebool(baos, enabled); byte[] message = sendMessage(baos); if (ackEnabled){ waitForAck(); } - if(record != null){ - txBuffer.append("> enableAck"); - txBuffer.append("/"); - txBuffer.append(enabled); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } + if(record != null){ + txBuffer.append("> enableAck"); + txBuffer.append("/"); + txBuffer.append(enabled); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } return message; } catch (Exception e) { @@ -847,27 +850,27 @@ public synchronized byte[] echo(Float myFloat/*f32*/, Integer myByte/*byte*/, Fl try { appendMessage(baos, MAGIC_NUMBER); appendMessage(baos, 1 + 4 + 1 + 4); // size - appendMessage(baos, ECHO); // msgType = 10 - appendMessagef32(baos, myFloat); - appendMessage(baos, myByte); - appendMessagef32(baos, secondFloat); + appendMessage(baos, ECHO); // msgType = 10 + appendMessagef32(baos, myFloat); + appendMessage(baos, myByte); + appendMessagef32(baos, secondFloat); byte[] message = sendMessage(baos); if (ackEnabled){ waitForAck(); } - if(record != null){ - txBuffer.append("> echo"); - txBuffer.append("/"); - txBuffer.append(myFloat); - txBuffer.append("/"); - txBuffer.append(myByte); - txBuffer.append("/"); - txBuffer.append(secondFloat); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } + if(record != null){ + txBuffer.append("> echo"); + txBuffer.append("/"); + txBuffer.append(myFloat); + txBuffer.append("/"); + txBuffer.append(myByte); + txBuffer.append("/"); + txBuffer.append(secondFloat); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } return message; } catch (Exception e) { @@ -884,21 +887,21 @@ public synchronized byte[] customMsg(int[] msg/*[]*/) { try { appendMessage(baos, MAGIC_NUMBER); appendMessage(baos, 1 + (1 + msg.length)); // size - appendMessage(baos, CUSTOM_MSG); // msgType = 12 - appendMessage(baos, msg); + appendMessage(baos, CUSTOM_MSG); // msgType = 12 + appendMessage(baos, msg); byte[] message = sendMessage(baos); if (ackEnabled){ waitForAck(); } - if(record != null){ - txBuffer.append("> customMsg"); - txBuffer.append("/"); - txBuffer.append(Arrays.toString(msg)); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } + if(record != null){ + txBuffer.append("> customMsg"); + txBuffer.append("/"); + txBuffer.append(Arrays.toString(msg)); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } return message; } catch (Exception e) { @@ -915,21 +918,21 @@ public synchronized byte[] deviceDetach(Integer deviceId/*byte*/) { try { appendMessage(baos, MAGIC_NUMBER); appendMessage(baos, 1 + 1); // size - appendMessage(baos, DEVICE_DETACH); // msgType = 14 - appendMessage(baos, deviceId); + appendMessage(baos, DEVICE_DETACH); // msgType = 14 + appendMessage(baos, deviceId); byte[] message = sendMessage(baos); if (ackEnabled){ waitForAck(); } - if(record != null){ - txBuffer.append("> deviceDetach"); - txBuffer.append("/"); - txBuffer.append(deviceId); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } + if(record != null){ + txBuffer.append("> deviceDetach"); + txBuffer.append("/"); + txBuffer.append(deviceId); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } return message; } catch (Exception e) { @@ -946,24 +949,24 @@ public synchronized byte[] i2cBusAttach(Integer deviceId/*byte*/, Integer i2cBus try { appendMessage(baos, MAGIC_NUMBER); appendMessage(baos, 1 + 1 + 1); // size - appendMessage(baos, I2C_BUS_ATTACH); // msgType = 15 - appendMessage(baos, deviceId); - appendMessage(baos, i2cBus); + appendMessage(baos, I2C_BUS_ATTACH); // msgType = 15 + appendMessage(baos, deviceId); + appendMessage(baos, i2cBus); byte[] message = sendMessage(baos); if (ackEnabled){ waitForAck(); } - if(record != null){ - txBuffer.append("> i2cBusAttach"); - txBuffer.append("/"); - txBuffer.append(deviceId); - txBuffer.append("/"); - txBuffer.append(i2cBus); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } + if(record != null){ + txBuffer.append("> i2cBusAttach"); + txBuffer.append("/"); + txBuffer.append(deviceId); + txBuffer.append("/"); + txBuffer.append(i2cBus); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } return message; } catch (Exception e) { @@ -980,27 +983,27 @@ public synchronized byte[] i2cRead(Integer deviceId/*byte*/, Integer deviceAddre try { appendMessage(baos, MAGIC_NUMBER); appendMessage(baos, 1 + 1 + 1 + 1); // size - appendMessage(baos, I2C_READ); // msgType = 16 - appendMessage(baos, deviceId); - appendMessage(baos, deviceAddress); - appendMessage(baos, size); + appendMessage(baos, I2C_READ); // msgType = 16 + appendMessage(baos, deviceId); + appendMessage(baos, deviceAddress); + appendMessage(baos, size); byte[] message = sendMessage(baos); if (ackEnabled){ waitForAck(); } - if(record != null){ - txBuffer.append("> i2cRead"); - txBuffer.append("/"); - txBuffer.append(deviceId); - txBuffer.append("/"); - txBuffer.append(deviceAddress); - txBuffer.append("/"); - txBuffer.append(size); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } + if(record != null){ + txBuffer.append("> i2cRead"); + txBuffer.append("/"); + txBuffer.append(deviceId); + txBuffer.append("/"); + txBuffer.append(deviceAddress); + txBuffer.append("/"); + txBuffer.append(size); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } return message; } catch (Exception e) { @@ -1017,27 +1020,27 @@ public synchronized byte[] i2cWrite(Integer deviceId/*byte*/, Integer deviceAddr try { appendMessage(baos, MAGIC_NUMBER); appendMessage(baos, 1 + 1 + 1 + (1 + data.length)); // size - appendMessage(baos, I2C_WRITE); // msgType = 17 - appendMessage(baos, deviceId); - appendMessage(baos, deviceAddress); - appendMessage(baos, data); + appendMessage(baos, I2C_WRITE); // msgType = 17 + appendMessage(baos, deviceId); + appendMessage(baos, deviceAddress); + appendMessage(baos, data); byte[] message = sendMessage(baos); if (ackEnabled){ waitForAck(); } - if(record != null){ - txBuffer.append("> i2cWrite"); - txBuffer.append("/"); - txBuffer.append(deviceId); - txBuffer.append("/"); - txBuffer.append(deviceAddress); - txBuffer.append("/"); - txBuffer.append(Arrays.toString(data)); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } + if(record != null){ + txBuffer.append("> i2cWrite"); + txBuffer.append("/"); + txBuffer.append(deviceId); + txBuffer.append("/"); + txBuffer.append(deviceAddress); + txBuffer.append("/"); + txBuffer.append(Arrays.toString(data)); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } return message; } catch (Exception e) { @@ -1054,30 +1057,30 @@ public synchronized byte[] i2cWriteRead(Integer deviceId/*byte*/, Integer device try { appendMessage(baos, MAGIC_NUMBER); appendMessage(baos, 1 + 1 + 1 + 1 + 1); // size - appendMessage(baos, I2C_WRITE_READ); // msgType = 18 - appendMessage(baos, deviceId); - appendMessage(baos, deviceAddress); - appendMessage(baos, readSize); - appendMessage(baos, writeValue); + appendMessage(baos, I2C_WRITE_READ); // msgType = 18 + appendMessage(baos, deviceId); + appendMessage(baos, deviceAddress); + appendMessage(baos, readSize); + appendMessage(baos, writeValue); byte[] message = sendMessage(baos); if (ackEnabled){ waitForAck(); } - if(record != null){ - txBuffer.append("> i2cWriteRead"); - txBuffer.append("/"); - txBuffer.append(deviceId); - txBuffer.append("/"); - txBuffer.append(deviceAddress); - txBuffer.append("/"); - txBuffer.append(readSize); - txBuffer.append("/"); - txBuffer.append(writeValue); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } + if(record != null){ + txBuffer.append("> i2cWriteRead"); + txBuffer.append("/"); + txBuffer.append(deviceId); + txBuffer.append("/"); + txBuffer.append(deviceAddress); + txBuffer.append("/"); + txBuffer.append(readSize); + txBuffer.append("/"); + txBuffer.append(writeValue); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } return message; } catch (Exception e) { @@ -1094,30 +1097,30 @@ public synchronized byte[] neoPixelAttach(Integer deviceId/*byte*/, Integer pin/ try { appendMessage(baos, MAGIC_NUMBER); appendMessage(baos, 1 + 1 + 1 + 2 + 1); // size - appendMessage(baos, NEO_PIXEL_ATTACH); // msgType = 20 - appendMessage(baos, deviceId); - appendMessage(baos, pin); - appendMessageb16(baos, numPixels); - appendMessage(baos, depth); + appendMessage(baos, NEO_PIXEL_ATTACH); // msgType = 20 + appendMessage(baos, deviceId); + appendMessage(baos, pin); + appendMessageb16(baos, numPixels); + appendMessage(baos, depth); byte[] message = sendMessage(baos); if (ackEnabled){ waitForAck(); } - if(record != null){ - txBuffer.append("> neoPixelAttach"); - txBuffer.append("/"); - txBuffer.append(deviceId); - txBuffer.append("/"); - txBuffer.append(pin); - txBuffer.append("/"); - txBuffer.append(numPixels); - txBuffer.append("/"); - txBuffer.append(depth); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } + if(record != null){ + txBuffer.append("> neoPixelAttach"); + txBuffer.append("/"); + txBuffer.append(deviceId); + txBuffer.append("/"); + txBuffer.append(pin); + txBuffer.append("/"); + txBuffer.append(numPixels); + txBuffer.append("/"); + txBuffer.append(depth); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } return message; } catch (Exception e) { @@ -1134,39 +1137,39 @@ public synchronized byte[] neoPixelSetAnimation(Integer deviceId/*byte*/, Intege try { appendMessage(baos, MAGIC_NUMBER); appendMessage(baos, 1 + 1 + 1 + 1 + 1 + 1 + 1 + 4); // size - appendMessage(baos, NEO_PIXEL_SET_ANIMATION); // msgType = 21 - appendMessage(baos, deviceId); - appendMessage(baos, animation); - appendMessage(baos, red); - appendMessage(baos, green); - appendMessage(baos, blue); - appendMessage(baos, white); - appendMessageb32(baos, wait_ms); + appendMessage(baos, NEO_PIXEL_SET_ANIMATION); // msgType = 21 + appendMessage(baos, deviceId); + appendMessage(baos, animation); + appendMessage(baos, red); + appendMessage(baos, green); + appendMessage(baos, blue); + appendMessage(baos, white); + appendMessageb32(baos, wait_ms); byte[] message = sendMessage(baos); if (ackEnabled){ waitForAck(); } - if(record != null){ - txBuffer.append("> neoPixelSetAnimation"); - txBuffer.append("/"); - txBuffer.append(deviceId); - txBuffer.append("/"); - txBuffer.append(animation); - txBuffer.append("/"); - txBuffer.append(red); - txBuffer.append("/"); - txBuffer.append(green); - txBuffer.append("/"); - txBuffer.append(blue); - txBuffer.append("/"); - txBuffer.append(white); - txBuffer.append("/"); - txBuffer.append(wait_ms); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } + if(record != null){ + txBuffer.append("> neoPixelSetAnimation"); + txBuffer.append("/"); + txBuffer.append(deviceId); + txBuffer.append("/"); + txBuffer.append(animation); + txBuffer.append("/"); + txBuffer.append(red); + txBuffer.append("/"); + txBuffer.append(green); + txBuffer.append("/"); + txBuffer.append(blue); + txBuffer.append("/"); + txBuffer.append(white); + txBuffer.append("/"); + txBuffer.append(wait_ms); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } return message; } catch (Exception e) { @@ -1183,24 +1186,24 @@ public synchronized byte[] neoPixelWriteMatrix(Integer deviceId/*byte*/, int[] b try { appendMessage(baos, MAGIC_NUMBER); appendMessage(baos, 1 + 1 + (1 + buffer.length)); // size - appendMessage(baos, NEO_PIXEL_WRITE_MATRIX); // msgType = 22 - appendMessage(baos, deviceId); - appendMessage(baos, buffer); + appendMessage(baos, NEO_PIXEL_WRITE_MATRIX); // msgType = 22 + appendMessage(baos, deviceId); + appendMessage(baos, buffer); byte[] message = sendMessage(baos); if (ackEnabled){ waitForAck(); } - if(record != null){ - txBuffer.append("> neoPixelWriteMatrix"); - txBuffer.append("/"); - txBuffer.append(deviceId); - txBuffer.append("/"); - txBuffer.append(Arrays.toString(buffer)); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } + if(record != null){ + txBuffer.append("> neoPixelWriteMatrix"); + txBuffer.append("/"); + txBuffer.append(deviceId); + txBuffer.append("/"); + txBuffer.append(Arrays.toString(buffer)); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } return message; } catch (Exception e) { @@ -1217,39 +1220,39 @@ public synchronized byte[] neoPixelFill(Integer deviceId/*byte*/, Integer addres try { appendMessage(baos, MAGIC_NUMBER); appendMessage(baos, 1 + 1 + 2 + 2 + 1 + 1 + 1 + 1); // size - appendMessage(baos, NEO_PIXEL_FILL); // msgType = 23 - appendMessage(baos, deviceId); - appendMessageb16(baos, address); - appendMessageb16(baos, count); - appendMessage(baos, red); - appendMessage(baos, green); - appendMessage(baos, blue); - appendMessage(baos, white); + appendMessage(baos, NEO_PIXEL_FILL); // msgType = 23 + appendMessage(baos, deviceId); + appendMessageb16(baos, address); + appendMessageb16(baos, count); + appendMessage(baos, red); + appendMessage(baos, green); + appendMessage(baos, blue); + appendMessage(baos, white); byte[] message = sendMessage(baos); if (ackEnabled){ waitForAck(); } - if(record != null){ - txBuffer.append("> neoPixelFill"); - txBuffer.append("/"); - txBuffer.append(deviceId); - txBuffer.append("/"); - txBuffer.append(address); - txBuffer.append("/"); - txBuffer.append(count); - txBuffer.append("/"); - txBuffer.append(red); - txBuffer.append("/"); - txBuffer.append(green); - txBuffer.append("/"); - txBuffer.append(blue); - txBuffer.append("/"); - txBuffer.append(white); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } + if(record != null){ + txBuffer.append("> neoPixelFill"); + txBuffer.append("/"); + txBuffer.append(deviceId); + txBuffer.append("/"); + txBuffer.append(address); + txBuffer.append("/"); + txBuffer.append(count); + txBuffer.append("/"); + txBuffer.append(red); + txBuffer.append("/"); + txBuffer.append(green); + txBuffer.append("/"); + txBuffer.append(blue); + txBuffer.append("/"); + txBuffer.append(white); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } return message; } catch (Exception e) { @@ -1266,24 +1269,24 @@ public synchronized byte[] neoPixelSetBrightness(Integer deviceId/*byte*/, Integ try { appendMessage(baos, MAGIC_NUMBER); appendMessage(baos, 1 + 1 + 1); // size - appendMessage(baos, NEO_PIXEL_SET_BRIGHTNESS); // msgType = 24 - appendMessage(baos, deviceId); - appendMessage(baos, brightness); + appendMessage(baos, NEO_PIXEL_SET_BRIGHTNESS); // msgType = 24 + appendMessage(baos, deviceId); + appendMessage(baos, brightness); byte[] message = sendMessage(baos); if (ackEnabled){ waitForAck(); } - if(record != null){ - txBuffer.append("> neoPixelSetBrightness"); - txBuffer.append("/"); - txBuffer.append(deviceId); - txBuffer.append("/"); - txBuffer.append(brightness); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } + if(record != null){ + txBuffer.append("> neoPixelSetBrightness"); + txBuffer.append("/"); + txBuffer.append(deviceId); + txBuffer.append("/"); + txBuffer.append(brightness); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } return message; } catch (Exception e) { @@ -1300,21 +1303,21 @@ public synchronized byte[] neoPixelClear(Integer deviceId/*byte*/) { try { appendMessage(baos, MAGIC_NUMBER); appendMessage(baos, 1 + 1); // size - appendMessage(baos, NEO_PIXEL_CLEAR); // msgType = 25 - appendMessage(baos, deviceId); + appendMessage(baos, NEO_PIXEL_CLEAR); // msgType = 25 + appendMessage(baos, deviceId); byte[] message = sendMessage(baos); if (ackEnabled){ waitForAck(); } - if(record != null){ - txBuffer.append("> neoPixelClear"); - txBuffer.append("/"); - txBuffer.append(deviceId); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } + if(record != null){ + txBuffer.append("> neoPixelClear"); + txBuffer.append("/"); + txBuffer.append(deviceId); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } return message; } catch (Exception e) { @@ -1331,24 +1334,24 @@ public synchronized byte[] analogWrite(Integer pin/*byte*/, Integer value/*byte* try { appendMessage(baos, MAGIC_NUMBER); appendMessage(baos, 1 + 1 + 1); // size - appendMessage(baos, ANALOG_WRITE); // msgType = 26 - appendMessage(baos, pin); - appendMessage(baos, value); + appendMessage(baos, ANALOG_WRITE); // msgType = 26 + appendMessage(baos, pin); + appendMessage(baos, value); byte[] message = sendMessage(baos); if (ackEnabled){ waitForAck(); } - if(record != null){ - txBuffer.append("> analogWrite"); - txBuffer.append("/"); - txBuffer.append(pin); - txBuffer.append("/"); - txBuffer.append(value); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } + if(record != null){ + txBuffer.append("> analogWrite"); + txBuffer.append("/"); + txBuffer.append(pin); + txBuffer.append("/"); + txBuffer.append(value); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } return message; } catch (Exception e) { @@ -1365,24 +1368,24 @@ public synchronized byte[] digitalWrite(Integer pin/*byte*/, Integer value/*byte try { appendMessage(baos, MAGIC_NUMBER); appendMessage(baos, 1 + 1 + 1); // size - appendMessage(baos, DIGITAL_WRITE); // msgType = 27 - appendMessage(baos, pin); - appendMessage(baos, value); + appendMessage(baos, DIGITAL_WRITE); // msgType = 27 + appendMessage(baos, pin); + appendMessage(baos, value); byte[] message = sendMessage(baos); if (ackEnabled){ waitForAck(); } - if(record != null){ - txBuffer.append("> digitalWrite"); - txBuffer.append("/"); - txBuffer.append(pin); - txBuffer.append("/"); - txBuffer.append(value); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } + if(record != null){ + txBuffer.append("> digitalWrite"); + txBuffer.append("/"); + txBuffer.append(pin); + txBuffer.append("/"); + txBuffer.append(value); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } return message; } catch (Exception e) { @@ -1399,21 +1402,21 @@ public synchronized byte[] disablePin(Integer pin/*byte*/) { try { appendMessage(baos, MAGIC_NUMBER); appendMessage(baos, 1 + 1); // size - appendMessage(baos, DISABLE_PIN); // msgType = 28 - appendMessage(baos, pin); + appendMessage(baos, DISABLE_PIN); // msgType = 28 + appendMessage(baos, pin); byte[] message = sendMessage(baos); if (ackEnabled){ waitForAck(); } - if(record != null){ - txBuffer.append("> disablePin"); - txBuffer.append("/"); - txBuffer.append(pin); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } + if(record != null){ + txBuffer.append("> disablePin"); + txBuffer.append("/"); + txBuffer.append(pin); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } return message; } catch (Exception e) { @@ -1430,18 +1433,18 @@ public synchronized byte[] disablePins() { try { appendMessage(baos, MAGIC_NUMBER); appendMessage(baos, 1); // size - appendMessage(baos, DISABLE_PINS); // msgType = 29 + appendMessage(baos, DISABLE_PINS); // msgType = 29 byte[] message = sendMessage(baos); if (ackEnabled){ waitForAck(); } - if(record != null){ - txBuffer.append("> disablePins"); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } + if(record != null){ + txBuffer.append("> disablePins"); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } return message; } catch (Exception e) { @@ -1458,24 +1461,24 @@ public synchronized byte[] pinMode(Integer pin/*byte*/, Integer mode/*byte*/) { try { appendMessage(baos, MAGIC_NUMBER); appendMessage(baos, 1 + 1 + 1); // size - appendMessage(baos, PIN_MODE); // msgType = 30 - appendMessage(baos, pin); - appendMessage(baos, mode); + appendMessage(baos, PIN_MODE); // msgType = 30 + appendMessage(baos, pin); + appendMessage(baos, mode); byte[] message = sendMessage(baos); if (ackEnabled){ waitForAck(); } - if(record != null){ - txBuffer.append("> pinMode"); - txBuffer.append("/"); - txBuffer.append(pin); - txBuffer.append("/"); - txBuffer.append(mode); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } + if(record != null){ + txBuffer.append("> pinMode"); + txBuffer.append("/"); + txBuffer.append(pin); + txBuffer.append("/"); + txBuffer.append(mode); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } return message; } catch (Exception e) { @@ -1492,24 +1495,24 @@ public synchronized byte[] setTrigger(Integer pin/*byte*/, Integer triggerValue/ try { appendMessage(baos, MAGIC_NUMBER); appendMessage(baos, 1 + 1 + 1); // size - appendMessage(baos, SET_TRIGGER); // msgType = 33 - appendMessage(baos, pin); - appendMessage(baos, triggerValue); + appendMessage(baos, SET_TRIGGER); // msgType = 33 + appendMessage(baos, pin); + appendMessage(baos, triggerValue); byte[] message = sendMessage(baos); if (ackEnabled){ waitForAck(); } - if(record != null){ - txBuffer.append("> setTrigger"); - txBuffer.append("/"); - txBuffer.append(pin); - txBuffer.append("/"); - txBuffer.append(triggerValue); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } + if(record != null){ + txBuffer.append("> setTrigger"); + txBuffer.append("/"); + txBuffer.append(pin); + txBuffer.append("/"); + txBuffer.append(triggerValue); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } return message; } catch (Exception e) { @@ -1526,24 +1529,24 @@ public synchronized byte[] setDebounce(Integer pin/*byte*/, Integer delay/*byte* try { appendMessage(baos, MAGIC_NUMBER); appendMessage(baos, 1 + 1 + 1); // size - appendMessage(baos, SET_DEBOUNCE); // msgType = 34 - appendMessage(baos, pin); - appendMessage(baos, delay); + appendMessage(baos, SET_DEBOUNCE); // msgType = 34 + appendMessage(baos, pin); + appendMessage(baos, delay); byte[] message = sendMessage(baos); if (ackEnabled){ waitForAck(); } - if(record != null){ - txBuffer.append("> setDebounce"); - txBuffer.append("/"); - txBuffer.append(pin); - txBuffer.append("/"); - txBuffer.append(delay); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } + if(record != null){ + txBuffer.append("> setDebounce"); + txBuffer.append("/"); + txBuffer.append(pin); + txBuffer.append("/"); + txBuffer.append(delay); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } return message; } catch (Exception e) { @@ -1560,33 +1563,33 @@ public synchronized byte[] servoAttach(Integer deviceId/*byte*/, Integer pin/*by try { appendMessage(baos, MAGIC_NUMBER); appendMessage(baos, 1 + 1 + 1 + 2 + 2 + (1 + name.length())); // size - appendMessage(baos, SERVO_ATTACH); // msgType = 35 - appendMessage(baos, deviceId); - appendMessage(baos, pin); - appendMessageb16(baos, initPos); - appendMessageb16(baos, initVelocity); - appendMessage(baos, name); + appendMessage(baos, SERVO_ATTACH); // msgType = 35 + appendMessage(baos, deviceId); + appendMessage(baos, pin); + appendMessageb16(baos, initPos); + appendMessageb16(baos, initVelocity); + appendMessage(baos, name); byte[] message = sendMessage(baos); if (ackEnabled){ waitForAck(); } - if(record != null){ - txBuffer.append("> servoAttach"); - txBuffer.append("/"); - txBuffer.append(deviceId); - txBuffer.append("/"); - txBuffer.append(pin); - txBuffer.append("/"); - txBuffer.append(initPos); - txBuffer.append("/"); - txBuffer.append(initVelocity); - txBuffer.append("/"); - txBuffer.append(name); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } + if(record != null){ + txBuffer.append("> servoAttach"); + txBuffer.append("/"); + txBuffer.append(deviceId); + txBuffer.append("/"); + txBuffer.append(pin); + txBuffer.append("/"); + txBuffer.append(initPos); + txBuffer.append("/"); + txBuffer.append(initVelocity); + txBuffer.append("/"); + txBuffer.append(name); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } return message; } catch (Exception e) { @@ -1603,24 +1606,24 @@ public synchronized byte[] servoAttachPin(Integer deviceId/*byte*/, Integer pin/ try { appendMessage(baos, MAGIC_NUMBER); appendMessage(baos, 1 + 1 + 1); // size - appendMessage(baos, SERVO_ATTACH_PIN); // msgType = 36 - appendMessage(baos, deviceId); - appendMessage(baos, pin); + appendMessage(baos, SERVO_ATTACH_PIN); // msgType = 36 + appendMessage(baos, deviceId); + appendMessage(baos, pin); byte[] message = sendMessage(baos); if (ackEnabled){ waitForAck(); } - if(record != null){ - txBuffer.append("> servoAttachPin"); - txBuffer.append("/"); - txBuffer.append(deviceId); - txBuffer.append("/"); - txBuffer.append(pin); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } + if(record != null){ + txBuffer.append("> servoAttachPin"); + txBuffer.append("/"); + txBuffer.append(deviceId); + txBuffer.append("/"); + txBuffer.append(pin); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } return message; } catch (Exception e) { @@ -1637,21 +1640,21 @@ public synchronized byte[] servoDetachPin(Integer deviceId/*byte*/) { try { appendMessage(baos, MAGIC_NUMBER); appendMessage(baos, 1 + 1); // size - appendMessage(baos, SERVO_DETACH_PIN); // msgType = 37 - appendMessage(baos, deviceId); + appendMessage(baos, SERVO_DETACH_PIN); // msgType = 37 + appendMessage(baos, deviceId); byte[] message = sendMessage(baos); if (ackEnabled){ waitForAck(); } - if(record != null){ - txBuffer.append("> servoDetachPin"); - txBuffer.append("/"); - txBuffer.append(deviceId); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } + if(record != null){ + txBuffer.append("> servoDetachPin"); + txBuffer.append("/"); + txBuffer.append(deviceId); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } return message; } catch (Exception e) { @@ -1668,24 +1671,24 @@ public synchronized byte[] servoSetVelocity(Integer deviceId/*byte*/, Integer ve try { appendMessage(baos, MAGIC_NUMBER); appendMessage(baos, 1 + 1 + 2); // size - appendMessage(baos, SERVO_SET_VELOCITY); // msgType = 38 - appendMessage(baos, deviceId); - appendMessageb16(baos, velocity); + appendMessage(baos, SERVO_SET_VELOCITY); // msgType = 38 + appendMessage(baos, deviceId); + appendMessageb16(baos, velocity); byte[] message = sendMessage(baos); if (ackEnabled){ waitForAck(); } - if(record != null){ - txBuffer.append("> servoSetVelocity"); - txBuffer.append("/"); - txBuffer.append(deviceId); - txBuffer.append("/"); - txBuffer.append(velocity); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } + if(record != null){ + txBuffer.append("> servoSetVelocity"); + txBuffer.append("/"); + txBuffer.append(deviceId); + txBuffer.append("/"); + txBuffer.append(velocity); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } return message; } catch (Exception e) { @@ -1702,30 +1705,30 @@ public synchronized byte[] servoSweepStart(Integer deviceId/*byte*/, Integer min try { appendMessage(baos, MAGIC_NUMBER); appendMessage(baos, 1 + 1 + 1 + 1 + 1); // size - appendMessage(baos, SERVO_SWEEP_START); // msgType = 39 - appendMessage(baos, deviceId); - appendMessage(baos, min); - appendMessage(baos, max); - appendMessage(baos, step); + appendMessage(baos, SERVO_SWEEP_START); // msgType = 39 + appendMessage(baos, deviceId); + appendMessage(baos, min); + appendMessage(baos, max); + appendMessage(baos, step); byte[] message = sendMessage(baos); if (ackEnabled){ waitForAck(); } - if(record != null){ - txBuffer.append("> servoSweepStart"); - txBuffer.append("/"); - txBuffer.append(deviceId); - txBuffer.append("/"); - txBuffer.append(min); - txBuffer.append("/"); - txBuffer.append(max); - txBuffer.append("/"); - txBuffer.append(step); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } + if(record != null){ + txBuffer.append("> servoSweepStart"); + txBuffer.append("/"); + txBuffer.append(deviceId); + txBuffer.append("/"); + txBuffer.append(min); + txBuffer.append("/"); + txBuffer.append(max); + txBuffer.append("/"); + txBuffer.append(step); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } return message; } catch (Exception e) { @@ -1742,21 +1745,21 @@ public synchronized byte[] servoSweepStop(Integer deviceId/*byte*/) { try { appendMessage(baos, MAGIC_NUMBER); appendMessage(baos, 1 + 1); // size - appendMessage(baos, SERVO_SWEEP_STOP); // msgType = 40 - appendMessage(baos, deviceId); + appendMessage(baos, SERVO_SWEEP_STOP); // msgType = 40 + appendMessage(baos, deviceId); byte[] message = sendMessage(baos); if (ackEnabled){ waitForAck(); } - if(record != null){ - txBuffer.append("> servoSweepStop"); - txBuffer.append("/"); - txBuffer.append(deviceId); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } + if(record != null){ + txBuffer.append("> servoSweepStop"); + txBuffer.append("/"); + txBuffer.append(deviceId); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } return message; } catch (Exception e) { @@ -1773,24 +1776,24 @@ public synchronized byte[] servoMoveToMicroseconds(Integer deviceId/*byte*/, Int try { appendMessage(baos, MAGIC_NUMBER); appendMessage(baos, 1 + 1 + 2); // size - appendMessage(baos, SERVO_MOVE_TO_MICROSECONDS); // msgType = 41 - appendMessage(baos, deviceId); - appendMessageb16(baos, target); + appendMessage(baos, SERVO_MOVE_TO_MICROSECONDS); // msgType = 41 + appendMessage(baos, deviceId); + appendMessageb16(baos, target); byte[] message = sendMessage(baos); if (ackEnabled){ waitForAck(); } - if(record != null){ - txBuffer.append("> servoMoveToMicroseconds"); - txBuffer.append("/"); - txBuffer.append(deviceId); - txBuffer.append("/"); - txBuffer.append(target); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } + if(record != null){ + txBuffer.append("> servoMoveToMicroseconds"); + txBuffer.append("/"); + txBuffer.append(deviceId); + txBuffer.append("/"); + txBuffer.append(target); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } return message; } catch (Exception e) { @@ -1807,24 +1810,24 @@ public synchronized byte[] servoSetAcceleration(Integer deviceId/*byte*/, Intege try { appendMessage(baos, MAGIC_NUMBER); appendMessage(baos, 1 + 1 + 2); // size - appendMessage(baos, SERVO_SET_ACCELERATION); // msgType = 42 - appendMessage(baos, deviceId); - appendMessageb16(baos, acceleration); + appendMessage(baos, SERVO_SET_ACCELERATION); // msgType = 42 + appendMessage(baos, deviceId); + appendMessageb16(baos, acceleration); byte[] message = sendMessage(baos); if (ackEnabled){ waitForAck(); } - if(record != null){ - txBuffer.append("> servoSetAcceleration"); - txBuffer.append("/"); - txBuffer.append(deviceId); - txBuffer.append("/"); - txBuffer.append(acceleration); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } + if(record != null){ + txBuffer.append("> servoSetAcceleration"); + txBuffer.append("/"); + txBuffer.append(deviceId); + txBuffer.append("/"); + txBuffer.append(acceleration); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } return message; } catch (Exception e) { @@ -1841,24 +1844,24 @@ public synchronized byte[] serialAttach(Integer deviceId/*byte*/, Integer relayP try { appendMessage(baos, MAGIC_NUMBER); appendMessage(baos, 1 + 1 + 1); // size - appendMessage(baos, SERIAL_ATTACH); // msgType = 44 - appendMessage(baos, deviceId); - appendMessage(baos, relayPin); + appendMessage(baos, SERIAL_ATTACH); // msgType = 44 + appendMessage(baos, deviceId); + appendMessage(baos, relayPin); byte[] message = sendMessage(baos); if (ackEnabled){ waitForAck(); } - if(record != null){ - txBuffer.append("> serialAttach"); - txBuffer.append("/"); - txBuffer.append(deviceId); - txBuffer.append("/"); - txBuffer.append(relayPin); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } + if(record != null){ + txBuffer.append("> serialAttach"); + txBuffer.append("/"); + txBuffer.append(deviceId); + txBuffer.append("/"); + txBuffer.append(relayPin); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } return message; } catch (Exception e) { @@ -1875,24 +1878,24 @@ public synchronized byte[] serialRelay(Integer deviceId/*byte*/, int[] data/*[]* try { appendMessage(baos, MAGIC_NUMBER); appendMessage(baos, 1 + 1 + (1 + data.length)); // size - appendMessage(baos, SERIAL_RELAY); // msgType = 45 - appendMessage(baos, deviceId); - appendMessage(baos, data); + appendMessage(baos, SERIAL_RELAY); // msgType = 45 + appendMessage(baos, deviceId); + appendMessage(baos, data); byte[] message = sendMessage(baos); if (ackEnabled){ waitForAck(); } - if(record != null){ - txBuffer.append("> serialRelay"); - txBuffer.append("/"); - txBuffer.append(deviceId); - txBuffer.append("/"); - txBuffer.append(Arrays.toString(data)); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } + if(record != null){ + txBuffer.append("> serialRelay"); + txBuffer.append("/"); + txBuffer.append(deviceId); + txBuffer.append("/"); + txBuffer.append(Arrays.toString(data)); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } return message; } catch (Exception e) { @@ -1909,27 +1912,27 @@ public synchronized byte[] ultrasonicSensorAttach(Integer deviceId/*byte*/, Inte try { appendMessage(baos, MAGIC_NUMBER); appendMessage(baos, 1 + 1 + 1 + 1); // size - appendMessage(baos, ULTRASONIC_SENSOR_ATTACH); // msgType = 47 - appendMessage(baos, deviceId); - appendMessage(baos, triggerPin); - appendMessage(baos, echoPin); + appendMessage(baos, ULTRASONIC_SENSOR_ATTACH); // msgType = 47 + appendMessage(baos, deviceId); + appendMessage(baos, triggerPin); + appendMessage(baos, echoPin); byte[] message = sendMessage(baos); if (ackEnabled){ waitForAck(); } - if(record != null){ - txBuffer.append("> ultrasonicSensorAttach"); - txBuffer.append("/"); - txBuffer.append(deviceId); - txBuffer.append("/"); - txBuffer.append(triggerPin); - txBuffer.append("/"); - txBuffer.append(echoPin); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } + if(record != null){ + txBuffer.append("> ultrasonicSensorAttach"); + txBuffer.append("/"); + txBuffer.append(deviceId); + txBuffer.append("/"); + txBuffer.append(triggerPin); + txBuffer.append("/"); + txBuffer.append(echoPin); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } return message; } catch (Exception e) { @@ -1946,21 +1949,21 @@ public synchronized byte[] ultrasonicSensorStartRanging(Integer deviceId/*byte*/ try { appendMessage(baos, MAGIC_NUMBER); appendMessage(baos, 1 + 1); // size - appendMessage(baos, ULTRASONIC_SENSOR_START_RANGING); // msgType = 48 - appendMessage(baos, deviceId); + appendMessage(baos, ULTRASONIC_SENSOR_START_RANGING); // msgType = 48 + appendMessage(baos, deviceId); byte[] message = sendMessage(baos); if (ackEnabled){ waitForAck(); } - if(record != null){ - txBuffer.append("> ultrasonicSensorStartRanging"); - txBuffer.append("/"); - txBuffer.append(deviceId); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } + if(record != null){ + txBuffer.append("> ultrasonicSensorStartRanging"); + txBuffer.append("/"); + txBuffer.append(deviceId); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } return message; } catch (Exception e) { @@ -1977,21 +1980,21 @@ public synchronized byte[] ultrasonicSensorStopRanging(Integer deviceId/*byte*/) try { appendMessage(baos, MAGIC_NUMBER); appendMessage(baos, 1 + 1); // size - appendMessage(baos, ULTRASONIC_SENSOR_STOP_RANGING); // msgType = 49 - appendMessage(baos, deviceId); + appendMessage(baos, ULTRASONIC_SENSOR_STOP_RANGING); // msgType = 49 + appendMessage(baos, deviceId); byte[] message = sendMessage(baos); if (ackEnabled){ waitForAck(); } - if(record != null){ - txBuffer.append("> ultrasonicSensorStopRanging"); - txBuffer.append("/"); - txBuffer.append(deviceId); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } + if(record != null){ + txBuffer.append("> ultrasonicSensorStopRanging"); + txBuffer.append("/"); + txBuffer.append(deviceId); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } return message; } catch (Exception e) { @@ -2008,21 +2011,21 @@ public synchronized byte[] setAref(Integer type/*b16*/) { try { appendMessage(baos, MAGIC_NUMBER); appendMessage(baos, 1 + 2); // size - appendMessage(baos, SET_AREF); // msgType = 51 - appendMessageb16(baos, type); + appendMessage(baos, SET_AREF); // msgType = 51 + appendMessageb16(baos, type); byte[] message = sendMessage(baos); if (ackEnabled){ waitForAck(); } - if(record != null){ - txBuffer.append("> setAref"); - txBuffer.append("/"); - txBuffer.append(type); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } + if(record != null){ + txBuffer.append("> setAref"); + txBuffer.append("/"); + txBuffer.append(type); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } return message; } catch (Exception e) { @@ -2039,27 +2042,27 @@ public synchronized byte[] motorAttach(Integer deviceId/*byte*/, Integer type/*b try { appendMessage(baos, MAGIC_NUMBER); appendMessage(baos, 1 + 1 + 1 + (1 + pins.length)); // size - appendMessage(baos, MOTOR_ATTACH); // msgType = 52 - appendMessage(baos, deviceId); - appendMessage(baos, type); - appendMessage(baos, pins); + appendMessage(baos, MOTOR_ATTACH); // msgType = 52 + appendMessage(baos, deviceId); + appendMessage(baos, type); + appendMessage(baos, pins); byte[] message = sendMessage(baos); if (ackEnabled){ waitForAck(); } - if(record != null){ - txBuffer.append("> motorAttach"); - txBuffer.append("/"); - txBuffer.append(deviceId); - txBuffer.append("/"); - txBuffer.append(type); - txBuffer.append("/"); - txBuffer.append(Arrays.toString(pins)); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } + if(record != null){ + txBuffer.append("> motorAttach"); + txBuffer.append("/"); + txBuffer.append(deviceId); + txBuffer.append("/"); + txBuffer.append(type); + txBuffer.append("/"); + txBuffer.append(Arrays.toString(pins)); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } return message; } catch (Exception e) { @@ -2076,24 +2079,24 @@ public synchronized byte[] motorMove(Integer deviceId/*byte*/, Integer pwr/*byte try { appendMessage(baos, MAGIC_NUMBER); appendMessage(baos, 1 + 1 + 1); // size - appendMessage(baos, MOTOR_MOVE); // msgType = 53 - appendMessage(baos, deviceId); - appendMessage(baos, pwr); + appendMessage(baos, MOTOR_MOVE); // msgType = 53 + appendMessage(baos, deviceId); + appendMessage(baos, pwr); byte[] message = sendMessage(baos); if (ackEnabled){ waitForAck(); } - if(record != null){ - txBuffer.append("> motorMove"); - txBuffer.append("/"); - txBuffer.append(deviceId); - txBuffer.append("/"); - txBuffer.append(pwr); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } + if(record != null){ + txBuffer.append("> motorMove"); + txBuffer.append("/"); + txBuffer.append(deviceId); + txBuffer.append("/"); + txBuffer.append(pwr); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } return message; } catch (Exception e) { @@ -2110,24 +2113,24 @@ public synchronized byte[] motorMoveTo(Integer deviceId/*byte*/, Integer pos/*by try { appendMessage(baos, MAGIC_NUMBER); appendMessage(baos, 1 + 1 + 1); // size - appendMessage(baos, MOTOR_MOVE_TO); // msgType = 54 - appendMessage(baos, deviceId); - appendMessage(baos, pos); + appendMessage(baos, MOTOR_MOVE_TO); // msgType = 54 + appendMessage(baos, deviceId); + appendMessage(baos, pos); byte[] message = sendMessage(baos); if (ackEnabled){ waitForAck(); } - if(record != null){ - txBuffer.append("> motorMoveTo"); - txBuffer.append("/"); - txBuffer.append(deviceId); - txBuffer.append("/"); - txBuffer.append(pos); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } + if(record != null){ + txBuffer.append("> motorMoveTo"); + txBuffer.append("/"); + txBuffer.append(deviceId); + txBuffer.append("/"); + txBuffer.append(pos); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } return message; } catch (Exception e) { @@ -2144,27 +2147,27 @@ public synchronized byte[] encoderAttach(Integer deviceId/*byte*/, Integer type/ try { appendMessage(baos, MAGIC_NUMBER); appendMessage(baos, 1 + 1 + 1 + 1); // size - appendMessage(baos, ENCODER_ATTACH); // msgType = 55 - appendMessage(baos, deviceId); - appendMessage(baos, type); - appendMessage(baos, pin); + appendMessage(baos, ENCODER_ATTACH); // msgType = 55 + appendMessage(baos, deviceId); + appendMessage(baos, type); + appendMessage(baos, pin); byte[] message = sendMessage(baos); if (ackEnabled){ waitForAck(); } - if(record != null){ - txBuffer.append("> encoderAttach"); - txBuffer.append("/"); - txBuffer.append(deviceId); - txBuffer.append("/"); - txBuffer.append(type); - txBuffer.append("/"); - txBuffer.append(pin); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } + if(record != null){ + txBuffer.append("> encoderAttach"); + txBuffer.append("/"); + txBuffer.append(deviceId); + txBuffer.append("/"); + txBuffer.append(type); + txBuffer.append("/"); + txBuffer.append(pin); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } return message; } catch (Exception e) { @@ -2181,21 +2184,21 @@ public synchronized byte[] setZeroPoint(Integer deviceId/*byte*/) { try { appendMessage(baos, MAGIC_NUMBER); appendMessage(baos, 1 + 1); // size - appendMessage(baos, SET_ZERO_POINT); // msgType = 56 - appendMessage(baos, deviceId); + appendMessage(baos, SET_ZERO_POINT); // msgType = 56 + appendMessage(baos, deviceId); byte[] message = sendMessage(baos); if (ackEnabled){ waitForAck(); } - if(record != null){ - txBuffer.append("> setZeroPoint"); - txBuffer.append("/"); - txBuffer.append(deviceId); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } + if(record != null){ + txBuffer.append("> setZeroPoint"); + txBuffer.append("/"); + txBuffer.append(deviceId); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } return message; } catch (Exception e) { @@ -2212,21 +2215,21 @@ public synchronized byte[] servoStop(Integer deviceId/*byte*/) { try { appendMessage(baos, MAGIC_NUMBER); appendMessage(baos, 1 + 1); // size - appendMessage(baos, SERVO_STOP); // msgType = 59 - appendMessage(baos, deviceId); + appendMessage(baos, SERVO_STOP); // msgType = 59 + appendMessage(baos, deviceId); byte[] message = sendMessage(baos); if (ackEnabled){ waitForAck(); } - if(record != null){ - txBuffer.append("> servoStop"); - txBuffer.append("/"); - txBuffer.append(deviceId); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } + if(record != null){ + txBuffer.append("> servoStop"); + txBuffer.append("/"); + txBuffer.append(deviceId); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } return message; } catch (Exception e) { @@ -2238,183 +2241,183 @@ public synchronized byte[] servoStop(Integer deviceId/*byte*/) { public static String methodToString(int method) { switch (method) { - case PUBLISH_MRLCOMM_ERROR:{ - return "publishMRLCommError"; - } - case GET_BOARD_INFO:{ - return "getBoardInfo"; - } - case PUBLISH_BOARD_INFO:{ - return "publishBoardInfo"; - } - case ENABLE_PIN:{ - return "enablePin"; - } - case SET_DEBUG:{ - return "setDebug"; - } - case SET_SERIAL_RATE:{ - return "setSerialRate"; - } - case SOFT_RESET:{ - return "softReset"; - } - case ENABLE_ACK:{ - return "enableAck"; - } - case PUBLISH_ACK:{ - return "publishAck"; - } - case ECHO:{ - return "echo"; - } - case PUBLISH_ECHO:{ - return "publishEcho"; - } - case CUSTOM_MSG:{ - return "customMsg"; - } - case PUBLISH_CUSTOM_MSG:{ - return "publishCustomMsg"; - } - case DEVICE_DETACH:{ - return "deviceDetach"; - } - case I2C_BUS_ATTACH:{ - return "i2cBusAttach"; - } - case I2C_READ:{ - return "i2cRead"; - } - case I2C_WRITE:{ - return "i2cWrite"; - } - case I2C_WRITE_READ:{ - return "i2cWriteRead"; - } - case PUBLISH_I2C_DATA:{ - return "publishI2cData"; - } - case NEO_PIXEL_ATTACH:{ - return "neoPixelAttach"; - } - case NEO_PIXEL_SET_ANIMATION:{ - return "neoPixelSetAnimation"; - } - case NEO_PIXEL_WRITE_MATRIX:{ - return "neoPixelWriteMatrix"; - } - case NEO_PIXEL_FILL:{ - return "neoPixelFill"; - } - case NEO_PIXEL_SET_BRIGHTNESS:{ - return "neoPixelSetBrightness"; - } - case NEO_PIXEL_CLEAR:{ - return "neoPixelClear"; - } - case ANALOG_WRITE:{ - return "analogWrite"; - } - case DIGITAL_WRITE:{ - return "digitalWrite"; - } - case DISABLE_PIN:{ - return "disablePin"; - } - case DISABLE_PINS:{ - return "disablePins"; - } - case PIN_MODE:{ - return "pinMode"; - } - case PUBLISH_DEBUG:{ - return "publishDebug"; - } - case PUBLISH_PIN_ARRAY:{ - return "publishPinArray"; - } - case SET_TRIGGER:{ - return "setTrigger"; - } - case SET_DEBOUNCE:{ - return "setDebounce"; - } - case SERVO_ATTACH:{ - return "servoAttach"; - } - case SERVO_ATTACH_PIN:{ - return "servoAttachPin"; - } - case SERVO_DETACH_PIN:{ - return "servoDetachPin"; - } - case SERVO_SET_VELOCITY:{ - return "servoSetVelocity"; - } - case SERVO_SWEEP_START:{ - return "servoSweepStart"; - } - case SERVO_SWEEP_STOP:{ - return "servoSweepStop"; - } - case SERVO_MOVE_TO_MICROSECONDS:{ - return "servoMoveToMicroseconds"; - } - case SERVO_SET_ACCELERATION:{ - return "servoSetAcceleration"; - } - case PUBLISH_SERVO_EVENT:{ - return "publishServoEvent"; - } - case SERIAL_ATTACH:{ - return "serialAttach"; - } - case SERIAL_RELAY:{ - return "serialRelay"; - } - case PUBLISH_SERIAL_DATA:{ - return "publishSerialData"; - } - case ULTRASONIC_SENSOR_ATTACH:{ - return "ultrasonicSensorAttach"; - } - case ULTRASONIC_SENSOR_START_RANGING:{ - return "ultrasonicSensorStartRanging"; - } - case ULTRASONIC_SENSOR_STOP_RANGING:{ - return "ultrasonicSensorStopRanging"; - } - case PUBLISH_ULTRASONIC_SENSOR_DATA:{ - return "publishUltrasonicSensorData"; - } - case SET_AREF:{ - return "setAref"; - } - case MOTOR_ATTACH:{ - return "motorAttach"; - } - case MOTOR_MOVE:{ - return "motorMove"; - } - case MOTOR_MOVE_TO:{ - return "motorMoveTo"; - } - case ENCODER_ATTACH:{ - return "encoderAttach"; - } - case SET_ZERO_POINT:{ - return "setZeroPoint"; - } - case PUBLISH_ENCODER_DATA:{ - return "publishEncoderData"; - } - case PUBLISH_MRL_COMM_BEGIN:{ - return "publishMrlCommBegin"; - } - case SERVO_STOP:{ - return "servoStop"; - } + case PUBLISH_MRLCOMM_ERROR:{ + return "publishMRLCommError"; + } + case GET_BOARD_INFO:{ + return "getBoardInfo"; + } + case PUBLISH_BOARD_INFO:{ + return "publishBoardInfo"; + } + case ENABLE_PIN:{ + return "enablePin"; + } + case SET_DEBUG:{ + return "setDebug"; + } + case SET_SERIAL_RATE:{ + return "setSerialRate"; + } + case SOFT_RESET:{ + return "softReset"; + } + case ENABLE_ACK:{ + return "enableAck"; + } + case PUBLISH_ACK:{ + return "publishAck"; + } + case ECHO:{ + return "echo"; + } + case PUBLISH_ECHO:{ + return "publishEcho"; + } + case CUSTOM_MSG:{ + return "customMsg"; + } + case PUBLISH_CUSTOM_MSG:{ + return "publishCustomMsg"; + } + case DEVICE_DETACH:{ + return "deviceDetach"; + } + case I2C_BUS_ATTACH:{ + return "i2cBusAttach"; + } + case I2C_READ:{ + return "i2cRead"; + } + case I2C_WRITE:{ + return "i2cWrite"; + } + case I2C_WRITE_READ:{ + return "i2cWriteRead"; + } + case PUBLISH_I2C_DATA:{ + return "publishI2cData"; + } + case NEO_PIXEL_ATTACH:{ + return "neoPixelAttach"; + } + case NEO_PIXEL_SET_ANIMATION:{ + return "neoPixelSetAnimation"; + } + case NEO_PIXEL_WRITE_MATRIX:{ + return "neoPixelWriteMatrix"; + } + case NEO_PIXEL_FILL:{ + return "neoPixelFill"; + } + case NEO_PIXEL_SET_BRIGHTNESS:{ + return "neoPixelSetBrightness"; + } + case NEO_PIXEL_CLEAR:{ + return "neoPixelClear"; + } + case ANALOG_WRITE:{ + return "analogWrite"; + } + case DIGITAL_WRITE:{ + return "digitalWrite"; + } + case DISABLE_PIN:{ + return "disablePin"; + } + case DISABLE_PINS:{ + return "disablePins"; + } + case PIN_MODE:{ + return "pinMode"; + } + case PUBLISH_DEBUG:{ + return "publishDebug"; + } + case PUBLISH_PIN_ARRAY:{ + return "publishPinArray"; + } + case SET_TRIGGER:{ + return "setTrigger"; + } + case SET_DEBOUNCE:{ + return "setDebounce"; + } + case SERVO_ATTACH:{ + return "servoAttach"; + } + case SERVO_ATTACH_PIN:{ + return "servoAttachPin"; + } + case SERVO_DETACH_PIN:{ + return "servoDetachPin"; + } + case SERVO_SET_VELOCITY:{ + return "servoSetVelocity"; + } + case SERVO_SWEEP_START:{ + return "servoSweepStart"; + } + case SERVO_SWEEP_STOP:{ + return "servoSweepStop"; + } + case SERVO_MOVE_TO_MICROSECONDS:{ + return "servoMoveToMicroseconds"; + } + case SERVO_SET_ACCELERATION:{ + return "servoSetAcceleration"; + } + case PUBLISH_SERVO_EVENT:{ + return "publishServoEvent"; + } + case SERIAL_ATTACH:{ + return "serialAttach"; + } + case SERIAL_RELAY:{ + return "serialRelay"; + } + case PUBLISH_SERIAL_DATA:{ + return "publishSerialData"; + } + case ULTRASONIC_SENSOR_ATTACH:{ + return "ultrasonicSensorAttach"; + } + case ULTRASONIC_SENSOR_START_RANGING:{ + return "ultrasonicSensorStartRanging"; + } + case ULTRASONIC_SENSOR_STOP_RANGING:{ + return "ultrasonicSensorStopRanging"; + } + case PUBLISH_ULTRASONIC_SENSOR_DATA:{ + return "publishUltrasonicSensorData"; + } + case SET_AREF:{ + return "setAref"; + } + case MOTOR_ATTACH:{ + return "motorAttach"; + } + case MOTOR_MOVE:{ + return "motorMove"; + } + case MOTOR_MOVE_TO:{ + return "motorMoveTo"; + } + case ENCODER_ATTACH:{ + return "encoderAttach"; + } + case SET_ZERO_POINT:{ + return "setZeroPoint"; + } + case PUBLISH_ENCODER_DATA:{ + return "publishEncoderData"; + } + case PUBLISH_MRL_COMM_BEGIN:{ + return "publishMrlCommBegin"; + } + case SERVO_STOP:{ + return "servoStop"; + } default: { return "ERROR UNKNOWN METHOD (" + Integer.toString(method) + ")"; @@ -2486,7 +2489,9 @@ public void onBytes(byte[] bytes) { msgSize = 0; Arrays.fill(ioCmd, 0); // FIXME - optimize - remove // warn(String.format("Arduino->MRL error - bad magic number %d - %d rx errors", newByte, ++errorServiceToHardwareRxCnt)); - log.warn("Arduino->MRL error - bad magic number {} - {} rx errors", newByte, ++errorServiceToHardwareRxCnt); + if (!arduino.isConnecting()){ + log.warn("Arduino->MRL error - bad magic number {} - {} rx errors", newByte, ++errorServiceToHardwareRxCnt); + } } continue; } else if (byteCount.get() == 2) { @@ -2519,7 +2524,10 @@ public void onBytes(byte[] bytes) { } if (!clearToSend) { - log.warn("NOT CLEAR TO SEND! resetting parser!"); + if (!arduino.isConnecting()) { + // we're connecting, so we're going to ignore the message. + log.warn("NOT CLEAR TO SEND! resetting parser!"); + } // We opened the port, and we got some data that isn't a Begin message. // so, I think we need to reset the parser and continue processing bytes... // there will be errors until the next magic byte is seen. @@ -2716,46 +2724,46 @@ record = null; public static String deviceTypeToString(int typeId) { switch(typeId){ - case 0 : { - return "unknown"; - - } - case 1 : { - return "Arduino"; - - } - case 2 : { - return "UltrasonicSensor"; - - } - case 3 : { - return "Stepper"; - - } - case 4 : { - return "Motor"; - - } - case 5 : { - return "Servo"; - - } - case 6 : { - return "Serial"; - - } - case 7 : { - return "I2c"; - - } - case 8 : { - return "NeoPixel"; - - } - case 9 : { - return "Encoder"; - - } + case 0 : { + return "unknown"; + + } + case 1 : { + return "Arduino"; + + } + case 2 : { + return "UltrasonicSensor"; + + } + case 3 : { + return "Stepper"; + + } + case 4 : { + return "Motor"; + + } + case 5 : { + return "Servo"; + + } + case 6 : { + return "Serial"; + + } + case 7 : { + return "I2c"; + + } + case 8 : { + return "NeoPixel"; + + } + case 9 : { + return "Encoder"; + + } default: { return "unknown"; diff --git a/src/main/java/org/myrobotlab/arduino/virtual/MrlComm.java b/src/main/java/org/myrobotlab/arduino/virtual/MrlComm.java index 2f5e4b9000..1df204ecc3 100644 --- a/src/main/java/org/myrobotlab/arduino/virtual/MrlComm.java +++ b/src/main/java/org/myrobotlab/arduino/virtual/MrlComm.java @@ -1018,4 +1018,9 @@ public void neoPixelSetBrightness(Integer deviceId, Integer brightness) { } + public boolean isConnecting() { + // TODO Auto-generated method stub + return false; + } + } diff --git a/src/main/java/org/myrobotlab/service/Arduino.java b/src/main/java/org/myrobotlab/service/Arduino.java index bff3e027ac..f06216e55f 100644 --- a/src/main/java/org/myrobotlab/service/Arduino.java +++ b/src/main/java/org/myrobotlab/service/Arduino.java @@ -129,6 +129,12 @@ public static class I2CDeviceMap { boolean boardInfoEnabled = true; private long boardInfoRequestTs; + + /** + * connecting is true while the arduino is in the process of connecting + * to a port + */ + protected boolean connecting = false; @Deprecated /* * should develop a MrlSerial on Arduinos and @@ -535,7 +541,7 @@ public VirtualArduino getVirtual() { */ @Override public void connect(String port, int rate, int databits, int stopbits, int parity) { - + connecting = true; if (port == null) { warn("%s attempted to connect with a null port", getName()); return; @@ -596,6 +602,7 @@ public void connect(String port, int rate, int databits, int stopbits, int parit sleep(30); } + connecting = false; log.info("waited {} ms for Arduino {} to say hello", System.currentTimeMillis() - startBoardRequestTs, getName()); // we might be connected now @@ -1341,6 +1348,10 @@ public boolean isAttached(Attachable service) { public boolean isAttached(String name) { return deviceList.containsKey(name); } + + public boolean isConnecting() { + return connecting; + } @Override public boolean isConnected() { @@ -1595,13 +1606,16 @@ public BoardInfo publishBoardInfo(Integer version/* byte */, boardInfo = new BoardInfo(version, boardTypeId, boardTypeName, microsPerLoop, sram, activePins, arrayToDeviceSummary(deviceSummary), boardInfoRequestTs); + // connected now + connecting = false; + boardInfoRequestTs = System.currentTimeMillis(); log.debug("Version return by Arduino: {}", boardInfo.getVersion()); log.debug("Board type currently set: {} => {}", boardTypeId, boardTypeName); if (lastBoardInfo == null || !lastBoardInfo.getBoardTypeName().equals(board)) { - log.warn("setting board to type {}", board); + log.info("setting board to type {}", boardInfo.getBoardTypeName()); this.board = boardInfo.getBoardTypeName(); // we don't invoke, because // it might get into a race condition diff --git a/src/main/java/org/myrobotlab/service/interfaces/MrlCommPublisher.java b/src/main/java/org/myrobotlab/service/interfaces/MrlCommPublisher.java index 563f1e007a..4a4ebe49d2 100755 --- a/src/main/java/org/myrobotlab/service/interfaces/MrlCommPublisher.java +++ b/src/main/java/org/myrobotlab/service/interfaces/MrlCommPublisher.java @@ -41,7 +41,9 @@ public BoardInfo publishBoardInfo(Integer version/* byte */, public String publishMRLCommError(String errorMsg); public PinData[] publishPinArray(int[] data); - + + public boolean isConnecting(); + public String getName(); public Integer publishUltrasonicSensorData(Integer deviceId, Integer echoTime); diff --git a/src/main/resources/resource/Arduino/MrlComm/MrlNeopixel.cpp b/src/main/resources/resource/Arduino/MrlComm/MrlNeopixel.cpp index 636d7c3add..4371f6a1f1 100755 --- a/src/main/resources/resource/Arduino/MrlComm/MrlNeopixel.cpp +++ b/src/main/resources/resource/Arduino/MrlComm/MrlNeopixel.cpp @@ -176,6 +176,8 @@ void MrlNeopixel::setAnimation(byte animation, byte red, byte green, byte blue, { animationIndex = animation; x = 0; + y = 0; + z = 0; color = Adafruit_NeoPixel::Color(red, green, blue, white); wait = wait_ms; if (animation == 0) diff --git a/src/main/resources/resource/Arduino/generate/Msg.java.template b/src/main/resources/resource/Arduino/generate/Msg.java.template index dd6ba668b5..a2e48567bc 100644 --- a/src/main/resources/resource/Arduino/generate/Msg.java.template +++ b/src/main/resources/resource/Arduino/generate/Msg.java.template @@ -234,7 +234,9 @@ public class %javaClass% { msgSize = 0; Arrays.fill(ioCmd, 0); // FIXME - optimize - remove // warn(String.format("Arduino->MRL error - bad magic number %d - %d rx errors", newByte, ++errorServiceToHardwareRxCnt)); - log.warn("Arduino->MRL error - bad magic number {} - {} rx errors", newByte, ++errorServiceToHardwareRxCnt); + if (!arduino.isConnecting()){ + log.warn("Arduino->MRL error - bad magic number {} - {} rx errors", newByte, ++errorServiceToHardwareRxCnt); + } } continue; } else if (byteCount.get() == 2) { @@ -267,7 +269,10 @@ public class %javaClass% { } if (!clearToSend) { - log.warn("NOT CLEAR TO SEND! resetting parser!"); + if (!arduino.isConnecting()) { + // we're connecting, so we're going to ignore the message. + log.warn("NOT CLEAR TO SEND! resetting parser!"); + } // We opened the port, and we got some data that isn't a Begin message. // so, I think we need to reset the parser and continue processing bytes... // there will be errors until the next magic byte is seen. @@ -482,7 +487,7 @@ public class %javaClass% { } public void waitForAck(){ - if (!ackEnabled) { + if (!ackEnabled || serial == null || !serial.isConnected()) { return; } // if there's a pending message, we need to wait for the ack to be received. diff --git a/src/test/java/org/myrobotlab/arduino/MrlCommDirectTest.java b/src/test/java/org/myrobotlab/arduino/MrlCommDirectTest.java index 7bb84fdc91..4fb3e9a2e4 100755 --- a/src/test/java/org/myrobotlab/arduino/MrlCommDirectTest.java +++ b/src/test/java/org/myrobotlab/arduino/MrlCommDirectTest.java @@ -366,4 +366,10 @@ public Object invoke(String method, Object... params) { return null; } + @Override + public boolean isConnecting() { + // TODO Auto-generated method stub + return false; + } + } diff --git a/src/test/java/org/myrobotlab/service/VirtualArduinoTest.java b/src/test/java/org/myrobotlab/service/VirtualArduinoTest.java index bac8388134..ba1d028096 100755 --- a/src/test/java/org/myrobotlab/service/VirtualArduinoTest.java +++ b/src/test/java/org/myrobotlab/service/VirtualArduinoTest.java @@ -223,4 +223,10 @@ public Object invoke(String method, Object... params) { return null; } + @Override + public boolean isConnecting() { + // TODO Auto-generated method stub + return false; + } + }