diff --git a/robot/protocol.ts b/robot/protocol.ts index aa402c9f..a91f8a84 100644 --- a/robot/protocol.ts +++ b/robot/protocol.ts @@ -9,36 +9,69 @@ namespace microcode { * Runs the robot forward and backward. * speed: i16 % */ - MotorRun = 0x0001, + MotorRun = 0x01, /** * Turn the robot left and right. Right is positive values. * speed: i16 % */ - MotorTurn = 0x0002, - LedSetColor = 0x0010, + MotorTurn = 0x02, + /** + * Sets the robot LED to a 24bit RGB color + * rgb: u24 + */ + LedSetColor = 0x03, + } + + export interface RobotMessage { + /** + * message identifier to drop repeated messages; u8 + */ + messageId: number + /** + * Robot command + */ + cmd: RobotCommand + /** + * Command payload + */ + payload: Buffer } /** * Encodes the command and payload into a buffer that can be sent via radio + * + * 0 magic, u16 + * 2 message id, u8 + * 3 cmd, u8 + * 4 payload, u8[] */ - export function createRobotCommand(cmd: RobotCommand, payload: Buffer) { - const buf = pins.createBuffer(4 + payload.length) + export function encodeRobotMessage(msg: RobotMessage) { + const payload = msg.payload + const messageid = msg.messageId + const cmd = msg.cmd + + const buf = pins.createBuffer(6 + payload.length) buf.setNumber(NumberFormat.UInt16LE, 0, MESSAGE_MAGIC) - buf.setNumber(NumberFormat.UInt16LE, 2, cmd) + buf.setNumber(NumberFormat.UInt8LE, 2, messageid) + buf.setNumber(NumberFormat.UInt8LE, 3, cmd) buf.write(4, payload) - console.log(buf.toHex()) return buf } /** * Decodes message buffer */ - export function readRobotCommand(msg: Buffer): { cmd: RobotCommand, payload: Buffer } { + export function decodeRobotCommand(msg: Buffer): RobotMessage { + if (!msg || msg.length < 4) return undefined + const magic = msg.getNumber(NumberFormat.UInt16LE, 0) - if (magic !== MESSAGE_MAGIC) return undefined; // ignore - const cmd = msg.getNumber(NumberFormat.UInt16LE, 2) + if (magic !== MESSAGE_MAGIC) return undefined + + const messageId = msg.getNumber(NumberFormat.UInt8LE, 2) + const cmd = msg.getNumber(NumberFormat.UInt8LE, 3) const payload = msg.slice(4) - return { + return { + messageId: messageId, cmd: cmd, payload: payload } diff --git a/robot/radio.ts b/robot/radio.ts index 90a790ae..faae4e57 100644 --- a/robot/radio.ts +++ b/robot/radio.ts @@ -27,15 +27,24 @@ namespace microcode { whaleysans.showNumber(group) } + let nextMessageId = 0 export function sendCommand(cmd: RobotCommand, payload: Buffer) { - const buf = createRobotCommand(cmd, payload) + nextMessageId = (nextMessageId + 1) % 0xff + const buf = encodeRobotMessage({ messageId: nextMessageId, cmd, payload }) radio.sendBuffer(buf) } // handle radio package messages + let lastReceivedMessageId: number = undefined radio.onReceivedBuffer(buf => { - const msg = readRobotCommand(buf) + const msg = decodeRobotCommand(buf) if (!msg) return + + const messageId = msg.messageId + if (lastReceivedMessageId === messageId) + return // duplicate + + // decode message const cmd = msg.cmd const payload = msg.payload switch (cmd) {