clean up protocol

microsoft · Sep 20, 2023 · dcb90a4 · dcb90a4
1 parent 13b415c
commit dcb90a4
Show file tree

Hide file tree

Showing 2 changed files with 55 additions and 13 deletions.
diff --git 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 <RobotMessage>{
+            messageId: messageId,
             cmd: cmd,
             payload: payload
         }

diff --git 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) {