diff --git a/XRPLib/board.py b/XRPLib/board.py index eb17e4f..1a17ead 100644 --- a/XRPLib/board.py +++ b/XRPLib/board.py @@ -30,6 +30,7 @@ def __init__(self, vin_pin="BOARD_VIN_MEASURE", button_pin="BOARD_USER_BUTTON", self.on_switch = ADC(Pin(vin_pin)) self.button = Pin(button_pin, Pin.IN, Pin.PULL_UP) + time.sleep(.01) # give some time for the pull up to get to the proper voltage, otherwise the button could read as pressed self.led = Pin(led_pin, Pin.OUT) diff --git a/XRPLib/dashboard.py b/XRPLib/dashboard.py new file mode 100644 index 0000000..385e14e --- /dev/null +++ b/XRPLib/dashboard.py @@ -0,0 +1,257 @@ +from .encoded_motor import EncodedMotor +from .rangefinder import Rangefinder +from .imu import IMU +from .reflectance import Reflectance +from .puppet import Puppet, VAR_TYPE_INT, VAR_TYPE_FLOAT, PERM_READ_ONLY + +from machine import Timer, ADC, Pin +from micropython import const +import time + + +class Dashboard: + + _DEFAULT_DASHBOARD_INSTANCE = None + + # Variable type constants + VAR_TYPE_FLOAT = const(2) + + # Permission constants + PERM_READ_ONLY = const(1) + PERM_WRITE_ONLY = const(2) + + # Backward compatibility constants + YAW = const(0) + ROLL = const(1) + PTICH = const(2) + ACCX = const(3) + ACCY = const(4) + ACCZ = const(5) + ENCL = const(6) + ENCR = const(7) + ENC3 = const(8) + ENC4 = const(9) + CURRR = const(10) + CURRL = const(11) + CURR3 = const(12) + CURR4 = const(13) + DIST = const(14) + REFL = const(15) + REFR = const(16) + VOLTAGE = const(17) + + # Mapping from old index to XPP variable names + _VAR_NAMES = { + YAW: '$imu.yaw', + ROLL: '$imu.roll', + PTICH: '$imu.pitch', + ACCX: '$imu.acc_x', + ACCY: '$imu.acc_y', + ACCZ: '$imu.acc_z', + ENCL: '$encoder.left', + ENCR: '$encoder.right', + ENC3: '$encoder.3', + ENC4: '$encoder.4', + CURRL: '$current.left', + CURRR: '$current.right', + CURR3: '$current.3', + CURR4: '$current.4', + DIST: '$rangefinder.distance', + REFL: '$reflectance.left', + REFR: '$reflectance.right', + VOLTAGE: '$voltage', + } + + @classmethod + def get_default_dashboard(cls): + """ + Get the default XRP dashboard instance. This is a singleton, so only one instance of the dashboard sensor will ever exist. + """ + if cls._DEFAULT_DASHBOARD_INSTANCE is None: + cls._DEFAULT_DASHBOARD_INSTANCE = cls() + return cls._DEFAULT_DASHBOARD_INSTANCE + + def __init__(self): + """ + Manages communication with dashboard data going to a remote computer via XPP protocol. + """ + self.left_motor = EncodedMotor.get_default_encoded_motor(index=1) + self.right_motor = EncodedMotor.get_default_encoded_motor(index=2) + self.motor_three = EncodedMotor.get_default_encoded_motor(index=3) + self.motor_four = EncodedMotor.get_default_encoded_motor(index=4) + self.imu = IMU.get_default_imu() + 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')) + + # Get XPP instance + self._puppet = Puppet.get_default_puppet() + + # Register all sensor variables + self._register_variables() + + # Create timer for periodic updates + self.update_timer = Timer(-1) + self._update_rate = 3 # Default 3 Hz + + def _register_variables(self): + """ + Register all sensor variables with XPP. + """ + # IMU variables (float) + for var_name in ['$imu.yaw', '$imu.roll', '$imu.pitch', + '$imu.acc_x', '$imu.acc_y', '$imu.acc_z']: + self._puppet.define_variable(var_name, VAR_TYPE_FLOAT, PERM_READ_ONLY) + + # Encoder variables (int) + for var_name in ['$encoder.left', '$encoder.right', '$encoder.3', '$encoder.4']: + self._puppet.define_variable(var_name, VAR_TYPE_INT, PERM_READ_ONLY) + + # Current sensor variables (int) + for var_name in ['$current.left', '$current.right', '$current.3', '$current.4']: + self._puppet.define_variable(var_name, VAR_TYPE_INT, PERM_READ_ONLY) + + # Other sensor variables (float) + for var_name in ['$rangefinder.distance', '$reflectance.left', + '$reflectance.right', '$voltage']: + self._puppet.define_variable(var_name, VAR_TYPE_FLOAT, PERM_READ_ONLY) + + def sendIntValue(self, index, value): + """ + Send an integer value (backward compatibility method). + Now uses XPP protocol. + + :param index: Variable index constant + :type index: int + :param value: Integer value to send + :type value: int + """ + if index not in self._VAR_NAMES: + return + + var_name = self._VAR_NAMES[index] + try: + self._puppet.set_variable(var_name, value) + except: + pass # Variable might not be registered yet + + def sendFloatValue(self, index, value): + """ + Send a float value (backward compatibility method). + Now uses XPP protocol. + + :param index: Variable index constant + :type index: int + :param value: Float value to send + :type value: float + """ + if index not in self._VAR_NAMES: + return + + var_name = self._VAR_NAMES[index] + try: + self._puppet.set_variable(var_name, value) + except: + pass # Variable might not be registered yet + + def _dashboard_update(self): + """ + Update all sensor variables with current readings. + """ + # IMU data + self._puppet.set_variable('$imu.yaw', self.imu.get_yaw()) + self._puppet.set_variable('$imu.roll', self.imu.get_roll()) + self._puppet.set_variable('$imu.pitch', self.imu.get_pitch()) + self._puppet.set_variable('$imu.acc_x', self.imu.get_acc_x()) + self._puppet.set_variable('$imu.acc_y', self.imu.get_acc_y()) + self._puppet.set_variable('$imu.acc_z', self.imu.get_acc_z()) + + # Encoder data + self._puppet.set_variable('$encoder.left', self.left_motor.get_position_counts()) + self._puppet.set_variable('$encoder.right', self.right_motor.get_position_counts()) + self._puppet.set_variable('$encoder.3', self.motor_three.get_position_counts()) + 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()) + + # Other sensors + self._puppet.set_variable('$rangefinder.distance', self.rangefinder.distance()) + self._puppet.set_variable('$reflectance.left', self.reflectance.get_left()) + self._puppet.set_variable('$reflectance.right', self.reflectance.get_right()) + + # Voltage + voltage = self.VoltageADC.read_u16() / (1024*64/14) + self._puppet.set_variable('$voltage', voltage) + + def start(self, rate_hz=3): + """ + Start sending dashboard packets to the remote computer at the specified rate. + + :param rate_hz: Update rate in Hz (default: 3) + :type rate_hz: int + """ + self._update_rate = rate_hz + + # Subscribe all variables at the specified rate + for var_name in self._VAR_NAMES.values(): + try: + self._puppet.subscribe_variable(var_name, rate_hz) + except: + pass # Variable might not be registered yet + + # Also use timer for backward compatibility + 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): + """ + Stop sending dashboard data packets. + """ + # Unsubscribe from all variables + for var_name in self._VAR_NAMES.values(): + try: + self._puppet.subscribe_variable(var_name, 0) + except: + pass + + # Stop timer + self.update_timer.deinit() + self._puppet.stop() + + def set_value(self, name, value, rate_hz=3): + """ + Define a variable and subscribe to it at the specified rate. + + :param name: The variable name + :type name: str + :param value: The value to set + :type value: float + :param rate_hz: The update rate in Hz (default: 3) + :type rate_hz: int + """ + self._puppet.define_variable(name, VAR_TYPE_FLOAT, PERM_READ_ONLY) + try: + self._puppet.subscribe_variable(name, rate_hz) + except: + pass + self._puppet.set_variable(name, value) + + def get_value(self, name): + """ + Get the value of a variable. + + :param name: The variable name + :type name: str + """ + self._puppet.define_variable(name, VAR_TYPE_FLOAT, PERM_WRITE_ONLY) + return self._puppet.get_variable(name) \ No newline at end of file diff --git a/XRPLib/gamepad.py b/XRPLib/gamepad.py index 9a6e661..f3f08de 100644 --- a/XRPLib/gamepad.py +++ b/XRPLib/gamepad.py @@ -1,5 +1,4 @@ -from ble.blerepl import uart -import sys +from .puppet import Puppet, VAR_TYPE_FLOAT, VAR_TYPE_INT, VAR_TYPE_BOOL, PERM_WRITE_ONLY, PERM_READ_ONLY from micropython import const class Gamepad: @@ -25,12 +24,27 @@ class Gamepad: DPAD_L = const(16) DPAD_R = const(17) - _joyData = [ - 0.0, - 0.0, - 0.0, - 0.0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0] + # Mapping from index to variable name + _VAR_NAMES = { + 0: '$gamepad.x1', + 1: '$gamepad.y1', + 2: '$gamepad.x2', + 3: '$gamepad.y2', + 4: '$gamepad.button_a', + 5: '$gamepad.button_b', + 6: '$gamepad.button_x', + 7: '$gamepad.button_y', + 8: '$gamepad.bumper_l', + 9: '$gamepad.bumper_r', + 10: '$gamepad.trigger_l', + 11: '$gamepad.trigger_r', + 12: '$gamepad.back', + 13: '$gamepad.start', + 14: '$gamepad.dpad_up', + 15: '$gamepad.dpad_dn', + 16: '$gamepad.dpad_l', + 17: '$gamepad.dpad_r', + } @classmethod def get_default_gamepad(cls): @@ -44,28 +58,73 @@ def get_default_gamepad(cls): def __init__(self): """ - Manages communication with gamepad data coming from a remote computer via bluetooth + Manages communication with gamepad data coming from a remote computer via XPP protocol. + """ + self._puppet = Puppet.get_default_puppet() + self._started = False + + # Register all gamepad variables + self._register_variables() + def _register_variables(self): + """ + Register all gamepad variables with the XPP protocol. """ + # Joystick axes are floats + for idx in [self.X1, self.Y1, self.X2, self.Y2]: + var_name = self._VAR_NAMES[idx] + self._puppet.define_variable(var_name, VAR_TYPE_FLOAT, PERM_WRITE_ONLY) + + # Buttons are integers (1 or 0) + for idx in range(4, 18): + var_name = self._VAR_NAMES[idx] + self._puppet.define_variable(var_name, VAR_TYPE_INT, PERM_WRITE_ONLY) + + # Register gamepad enabled variable (read-only from XRP side) + self._puppet.define_variable('$gamepad.enabled', VAR_TYPE_BOOL, PERM_READ_ONLY) + def start(self): """ Signals the remote computer to begin sending gamepad data packets. + Subscribes to all gamepad variables at a high rate (50 Hz). """ - for i in range(len(self._joyData)): - self._joyData[i] = 0.0 - uart.set_data_callback(self._data_callback) - sys.stdout.write(chr(27)) - sys.stdout.write(chr(101)) + self._puppet.start() + self._puppet.send_program_start() + + # Enable gamepad - signal to client to start sending + self._puppet.set_variable('$gamepad.enabled', True) + if self._started: + return + + # Subscribe to all gamepad variables at 50 Hz + for var_name in self._VAR_NAMES.values(): + self._puppet.subscribe_variable(var_name, 50) + + self._started = True def stop(self): """ - Signals the remote computer to stop sending gamepad data packets. + Signals the remote computer to stop sending gamepad data packets. + Unsubscribes from all gamepad variables. """ - sys.stdout.write(chr(27)) - sys.stdout.write(chr(102)) - def get_value(self, index:int) -> float: + self._puppet.send_program_end() + + if not self._started: + return + + # Disable gamepad - signal to client to stop sending + self._puppet.set_variable('$gamepad.enabled', False) + + # 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: """ Get the current value of a joystick axis @@ -75,9 +134,19 @@ def get_value(self, index:int) -> float: :returns: The value of the joystick between -1 and 1 :rtype: float """ - return -self._joyData[index] #returning the negative to make normal for user + if index not in self._VAR_NAMES: + return 0.0 + + var_name = self._VAR_NAMES[index] + try: + value = self._puppet.get_variable(var_name) + # Return negative to make normal for user (backward compatibility) + return -value + except: + # Return default if variable not available + return 0.0 - def is_button_pressed(self, index:int) -> bool: + def is_button_pressed(self, index: int) -> bool: """ Checks if a specific button is currently pressed. @@ -87,11 +156,14 @@ def is_button_pressed(self, index:int) -> bool: :returns: The value of the button 1 or 0 :rtype: bool """ - return self._joyData[index] > 0 - - def _data_callback(self, data): - if(data[0] == 0x55 and len(data) == data[1] + 2): - for i in range(2, data[1] + 2, 2): - self._joyData[data[i]] = round(data[i + 1]/127.5 - 1, 2) - + if index not in self._VAR_NAMES: + return False + var_name = self._VAR_NAMES[index] + try: + value = self._puppet.get_variable(var_name) + # Works for both int and float + return value > 0 + except: + # Return default if variable not available + return False diff --git a/XRPLib/puppet.py b/XRPLib/puppet.py new file mode 100644 index 0000000..03c2269 --- /dev/null +++ b/XRPLib/puppet.py @@ -0,0 +1,915 @@ +""" +XRP Puppet Protocol (XPP) - Core Protocol Implementation + +A bidirectional protocol for communicating with the XRP robot remotely. +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,kbd_intr + +# Message framing constants +MSG_START_1 = const(0xAA) +MSG_START_2 = const(0x55) +MSG_END_1 = const(0x55) +MSG_END_2 = const(0xAA) +MAX_PAYLOAD_SIZE = const(251) + +# Message type constants +MSG_TYPE_VAR_DEF = const(1) +MSG_TYPE_VAR_UPDATE = const(2) +MSG_TYPE_VAR_SUBSCRIBE = const(3) +MSG_TYPE_VAR_UNSUBSCRIBE = const(4) +MSG_TYPE_PROGRAM_START = const(5) +MSG_TYPE_PROGRAM_END = const(6) +MSG_TYPE_HEARTBEAT = const(7) + +# Variable type constants +VAR_TYPE_INT = const(1) +VAR_TYPE_FLOAT = const(2) +VAR_TYPE_BOOL = const(3) + +# Permission constants +PERM_READ_ONLY = const(1) +PERM_WRITE_ONLY = const(2) +PERM_READ_WRITE = const(3) + +# First custom variable ID +FIRST_CUSTOM_VAR_ID = const(38) + +# Mapping of standard variable names to IDs (1-37) +_STANDARD_VAR_IDS = { + # Gamepad variables (1-19) + '$gamepad.x1': 1, + '$gamepad.y1': 2, + '$gamepad.x2': 3, + '$gamepad.y2': 4, + '$gamepad.button_a': 5, + '$gamepad.button_b': 6, + '$gamepad.button_x': 7, + '$gamepad.button_y': 8, + '$gamepad.bumper_l': 9, + '$gamepad.bumper_r': 10, + '$gamepad.trigger_l': 11, + '$gamepad.trigger_r': 12, + '$gamepad.back': 13, + '$gamepad.start': 14, + '$gamepad.dpad_up': 15, + '$gamepad.dpad_dn': 16, + '$gamepad.dpad_l': 17, + '$gamepad.dpad_r': 18, + '$gamepad.enabled': 19, + # IMU variables (20-25) + '$imu.yaw': 20, + '$imu.roll': 21, + '$imu.pitch': 22, + '$imu.acc_x': 23, + '$imu.acc_y': 24, + '$imu.acc_z': 25, + # Encoder variables (26-29) + '$encoder.left': 26, + '$encoder.right': 27, + '$encoder.3': 28, + '$encoder.4': 29, + # Current sensor variables (30-33) + '$current.left': 30, + '$current.right': 31, + '$current.3': 32, + '$current.4': 33, + # Other sensor variables (34-37) + '$rangefinder.distance': 34, + '$reflectance.left': 35, + '$reflectance.right': 36, + '$voltage': 37, +} + + +class Puppet: + """ + Core XRP Puppet Protocol implementation. + Manages bidirectional communication, variable registry, and message handling. + """ + + _DEFAULT_PUPPET_INSTANCE = None + _DEFAULT_TRANSPORT_MODE = 'AUTO' + + @classmethod + def get_default_puppet(cls, transport_mode='AUTO'): + """ + Get the default XPP instance. This is a singleton. + + :param transport_mode: 'AUTO', 'BLE', or 'USB_STDIO'. + Only used when creating the singleton for the first time. + """ + transport_mode = transport_mode.upper() + if transport_mode not in ('AUTO', 'BLE', 'USB_STDIO'): + raise ValueError("transport_mode must be 'AUTO', 'BLE', or 'USB_STDIO'") + if cls._DEFAULT_PUPPET_INSTANCE is None: + cls._DEFAULT_TRANSPORT_MODE = transport_mode + cls._DEFAULT_PUPPET_INSTANCE = cls() + return cls._DEFAULT_PUPPET_INSTANCE + + def __init__(self): + """ + Initialize the XPP protocol handler. + """ + self._transport_mode = self.__class__._DEFAULT_TRANSPORT_MODE + + # Variable registry: name -> (id, type, permissions, value, update_rate, last_sent_time) + self._variables = {} + self._variable_ids = {} # id -> name (reverse mapping) + self._next_var_id = FIRST_CUSTOM_VAR_ID # Start at 38 for custom variables + + # Transport layer + self._transport = None + self._transport_type = None + self._rx_buffer = bytearray() + self._rx_state = 0 # 0=waiting for start, 1=reading length, 2=reading payload, 3=waiting for end + + # Packet tracking + self.packets_sent = 0 + self.packets_received = 0 + self.packets_dropped = 0 + self._sequence_number = 0 + + # Update rate management + 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 + + # Initialize transport + self._init_transport() + + def _init_transport(self): + """ + Initialize transport based on selected mode: + - AUTO: BLE if currently connected, otherwise USB_STDIO + - BLE: force BLE transport even without active connections + - USB_STDIO: force USB STDIO transport + """ + if self._transport_mode == 'USB_STDIO': + self._transport_type = 'USB_STDIO' + self._start_poll_timer() + return + + try: + from ble.blerepl import uart + + if self._transport_mode == 'BLE': + self._transport = uart + self._transport_type = 'BLE' + self._transport.set_data_callback(self._data_callback) + return + + # AUTO mode: prefer BLE only when already connected + if len(uart._connections) > 0: + self._transport = uart + self._transport_type = 'BLE' + self._transport.set_data_callback(self._data_callback) + return + else: + self._transport_type = 'USB_STDIO' + self._start_poll_timer() + return + except ImportError: + if self._transport_mode == 'BLE': + raise RuntimeError("BLE transport requested but ble.blerepl.uart is unavailable") + self._transport_type = 'USB_STDIO' + self._start_poll_timer() + return + + + def _poll_stdio(self): + """ + Poll STDIO for incoming data using select.poll. + Reads raw bytes from the stdin buffer. + """ + 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(3) #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 _clear_custom_variables(self): + """ + Remove all custom variables from the registry. + Standard variables (IDs 1-37) are retained. + """ + custom_names = [name for name in self._variables if name not in _STANDARD_VAR_IDS] + for name in custom_names: + var_id = self._variables[name][0] + del self._variables[name] + if var_id in self._variable_ids: + del self._variable_ids[var_id] + self._next_var_id = FIRST_CUSTOM_VAR_ID + self._start_update_timer() + + def stop(self): + """ + Stop the STDIO polling and clear custom variables from the table. + + Removes all user-defined variables (IDs 38+) from the registry and + resets the custom variable ID pool. Standard variables ($gamepad.*, + $imu.*, $encoder.*, etc.) are retained. + """ + if self._transport_type == 'USB_STDIO': + self._stop_poll_timer() + self._clear_custom_variables() + + + def _data_callback(self, data): + """ + Handle incoming data from transport layer. + """ + if isinstance(data, (bytes, bytearray)): + self._rx_buffer.extend(data) + else: + self._rx_buffer.append(data) + + self._process_rx_buffer() + + def _process_rx_buffer(self): + """ + Process received data buffer, extracting complete messages. + """ + while len(self._rx_buffer) >= 2: + if self._rx_state == 0: # Waiting for start sequence + # Look for start sequence + idx = -1 + for i in range(len(self._rx_buffer) - 1): + if self._rx_buffer[i] == MSG_START_1 and self._rx_buffer[i+1] == MSG_START_2: + idx = i + break + + 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 + self._rx_state = 1 + continue + + elif self._rx_state == 1: # Reading type + if len(self._rx_buffer) < 3: # Need start(2) + type(1) + return + msg_type = self._rx_buffer[2] + self._rx_state = 2 + continue + + elif self._rx_state == 2: # Reading length + if len(self._rx_buffer) < 4: # Need start(2) + type(1) + length(1) + return + payload_len = self._rx_buffer[3] + if payload_len > MAX_PAYLOAD_SIZE: + # Invalid length, reset + self._rx_buffer = self._rx_buffer[4:] + self._rx_state = 0 + self.packets_dropped += 1 + continue + + self._rx_state = 3 + continue + + elif self._rx_state == 3: # Reading payload + payload_len = self._rx_buffer[3] + total_needed = 4 + payload_len + 2 # start(2) + type(1) + len(1) + payload + end(2) + + if len(self._rx_buffer) < total_needed: + return # Wait for more data + + # Check end sequence + end_idx = 4 + payload_len + if (self._rx_buffer[end_idx] != MSG_END_1 or + self._rx_buffer[end_idx + 1] != MSG_END_2): + # Invalid end sequence, reset + self._rx_buffer = self._rx_buffer[4:] + self._rx_state = 0 + self.packets_dropped += 1 + continue + + # Extract message type and payload data + msg_type = self._rx_buffer[2] + payload_data = bytes(self._rx_buffer[4:end_idx]) + self._rx_buffer = self._rx_buffer[total_needed:] + self._rx_state = 0 + + # Process message + self.packets_received += 1 + self._handle_message(msg_type, payload_data) + continue + + def _write_data(self, data): + """ + Write data to transport layer. + """ + if self._transport_type == 'BLE': + self._transport.write_data(data) + + elif self._transport_type == 'USB_STDIO': + sys.stdout.buffer.write(data) + + def _pack_message(self, msg_type, payload_data): + """ + Pack a message with framing. + Returns bytearray with complete message. + Format: [START] [TYPE] [LENGTH] [PAYLOAD] [END] + LENGTH includes payload_data length (not including TYPE byte) + """ + payload_len = len(payload_data) + if payload_len > MAX_PAYLOAD_SIZE: + raise ValueError(f"Payload too large: {payload_len} > {MAX_PAYLOAD_SIZE}") + + msg = bytearray() + msg.append(MSG_START_1) + msg.append(MSG_START_2) + msg.append(msg_type) + msg.append(payload_len) # Length of payload_data only + msg.extend(payload_data) + msg.append(MSG_END_1) + msg.append(MSG_END_2) + + return msg + + def _send_message(self, msg_type, payload_data): + """ + Send a message over the transport. + """ + msg = self._pack_message(msg_type, payload_data) + self._write_data(msg) + self.packets_sent += 1 + self._sequence_number = (self._sequence_number + 1) % 256 + + def _handle_message(self, msg_type, payload_data): + """ + Handle incoming message based on type. + + :param msg_type: Message type byte + :param payload_data: Message payload data (without type byte) + """ + if msg_type == MSG_TYPE_VAR_DEF: + self._handle_var_def(payload_data) + elif msg_type == MSG_TYPE_VAR_UPDATE: + self._handle_var_update(payload_data) + elif msg_type == MSG_TYPE_VAR_SUBSCRIBE: + self._handle_var_subscribe(payload_data) + elif msg_type == MSG_TYPE_VAR_UNSUBSCRIBE: + self._handle_var_unsubscribe(payload_data) + elif msg_type == MSG_TYPE_PROGRAM_START: + self._handle_program_start() + elif msg_type == MSG_TYPE_PROGRAM_END: + self._handle_program_end() + elif msg_type == MSG_TYPE_HEARTBEAT: + self._handle_heartbeat(payload_data) + + def _handle_var_def(self, payload): + """ + Handle variable definition message. + Format: name_len(1) name(name_len) type(1) permissions(1) var_id(1) + """ + if len(payload) < 3: + return + + name_len = payload[0] + if len(payload) < 1 + name_len + 3: # name_len + name + type + permissions + var_id + return + + name = payload[1:1+name_len].decode('utf-8') + var_type = payload[1+name_len] + permissions = payload[1+name_len+1] + var_id = payload[1+name_len+2] + + # If this is a standard variable, use the predefined ID instead + if name in _STANDARD_VAR_IDS: + var_id = _STANDARD_VAR_IDS[name] + + # Register the variable with the (corrected) ID + if name not in self._variables: + # Default value based on type + if var_type == VAR_TYPE_INT: + default_value = 0 + elif var_type == VAR_TYPE_FLOAT: + default_value = 0.0 + elif var_type == VAR_TYPE_BOOL: + default_value = False + else: + return + + self._variables[name] = (var_id, var_type, permissions, default_value, 0, 0) + self._variable_ids[var_id] = name + # Update next_var_id if needed (for custom variables only) + if var_id >= self._next_var_id and name not in _STANDARD_VAR_IDS: + self._next_var_id = var_id + 1 + if self._next_var_id > 255: + self._next_var_id = FIRST_CUSTOM_VAR_ID # Wrap to start of custom range + + def _handle_var_update(self, payload): + """ + Handle variable update message (batched format). + Format: count(1) [var_id(1) type(1) value(type-dependent)] * count + A count of 1 is equivalent to a single update. + """ + if len(payload) < 1: + return + + count = payload[0] + offset = 1 + + if count == 0 or count > 50: # Sanity check + return + + # Batched update format: count(1) [var_id(1) type(1) value] * count + for i in range(count): + if len(payload) < offset + 2: # Need at least var_id(1) + type(1) + break + + var_id = payload[offset] + offset += 1 + var_type = payload[offset] + offset += 1 + + if var_id not in self._variable_ids: + # Skip this update, but need to advance offset + if var_type == VAR_TYPE_INT: + offset += 4 + elif var_type == VAR_TYPE_FLOAT: + offset += 4 + elif var_type == VAR_TYPE_BOOL: + offset += 1 + continue + + name = self._variable_ids[var_id] + var_info = self._variables[name] + + # Unpack value based on type from message + if var_type == VAR_TYPE_INT: + if len(payload) < offset + 4: + break + value = struct.unpack(' 255: + raise RuntimeError("Maximum number of custom variables exceeded") + send_def = True # Send VAR_DEF for custom variables + + # Default value based on type + if var_type == VAR_TYPE_INT: + default_value = 0 + elif var_type == VAR_TYPE_FLOAT: + default_value = 0.0 + elif var_type == VAR_TYPE_BOOL: + default_value = False + else: + raise ValueError(f"Invalid variable type: {var_type}") + + self._variables[name] = (var_id, var_type, permissions, default_value, 0, 0) + self._variable_ids[var_id] = name + + # Only send variable definition for custom variables + if send_def: + self._send_var_def(name, var_type, permissions, var_id) + + return var_id + + def _send_var_def(self, name, var_type, permissions, var_id): + """ + Send variable definition message. + """ + name_bytes = name.encode('utf-8') + payload = bytearray() + payload.append(len(name_bytes)) + payload.extend(name_bytes) + payload.append(var_type) + payload.append(permissions) + payload.append(var_id) # 1 byte + + self._send_message(MSG_TYPE_VAR_DEF, payload) + + def set_variable(self, name, value): + """ + Set a variable value and send update if subscribed. + + :param name: Variable name + :param value: Variable value + """ + if name not in self._variables: + raise ValueError(f"Variable not defined: {name}") + + var_info = self._variables[name] + var_type = var_info[1] + + # Type check + if var_type == VAR_TYPE_INT and not isinstance(value, int): + raise TypeError(f"Variable {name} expects int, got {type(value)}") + elif var_type == VAR_TYPE_FLOAT and not isinstance(value, (int, float)): + raise TypeError(f"Variable {name} expects float, got {type(value)}") + elif var_type == VAR_TYPE_BOOL and not isinstance(value, bool): + raise TypeError(f"Variable {name} expects bool, got {type(value)}") + + # Update value + self._variables[name] = (var_info[0], var_info[1], var_info[2], + value, var_info[4], var_info[5]) + + # Send update if rate > 0 or if this is an on-demand update + # (on-demand updates are sent immediately when set) + if var_info[4] > 0 or var_info[4] == 0: + self._send_var_update(name) + + def get_variable(self, name): + """ + Get a variable value. + + :param name: Variable name + :return: Variable value + """ + if name not in self._variables: + raise ValueError(f"Variable not defined: {name}") + + return self._variables[name][3] + + def _send_var_update(self, name): + """ + Send single variable update message (uses batched format with count=1). + For multiple updates, use _send_batched_var_updates(). + """ + if name not in self._variables: + return + + var_info = self._variables[name] + var_id = var_info[0] + var_type = var_info[1] + value = var_info[3] + + # Pack as batched update with count=1 + payload = bytearray() + payload.append(1) # count = 1 + payload.append(var_id) # 1 byte + payload.append(var_type) # Include type byte + + if var_type == VAR_TYPE_INT: + payload.extend(struct.pack(' 0 and enough time has passed + if rate > 0: + period_ms = int(1000 / rate) + if time.ticks_diff(current_time, last_sent) < period_ms: + continue # Not ready yet + + updates.append((name, var_info)) + + if not updates: + return + + # Pack batched update: count(1) [var_id(1) type(1) value] * count + payload = bytearray() + payload.append(len(updates)) # count + + for name, var_info in updates: + var_id = var_info[0] + var_type = var_info[1] + value = var_info[3] + + payload.append(var_id) # 1 byte + payload.append(var_type) + + if var_type == VAR_TYPE_INT: + payload.extend(struct.pack(' max_rate: + max_rate = var_info[4] + + if max_rate > 0 and not self._update_timer_running: + # Start timer at max_rate Hz + period_ms = int(1000 / max_rate) + self._update_timer.init(period=period_ms, mode=Timer.PERIODIC, + callback=lambda t: self._update_timer_callback()) + self._update_timer_running = True + elif max_rate == 0 and self._update_timer_running: + # Stop timer + self._update_timer.deinit() + self._update_timer_running = False + elif max_rate > 0 and self._update_timer_running: + # Update timer period if rate changed + period_ms = int(1000 / max_rate) + self._update_timer.deinit() + self._update_timer.init(period=period_ms, mode=Timer.PERIODIC, + callback=lambda t: self._update_timer_callback()) + + def _update_timer_callback(self): + """ + Timer callback to send variable updates at their specified rates. + Batches all ready updates into a single message for efficiency. + """ + current_time = time.ticks_ms() + + # Collect all variables that are ready to send + ready_vars = [] + for name, var_info in self._variables.items(): + var_id, var_type, permissions, value, rate, last_sent = var_info + + # Only send if rate > 0 and enough time has passed + if rate > 0: + period_ms = int(1000 / rate) + if time.ticks_diff(current_time, last_sent) >= period_ms: + # Check if we have read permission + if permissions in (PERM_READ_ONLY, PERM_READ_WRITE): + ready_vars.append(name) + + # Send all ready updates in a single batched message + if ready_vars: + self._send_batched_var_updates(ready_vars) + + def send_program_start(self): + """ + Send program start message. + """ + self._send_message(MSG_TYPE_PROGRAM_START, b'') + self._program_running = True + + def send_program_end(self): + """ + Send program end message. + """ + self._send_message(MSG_TYPE_PROGRAM_END, b'') + self._program_running = False + + def send_heartbeat(self): + """ + Send heartbeat message. + """ + # Include stats + payload = struct.pack('