Merge pull request #213 from Open-STEM/FranksDev

fgrossman · web-flow · commit dee7b196c58a · 2026-02-09T14:33:53.000-05:00
Handle XPP over USB Serial
diff --git a/public/lib/XRPLib/dashboard.py b/public/lib/XRPLib/dashboard.py
@@ -83,10 +83,10 @@ def __init__(self):
         self.rangefinder = Rangefinder.get_default_rangefinder()
         self.reflectance = Reflectance.get_default_reflectance()
         self.VoltageADC = ADC(Pin('BOARD_VIN_MEASURE'))
-        self.CurrLADC = ADC(Pin('ML_CUR'))
-        self.CurrRADC = ADC(Pin('MR_CUR'))
-        self.Curr3ADC = ADC(Pin('M3_CUR'))
-        self.Curr4ADC = ADC(Pin('M4_CUR'))
+        #self.CurrLADC = ADC(Pin('ML_CUR'))
+        #self.CurrRADC = ADC(Pin('MR_CUR'))
+        #self.Curr3ADC = ADC(Pin('M3_CUR'))
+        #self.Curr4ADC = ADC(Pin('M4_CUR'))
 
         # Get XPP instance
         self._puppet = Puppet.get_default_puppet()
@@ -177,10 +177,10 @@ def _dashboard_update(self):
         self._puppet.set_variable('$encoder.4', self.motor_four.get_position_counts())
         
         # Current sensor data
-        self._puppet.set_variable('$current.left', self.CurrLADC.read_u16())
-        self._puppet.set_variable('$current.right', self.CurrRADC.read_u16())
-        self._puppet.set_variable('$current.3', self.Curr3ADC.read_u16())
-        self._puppet.set_variable('$current.4', self.Curr4ADC.read_u16())
+        #self._puppet.set_variable('$current.left', self.CurrLADC.read_u16())
+        #self._puppet.set_variable('$current.right', self.CurrRADC.read_u16())
+        #self._puppet.set_variable('$current.3', self.Curr3ADC.read_u16())
+        #self._puppet.set_variable('$current.4', self.Curr4ADC.read_u16())
         
         # Other sensors
         self._puppet.set_variable('$rangefinder.distance', self.rangefinder.distance())
@@ -211,6 +211,7 @@ def start(self, rate_hz=3):
         period_ms = int(1000 / rate_hz)
         self.update_timer.init(period=period_ms, mode=Timer.PERIODIC, 
                                callback=lambda t: self._dashboard_update())
+        self._puppet.start()
 
     def stop(self):
         """
@@ -225,6 +226,7 @@ def stop(self):
         
         # Stop timer
         self.update_timer.deinit()
+        self._puppet.stop()
 
     def set_value(self, name, value, rate_hz=3):
         """
diff --git a/public/lib/XRPLib/gamepad.py b/public/lib/XRPLib/gamepad.py
@@ -88,7 +88,7 @@ def start(self):
         Signals the remote computer to begin sending gamepad data packets.
         Subscribes to all gamepad variables at a high rate (50 Hz).
         """
-        
+        self._puppet.start()
         self._puppet.send_program_start()
 
         # Enable gamepad - signal to client to start sending
@@ -120,7 +120,8 @@ def stop(self):
         # Unsubscribe from all gamepad variables
         for var_name in self._VAR_NAMES.values():
             self._puppet.subscribe_variable(var_name, 0)
-        
+            
+        self._puppet.stop()
         self._started = False
 
     def get_value(self, index: int) -> float:
diff --git a/public/lib/XRPLib/puppet.py b/public/lib/XRPLib/puppet.py
@@ -5,11 +5,12 @@
 Uses a Network Tables-like architecture with variable ID mapping.
 """
 
+import select
 import struct
 import sys
 import time
 from machine import Timer
-from micropython import const
+from micropython import const,kbd_intr
 
 # Message framing constants
 MSG_START_1 = const(0xAA)
@@ -89,7 +90,7 @@
 
 class Puppet:
     """
-    Core XRP Puppet Protocol implementation. 
+    Core XRP Puppet Protocol implementation.
     Manages bidirectional communication, variable registry, and message handling.
     """
     
@@ -129,6 +130,11 @@ def __init__(self):
         self._update_timer = Timer(-1)
         self._update_timer_running = False
         
+        # STDIO polling (for USB_STDIO transport)
+        self._poll_timer = Timer(-1)
+        self._poll_timer_running = False
+        self._poll_stdin_poll = None
+        
         # Program state
         self._program_running = False
         
@@ -142,46 +148,86 @@ def _init_transport(self):
         # Try BLE first
         try:
             from ble.blerepl import uart
-            self._transport = uart
-            self._transport_type = 'BLE'
-            self._transport.set_data_callback(self._data_callback)
-            return
-        except (ImportError, AttributeError):
-            pass
-        
-        # Fallback to USB serial
-        try:
-            # For USB serial, we'll use sys.stdin/sys.stdout or machine.UART
-            # Check if we can use UART
-            try:
-                from machine import UART
-                # Try to open UART for USB serial (typically UART(0))
-                self._transport = UART(0, baudrate=115200)
-                self._transport_type = 'USB'
-                # For USB serial, we'll need to poll in a timer
-                self._usb_poll_timer = Timer(-2)
-                self._usb_poll_timer.init(period=10, mode=Timer.PERIODIC, 
-                                         callback=lambda t: self._poll_usb())
+            if len(uart._connections) > 0:
+                self._transport = uart
+                self._transport_type = 'BLE'
+                self._transport.set_data_callback(self._data_callback)
                 return
-            except:
-                # Last resort: use sys.stdin/stdout
-                self._transport = sys
+            else:
                 self._transport_type = 'USB_STDIO'
+                self._start_poll_timer()
                 return
-        except:
-            pass
-        
-        raise RuntimeError("No transport available (BLE or USB serial)")
-    
-    def _poll_usb(self):
+        except ImportError:
+            self._transport_type = 'USB_STDIO'
+            self._start_poll_timer()
+            return
+       
+
+    def _poll_stdio(self):
         """
-        Poll USB serial for incoming data.
+        Poll STDIO for incoming data using select.poll.
+        Reads raw bytes from the stdin buffer.
         """
-        if self._transport_type == 'USB' and self._transport.any():
-            data = self._transport.read(self._transport.any())
-            if data:
-                self._data_callback(data)
-    
+        data_read = False
+        while True:
+            events = self._stdin_poll.poll(0)
+            if not events:
+                if data_read:
+                    self._process_rx_buffer()
+                break
+            data = sys.stdin.buffer.read(1)
+            self._rx_buffer.extend(data)
+            data_read = True
+            
+            #if data:
+                #print(f"Received data: {data}")
+                #self._data_callback(data)
+
+    def _start_poll_timer(self):
+        """
+        Start STDIO polling for USB_STDIO transport.
+        Uses select.poll on the raw stdin buffer and a timer to check it.
+        """
+        if self._transport_type != 'USB_STDIO' or self._poll_timer_running:
+            return
+        self._stdin_poll = select.poll()
+        self._stdin_poll.register(sys.stdin.buffer, select.POLLIN)
+
+        kbd_intr(-1) #the data will have 03 in it, don't do a ctrl-c for that data.
+
+        self._poll_timer.init(period=20, mode=Timer.PERIODIC,
+                              callback=lambda t: self._poll_stdio())
+        self._poll_timer_running = True
+
+    def _stop_poll_timer(self):
+        """
+        Stop the STDIO polling timer and unregister from poll.
+        """
+        if self._poll_timer_running:
+            self._poll_timer.deinit()
+            kbd_intr(03) #start watching for ctrl-c again
+            self._poll_timer_running = False
+        if self._stdin_poll is not None:
+            try:
+                self._stdin_poll.unregister(sys.stdin.buffer)
+            except (OSError, AttributeError):
+                pass
+            self._stdin_poll = None
+    def start(self):
+        """
+        Start the STDIO polling.
+        """
+        if self._transport_type == 'USB_STDIO':
+            self._start_poll_timer()
+
+    def stop(self):
+        """
+        Stop the STDIO polling.
+        """
+        if self._transport_type == 'USB_STDIO':
+            self._stop_poll_timer()
+        
+
     def _data_callback(self, data):
         """
         Handle incoming data from transport layer.
@@ -209,11 +255,19 @@ def _process_rx_buffer(self):
                 if idx == -1:
                     # No start sequence found, clear buffer except last byte
                     if len(self._rx_buffer) > 1:
+                        #see if anything in the buffer is a ctrl-c
+                        for i in range(len(self._rx_buffer) - 1):
+                            if self._rx_buffer[i] == 0x03:
+                                self.stop() #stop the USB handler if that is what is running and that will reenable the ctrl-c handler
                         self._rx_buffer = self._rx_buffer[-1:]
                     return
                 
                 # Remove everything before start sequence
                 if idx > 0:
+                    #check for a ctrl-c in the non packet parts
+                    for i in range(idx - 1):
+                        if self._rx_buffer[i] == 0x03:
+                            self.stop() #stop the USB handler if that is what is running and that will reenable the ctrl-c handler
                     self._rx_buffer = self._rx_buffer[idx:]
                 
                 # Found start, move to length
@@ -275,11 +329,9 @@ def _write_data(self, data):
         """
         if self._transport_type == 'BLE':
             self._transport.write_data(data)
-        elif self._transport_type == 'USB':
-            self._transport.write(data)
+        
         elif self._transport_type == 'USB_STDIO':
             sys.stdout.buffer.write(data)
-            sys.stdout.buffer.flush()
     
     def _pack_message(self, msg_type, payload_data):
         """
diff --git a/public/lib/XRPLib/resetbot.py b/public/lib/XRPLib/resetbot.py
@@ -27,6 +27,11 @@ def reset_servos():
     # Turn off both Servos
     Servo.get_default_servo(1).free()
     Servo.get_default_servo(2).free()
+    try: #if 2350 then there are 4 servos
+        Servo.get_default_servo(3).free()
+        Servo.get_default_servo(4).free()
+    except:
+        pass
 
 def reset_webserver():
     from XRPLib.webserver import Webserver
diff --git a/public/lib/package.json b/public/lib/package.json
@@ -32,5 +32,5 @@
     "deps": [
       ["github:pimoroni/phew", "latest"]
     ],
-    "version": "2.2.0"
+    "version": "2.2.1"
   }
diff --git a/src/connections/bluetoothconnection.ts b/src/connections/bluetoothconnection.ts
@@ -39,11 +39,6 @@ export class BluetoothConnection extends Connection {
 
     private  Table: TableMgr | undefined = undefined;
 
-    // XPP Protocol constants for packet detection
-    private readonly XPP_START_SEQ: number[] = [0xAA, 0x55];
-    private readonly XPP_END_SEQ: number[] = [0x55, 0xAA];
-    private xppBuffer: Uint8Array = new Uint8Array(0); // Buffer for incomplete XPP packets
-
     constructor(connMgr: ConnectionMgr) {
         super();
         this.connMgr = connMgr;
@@ -181,72 +176,6 @@ export class BluetoothConnection extends Connection {
     }
 
 
-    /**
-     * Extracts complete XPP packets from the buffer and returns them.
-     * XPP packet format: [0xAA 0x55] [Type] [Length] [Payload] [0x55 0xAA]
-     * @param data - New data to add to the buffer
-     * @returns Array of complete XPP packets
-     */
-    private extractCompleteXPPPackets(data: Uint8Array): Uint8Array[] {
-        // Add new data to buffer
-        this.xppBuffer = this.concatUint8Arrays(this.xppBuffer, data);
-        
-        const completePackets: Uint8Array[] = [];
-        
-        while (this.xppBuffer.length >= 4) { // Minimum packet size is 4 bytes (start + type + length + end)
-            // Look for start sequence [0xAA 0x55]
-            const startIndex = this.xppBuffer.findIndex((val, idx) => 
-                idx < this.xppBuffer.length - 1 && 
-                val === this.XPP_START_SEQ[0] && 
-                this.xppBuffer[idx + 1] === this.XPP_START_SEQ[1]
-            );
-            
-            if (startIndex === -1) {
-                // No start sequence found, clear buffer
-                this.xppBuffer = new Uint8Array(0);
-                break;
-            }
-            
-            // Remove any data before the start sequence
-            if (startIndex > 0) {
-                this.xppBuffer = this.xppBuffer.subarray(startIndex);
-            }
-            
-            // Check if we have enough data for type and length (at least 4 bytes: start + type + length)
-            if (this.xppBuffer.length < 4) {
-                break; // Need more data
-            }
-            
-            // Extract length (byte 3 after start sequence)
-            // Type is at byte 2, but we don't need it for packet extraction
-            const payloadLength = this.xppBuffer[3];
-            
-            // Calculate total packet size: start(2) + type(1) + length(1) + payload + end(2)
-            const totalPacketSize = 2 + 1 + 1 + payloadLength + 2;
-            
-            // Check if we have the complete packet
-            if (this.xppBuffer.length < totalPacketSize) {
-                break; // Need more data
-            }
-            
-            // Verify end sequence
-            const endIndex = totalPacketSize - 2;
-            if (this.xppBuffer[endIndex] === this.XPP_END_SEQ[0] && 
-                this.xppBuffer[endIndex + 1] === this.XPP_END_SEQ[1]) {
-                // Complete packet found
-                const completePacket = this.xppBuffer.subarray(0, totalPacketSize);
-                completePackets.push(completePacket);
-                
-                // Remove processed packet from buffer
-                this.xppBuffer = this.xppBuffer.subarray(totalPacketSize);
-            } else {
-                // Invalid end sequence, skip this start sequence and continue
-                this.xppBuffer = this.xppBuffer.subarray(2);
-            }
-        }
-        
-        return completePackets;
-    }
 
     /**
      * readWorker - this worker read data from the XRP robot
@@ -266,11 +195,10 @@ export class BluetoothConnection extends Connection {
                         valuesD = await this.get2BLEData();
                         if(valuesD != undefined) {
                             // Extract complete XPP packets and only process those
-                            const completePackets = this.extractCompleteXPPPackets(valuesD);
-                            for (const packet of completePackets) {
-                                this.joyStick?.handleXPPMessage(packet);
-                                this.Table?.readFromDevice(packet);
-                                //NOTE: if there is one more of these then we should switch to a subscription model.
+                            // Note: regularData is ignored since bleDataReader only receives XPP packets
+                            const { packets } = this.extractCompleteXPPPackets(valuesD);
+                            for (const packet of packets) {
+                                this.processXPPPacket(packet, this.Table);
                             }
                         }
                     }
diff --git a/src/connections/connection.ts b/src/connections/connection.ts
diff --git a/src/connections/usbconnection.ts b/src/connections/usbconnection.ts

Original file line number	Diff line number	Diff line change
`@@ -32,5 +32,5 @@`
`32`	`32`	`"deps": [`
`33`	`33`	`["github:pimoroni/phew", "latest"]`
`34`	`34`	`],`
`35`		`- "version": "2.2.0"`
	`35`	`+ "version": "2.2.1"`
`36`	`36`	`}`