diff --git a/public/lib/XRPLib/dashboard.py b/public/lib/XRPLib/dashboard.py index 859413c..385e14e 100644 --- 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 index 6b2baa5..f3f08de 100644 --- 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 index 79fba08..b2788aa 100644 --- 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 index b85770d..5397560 100644 --- 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 index 0fe9a25..16a2007 100644 --- 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 index c8febe4..a072998 100644 --- 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 index 6ca0f59..60b74a8 100644 --- a/src/connections/connection.ts +++ b/src/connections/connection.ts @@ -1,6 +1,7 @@ import ConnectionMgr from "@/managers/connectionmgr"; import logger from "@/utils/logger"; import Joystick from '@/managers/joystickmgr'; +import TableMgr from '@/managers/tablemgr'; /** @@ -50,6 +51,11 @@ abstract class Connection { readonly XRP_SEND_BLOCK_SIZE: number = 250; // wired can handle 255 bytes, but BLE 5.0 is only 250 + // XPP Protocol constants for packet detection + protected readonly XPP_START_SEQ: number[] = [0xAA, 0x55]; + protected readonly XPP_END_SEQ: number[] = [0x55, 0xAA]; + protected xppBuffer: Uint8Array = new Uint8Array(0); // Buffer for incomplete XPP packets + joyStick: Joystick | undefined; constructor() { @@ -107,6 +113,162 @@ abstract class Connection { return result; } + /** + * 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 + * + * regularData anything that is not part of a XPP packet + * packets are the complete XPP packets + * anything before start sequence is regular data + * anything after an end with no other start sequence is regular data + * + */ + protected extractCompleteXPPPackets(data: Uint8Array): { packets: Uint8Array[], regularData: Uint8Array } { + // Add new data to buffer + this.xppBuffer = this.concatUint8Arrays(this.xppBuffer, data); + let regularData: Uint8Array = new Uint8Array(0); + + const completePackets: Uint8Array[] = []; + + // Process buffer to extract packets and regular data + while (this.xppBuffer.length > 0) { + // If buffer is too small to be a complete packet, check if we can pass through regular data + if (this.xppBuffer.length < 4) { + // Check if buffer could potentially start an XPP packet + if (this.xppBuffer.length === 1) { + // Single byte: if it's 0xAA, might be start of XPP, keep it + // Otherwise, it's definitely regular data + if (this.xppBuffer[0] !== this.XPP_START_SEQ[0]) { + regularData = this.concatUint8Arrays(regularData, this.xppBuffer); + this.xppBuffer = new Uint8Array(0); + } + break; // Need more data to determine + } else if (this.xppBuffer.length === 2) { + // Two bytes: check if it's [0xAA, 0x55] + if (this.xppBuffer[0] === this.XPP_START_SEQ[0] && + this.xppBuffer[1] === this.XPP_START_SEQ[1]) { + // Could be start of XPP packet, keep it + break; // Need more data + } else if (this.xppBuffer[0] === this.XPP_START_SEQ[0]) { + // First byte is 0xAA but second isn't 0x55, pass first byte as regular data + regularData = this.concatUint8Arrays(regularData, this.xppBuffer.subarray(0, 1)); + this.xppBuffer = this.xppBuffer.subarray(1); + // Continue processing the remaining byte + } else { + // First byte is not 0xAA, both bytes are regular data + regularData = this.concatUint8Arrays(regularData, this.xppBuffer); + this.xppBuffer = new Uint8Array(0); + break; + } + } else if (this.xppBuffer.length === 3) { + // Three bytes: check if it starts with [0xAA, 0x55] + if (this.xppBuffer[0] === this.XPP_START_SEQ[0] && + this.xppBuffer[1] === this.XPP_START_SEQ[1]) { + // Could be start of XPP packet, keep it + break; // Need more data + } else if (this.xppBuffer[0] === this.XPP_START_SEQ[0]) { + // First byte is 0xAA but second isn't 0x55, pass first byte as regular data + regularData = this.concatUint8Arrays(regularData, this.xppBuffer.subarray(0, 1)); + this.xppBuffer = this.xppBuffer.subarray(1); + // Continue processing the remaining 2 bytes + } else { + // First byte is not 0xAA, check if second byte is 0xAA (could be start of XPP) + if (this.xppBuffer[1] === this.XPP_START_SEQ[0]) { + // Second byte might be start, pass first byte as regular data + regularData = this.concatUint8Arrays(regularData, this.xppBuffer.subarray(0, 1)); + this.xppBuffer = this.xppBuffer.subarray(1); + // Continue processing the remaining 2 bytes + } else { + // No potential XPP start, pass all as regular data + regularData = this.concatUint8Arrays(regularData, this.xppBuffer); + this.xppBuffer = new Uint8Array(0); + break; + } + } + } + continue; // Re-check buffer after modifications + } + + // Buffer has at least 4 bytes, 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 + // Check if last byte is 0xAA (might be start of next XPP packet) + const lastByte = this.xppBuffer[this.xppBuffer.length - 1]; + if (lastByte === this.XPP_START_SEQ[0]) { + // Keep last byte in buffer, pass rest as regular data + regularData = this.concatUint8Arrays(regularData, this.xppBuffer.subarray(0, this.xppBuffer.length - 1)); + this.xppBuffer = this.xppBuffer.subarray(this.xppBuffer.length - 1); + } else { + // No potential XPP start, clear buffer + regularData = this.concatUint8Arrays(regularData, this.xppBuffer); + this.xppBuffer = new Uint8Array(0); + } + break; + } + + // Remove any data before the start sequence + if (startIndex > 0) { + regularData = this.concatUint8Arrays(regularData, this.xppBuffer.subarray(0, startIndex)); + 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 + regularData = this.concatUint8Arrays(regularData, this.xppBuffer.subarray(0, 2)); + this.xppBuffer = this.xppBuffer.subarray(2); + } + } + + return { packets: completePackets, regularData }; + } + + /** + * Processes a complete XPP packet by routing it to the appropriate handlers. + * @param packet - Complete XPP packet + * @param tableMgr - Optional TableMgr instance for handling table-related XPP messages + */ + protected processXPPPacket(packet: Uint8Array, tableMgr?: TableMgr): void { + this.joyStick?.handleXPPMessage(packet); + tableMgr?.readFromDevice(packet); + } + + + /** * readData - read data from the XRP connection * @param values diff --git a/src/connections/usbconnection.ts b/src/connections/usbconnection.ts index 1a49dc1..d98e7e0 100644 --- a/src/connections/usbconnection.ts +++ b/src/connections/usbconnection.ts @@ -3,6 +3,7 @@ import ConnectionMgr from '@/managers/connectionmgr'; import i18n from '@/utils/i18n'; import { ConnectionType } from '@/utils/types'; import Connection, { ConnectionState } from '@connections/connection'; +import TableMgr from '@/managers/tablemgr'; /** * USB Connection - establish USB serial connection to the XRP Robot @@ -12,6 +13,7 @@ export class USBConnection extends Connection { private port: SerialPort | undefined = undefined; private reader: ReadableStreamDefaultReader | undefined = undefined; // Reference to serial port reader, only one can be locked at a time private writer: WritableStreamDefaultWriter | undefined = undefined; // Reference to serial port writer, only one can be locked at a time + private Table: TableMgr | undefined = undefined; // Define USB connection constants readonly USB_VENDOR_ID_BETA: number = 11914; // For filtering ports during auto or manual selection @@ -23,6 +25,9 @@ export class USBConnection extends Connection { super(); this.connMgr = connMgr; this.isManualConnection = false; + this.Table = new TableMgr(); + if(this.joyStick) + this.joyStick.writeToDevice = this.writeToDevice.bind(this); // setup USB connection listeners // Check if browser can use WebSerial @@ -85,7 +90,19 @@ export class USBConnection extends Connection { this.reader.releaseLock(); break; } - this.readData(value); + + // Extract XPP packets and regular data from the incoming stream + const { packets, regularData } = this.extractCompleteXPPPackets(value); + + // Process complete XPP packets + for (const packet of packets) { + this.processXPPPacket(packet, this.Table); + } + + // Pass any regular (non-XPP) data to readData for normal processing + if (regularData.length > 0) { + this.readData(regularData); + } } } // eslint-disable-next-line @typescript-eslint/no-explicit-any