diff --git a/.gitignore b/.gitignore index 67f618e..ab4f7c5 100644 --- a/.gitignore +++ b/.gitignore @@ -6,4 +6,7 @@ BadgeBot.code-workspace .editorconfig .venv/ .venv-wsl*/ - +# minify.py build artefacts +vendor/**/*.min.py +vendor/**/*.renamed.py +EEPROM/*.renamed.py diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..f72a681 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,7 @@ +[submodule "vendor/HexDrive2"] + path = vendor/HexDrive2 + url = https://github.com/TeamRobotmad/HexDrive2.git +[submodule "vendor/HexDrive"] + path = vendor/HexDrive + url = https://github.com/TeamRobotmad/HexDrive.git + branch = main diff --git a/.vscode/settings.json b/.vscode/settings.json index a584ce5..f4f3aaf 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -1,26 +1,7 @@ { - // Force pylint to use the shared project config in pyproject.toml. + // Force pylint to use the shared sim/apps config. // This keeps CLI runs and VS Code diagnostics aligned. "pylint.args": [ - "--rcfile=${workspaceFolder}/pyproject.toml" - ], - - // Pylance diagnostics are separate from pylint. - // We disable missing-import noise for BadgeOS/MicroPython modules that - // only exist on-device, while keeping other analysis enabled. - "python.analysis.diagnosticSeverityOverrides": { - "reportMissingModuleSource": "none", - "reportMissingImports": "none" - }, - - // Point Pylance at local .pyi stubs for BadgeOS and MicroPython APIs. - // See typings/README.md for rationale and maintenance notes. - "python.analysis.stubPath": "typings", - - // No additional import roots are needed because stubs are provided via - // python.analysis.stubPath and project files resolve from workspace root. - "python.analysis.extraPaths": [], - - // Keep analysis enabled for all project files by default. - "python.analysis.ignore": [] -} \ No newline at end of file + "--rcfile=${workspaceFolder}/../pyproject.toml" + ] +} diff --git a/EEPROM/gps.py b/EEPROM/gps.py deleted file mode 100644 index ee09d4b..0000000 --- a/EEPROM/gps.py +++ /dev/null @@ -1,190 +0,0 @@ -""" GPS App for Hexpansion """ -import app - -from app_components.tokens import label_font_size, button_labels -from events.input import Buttons, BUTTON_TYPES, ButtonDownEvent -from system.eventbus import eventbus -from system.hexpansion.config import HexpansionConfig -from system.patterndisplay.events import PatternDisable, PatternEnable -from system.scheduler.events import RequestForegroundPopEvent, RequestForegroundPushEvent, RequestStopAppEvent -from tildagonos import tildagonos -from machine import UART, Pin - -# Minimal length method names to make the mpy file as small as possible so it might fit in the 2k hexpansion EEPROM. -# Minimal functionality to get a GPS fix -# This version is NOT for the App Store - -VERSION = 1 - -# Hardware defintions: -TX_PIN = 1 # HS_G for TX -RX_PIN = 0 # HS_F for RX -RESET_PIN = 2 # HS_H for reset -PPS_PIN = 3 # HS_I for PPS - -###JUST FOR USE WITH MY PROTOTYPE BOARD -ENABLE_PIN = 0 # First LS pin used to enable the SMPSU -###JUST FOR USE WITH MY PROTOTYPE BOARD - -class GPSApp(app.App): # pylint: disable=no-member - """ App to get GPS data from a GPS module connected to the hexpansion and display it on the badge. """ - def __init__(self, config: HexpansionConfig | None = None): - super().__init__() - # If run from EEPROM on the hexpansion, the config will be passed in with the correct pin objects - self.config: HexpansionConfig | None = config - if config is None: - return - self.tx_pin = config.pin[TX_PIN] - self.rx_pin = config.pin[RX_PIN] - self.reset = config.pin[RESET_PIN] - self.pps = config.pin[PPS_PIN] - -###JUST FOR USE WITH MY PROTOTYPE BOARD - self.power_control = config.ls_pin[ENABLE_PIN] - self.power_control.init(mode=Pin.OUT) - self.power_control.value(1) -###JUST FOR USE WITH MY PROTOTYPE BOARD - - self.foreground = False - self.button_states = Buttons(self) - self.last_fix = None - - # Event handlers for gaining and losing focus and for stopping the app - eventbus.on_async(RequestStopAppEvent, self.s, self) - eventbus.on_async(RequestForegroundPushEvent, self.r, self) - eventbus.on_async(RequestForegroundPopEvent, self.p, self) - - self.uart = UART(1, baudrate=9600, tx=self.tx_pin, rx=self.rx_pin) - self.reset.init(mode=Pin.OUT) - self.pps.init(mode=Pin.IN) - self.reset.value(1) # set reset high here and release when 100ms has passed in foreground update. - self.ticks_since_start = 0 - self.ticks_since_last_fix = 0 - - - def deinit(self): - """ Deinitialise the app, releasing any resources (e.g. UART) """ - self.uart.deinit() - self.power_control.value(0) # Cut power to the GPS to save power when not in use - - - def get_version(self) -> int: - """ Get the version of the app - this is used to determine if an upgrade is required. """ - return VERSION - - - async def s(self, event: RequestStopAppEvent): - """ Handle the RequestStopAppEvent so that we can release resources """ - if event.app == self: - self.deinit() - - - async def r(self, event: RequestForegroundPushEvent): - """ Handle the RequestForegroundPushEvent to know when we gain focus """ - if event.app == self: - eventbus.emit(PatternDisable()) - eventbus.on(ButtonDownEvent, self.d, self) - self.foreground = True - - - async def p(self, event: RequestForegroundPopEvent): - """ Handle the RequestForegroundPopEvent to know when we lose focus """ - if event.app == self: - eventbus.emit(PatternEnable()) - eventbus.remove(ButtonDownEvent, self.d, self) - - - def d(self, event: ButtonDownEvent): - """ Handle button down events """ - if event.button == BUTTON_TYPES["CANCEL"]: - self.button_states.clear() - self.minimise() - - - def update(self, delta): - """ Update the app state - expire last_fix if it is too old """ - if self.reset.value(): - self.ticks_since_start += delta - if self.ticks_since_start > 100: - # Release reset after 100ms to allow the GPS to start up - self.reset.value(0) - if not self.foreground: - # This triggers the automatic foreground display - eventbus.emit(RequestForegroundPushEvent(self)) - self.foreground = True - if self.last_fix: - self.ticks_since_last_fix += delta - if self.ticks_since_last_fix > 10000: - # If it's been more than 10 seconds since the last fix, disccard it - self.last_fix = None - - - def background_update(self, _delta): - """ Update in the background - read from the UART and parse any GPS data """ - line = self.uart.readline() - if line: - try: - line = line.decode().strip() - result = n(line) - if result: - self.last_fix = result - self.ticks_since_last_fix = 0 - except (UnicodeError, ValueError, AttributeError): - pass - - - def draw(self, ctx): - """ Draw the app - display the last GPS fix or a searching message if no fix is available """ - ctx.rgb(0, 0.2, 0).rectangle(-120, -120, 240, 240).fill() - ctx.rgb(0, 1, 0) - ctx.font_size = label_font_size - ctx.text_align = ctx.LEFT - ctx.text_baseline = ctx.BOTTOM - if self.last_fix: - ctx.move_to(-100, -10).text("Lat: " + str(round(self.last_fix["lat"], 5))) - ctx.move_to(-100, 20).text("Lon: " + str(round(self.last_fix["lon"], 5))) - for i in range(1, 13): - tildagonos.leds[i] = (0,1,0) - tildagonos.leds.write() - else: - ctx.move_to(-100, 0).text("Searching...") - for i in range(1,13): - tildagonos.leds[i] = (0,0,0) - tildagonos.leds.write() - - # show labels for buttons - button_labels(ctx, cancel_label="Exit") - -def n(line: str) -> dict[str, float] | None: - """ Parse an NMEA RMC sentence and return a dictionary with the latitude and longitude if valid, or None if invalid. """ - parts = line.split(',') - - if parts[0] not in ("$GNRMC", "$GPRMC"): - return None - elif parts[2] != "A": # A = valid, V = invalid - return None - else: - lat_raw = parts[3] - lat_dir = parts[4] - lon_raw = parts[5] - lon_dir = parts[6] - - if not lat_raw or not lon_raw: - return None - - # Convert to decimal degrees - lat = float(lat_raw[:2]) + float(lat_raw[2:]) / 60 - lon = float(lon_raw[:3]) + float(lon_raw[3:]) / 60 - - if lat_dir == "S": - lat = -lat - if lon_dir == "W": - lon = -lon - - return { - "lat": lat, - "lon": lon - } - - -__app_export__ = GPSApp #pylint: disable=invalid-name diff --git a/EEPROM/hexdrive.py b/EEPROM/hexdrive.py deleted file mode 100644 index 7cb0a83..0000000 --- a/EEPROM/hexdrive.py +++ /dev/null @@ -1,532 +0,0 @@ -"""HexDrive Hexpansion App for BadgeBot.""" - -# This is the app to be installed from the HexDrive Hexpansion EEPROM. -# it is copied onto the EEPROM and renamed as app.py/mpy -# It is then run from the EEPROM by the BadgeOS. - -import ota -from machine import PWM, Pin -from system.eventbus import eventbus -from system.hexpansion.config import HexpansionConfig -from system.scheduler.events import RequestStopAppEvent - -import app - -# HexDrive.py App Version - used to check if upgrade is required -VERSION = 7 - -# HexDrive Hexpansion constants -# Hardware defintions: -_ENABLE_PIN = 0 # First LS pin used to enable the SMPSU -#_DETECT_PIN = 1 # Second LS pin used to sense if the SMPSU has a source of power - -# Default values and limits: -_DEFAULT_PWM_FREQ = 20000 # 20kHz is a good default for motors as it is above the audible range for most people and works with most motors and ESCs -_DEFAULT_SERVO_FREQ = 50 # 50Hz = 20mS period -_DEFAULT_KEEP_ALIVE_PERIOD = 1000 # 1 second -_MAX_NUM_CHANNELS = 4 # Max number of PWM channels supported by any type of HexDrive (Hexpansion limitation, not BadgeBot limit) -_MAX_NUM_MOTORS = 2 # Max number of motor channels supported by any type of HexDrive - -# Servo Constants -_MAX_SERVO_FREQ = 200 # 200Hz = 5mS period (can work with some Servos but not all) -_SERVO_CENTRE = 1500 # 1500us pulse width is the centre position for most RC servos (but some may be different, so we allow this to be trimmed) -_MAX_SERVO_RANGE = 1400 # 1400us either side of centre (VERY WIDE) -_SERVO_MAX_TRIM = 1000 # 1000us either side of centre for trimming the centre position - -# EEPROM Constants -_EEPROM_ADDR = 0x50 # I2C address of the EEPROM on the HexDrive and HexSense Hexpansion -_EEPROM_NUM_ADDRESS_BYTES = 2 # Number of bytes used for the memory address when reading from the EEPROM (e.g. 2 for 16-bit addressing) -_PID_ADDR = 0x12 # Address in the EEPROM where the Product ID (PID) byte is stored - used to identify the type of Hexpansion - -# PID MSByte value for RobotMad HexDrive Hexpansion modules (used to identify the type of HexDrive when reading the EEPROM) -_HEXDRIVE_PID_MSB = 0xCB - - -class HexDriveType: - """Represents a sub-type of HexDrive Hexpansion module.""" - __slots__ = ("pid", "name", "motors", "servos") - - def __init__(self, pid_byte: int, motors: int = 0, servos: int = 0, name: str = "Unknown"): - self.pid: int = pid_byte # Product ID byte read from the EEPROM to identify the type of HexDrive - self.name: str = name # A friendly name for the type of HexDrive - self.motors: int = motors # Number of motor channels supported by this type of HexDrive (0, 1 or 2) - self.servos: int = servos # Number of servo channels supported by this type of HexDrive (0, 2 or 4) - -_HEXDRIVE_TYPES = ( - HexDriveType(0xCA, motors=2, name="2 Motor"), - HexDriveType(0xCB, motors=2, servos=4), - HexDriveType(0xCC, servos=4, name="4 Servo"), - HexDriveType(0xCD, motors=1, servos=2, name="1 Mot 2 Srvo"), -) - -class HexDriveApp(app.App): # pylint: disable=no-member - """ HexDrive Hexpansion App for BadgeBot.""" - def __init__(self, config: HexpansionConfig | None = None): - super().__init__() - self.config: HexpansionConfig | None = config - self._hexdrive_type: HexDriveType | None = None - self._logging: bool = True - self._keep_alive_period: int = _DEFAULT_KEEP_ALIVE_PERIOD - self._power_state: bool = False - self._pwm_setup: bool = False - self._time_since_last_update: int = 0 - self._outputs_energised: bool = False - self.PWMOutput: list[PWM | None] = [None] * _MAX_NUM_CHANNELS - self._freq: list[int] = [0] * _MAX_NUM_CHANNELS - self._motor_output: list[int] = [0] * _MAX_NUM_MOTORS - if config is None: - print("D:No Config") - return - # LS Pins - #self._power_detect = self.config.ls_pin[_DETECT_PIN] - self._power_control = self.config.ls_pin[_ENABLE_PIN] - - self._servo_centre = [_SERVO_CENTRE] * _MAX_NUM_CHANNELS - eventbus.on_async(RequestStopAppEvent, self._handle_stop_app, self) - # What version of BadgeOS are we running on? - try: - ver = self._parse_version(ota.get_version()) - #print(f"D:S/W {ver}") - # e.g. v1.9.0-beta.1 - if ver >= [1, 9, 0]: - # we need v1.9.0+ to be able to read the EEPROM with 16-bit addressing, so if we are running on an older version then we cannot continue - pass - else: - print("D:BadgeOS Upgrade to v1.9.0+ required") - return - except Exception as e: # pylint: disable=broad-except - print(f"D:Ver check failed {e}") - self.initialise() - - - def initialise(self) -> bool: - """Initialise the app - return True if successful, False if failed.""" - self._pwm_setup = False - if self.config is None: - return False - # report app starting and which port it is running on - print(f"D:HexDrive V{VERSION} by RobotMad on port {self.config.port}") - # Initialise HS Pins - for _, hs_pin in enumerate(self.config.pin): - # Set HexDrive Hexpansion HS pins to low level outputs - hs_pin.init(mode=Pin.OUT) - hs_pin.value(0) - # Initialise LS Pins - try: - #self._power_detect.init(mode=Pin.IN) - self._power_control.init(mode=Pin.OUT) - except Exception as e: # pylint: disable=broad-except - print(f"D:{self.config.port}:ls_pin setup failed {e}") - return False - # ensure SMPSU is turned off to start with - self.set_power(False) - - # read hexpansion header from EEPROM to find out which sub-type we are - # and allocate PWM outputs accordingly - self._hexdrive_type = self._check_port_for_hexdrive(self.config.port) - if self._logging and self._hexdrive_type is not None: - print(f"D:{self.config.port}:Type:'{self._hexdrive_type.name}'") - - return self._pwm_init() - - - def deinitialise(self) -> bool: - """ De-initialise the app - return True if successful, False if failed.""" - # Turn off all PWM outputs & release resources - self.set_power(False) - self._pwm_deinit() - for hs_pin in self.config.pin: - hs_pin.init(mode=Pin.IN) - return True - - - async def _handle_stop_app(self, event): - """ Handle the RequestStopAppEvent so that we can release resources """ - try: - if event.app == self: - if self._logging: - print(f"D:{self.config.port}:Stop") - self.deinitialise() - except (AttributeError, TypeError): - pass - - - def background_update(self, delta: int): - """ This is called from the main loop of the BadgeOS to allow the app to do any background processing it needs to do. """ - if (self.config is None) or not self._pwm_setup: - # if we are not properly initialised then do not attempt to do anything - return - # Check keep alive period and turn off PWM outputs if exceeded - self._time_since_last_update += delta - if self._time_since_last_update > self._keep_alive_period: - self._time_since_last_update = 0 - if self._outputs_energised: - self._outputs_energised = False - # First time the keep alive period has expired so report it - if self._logging: - print(f"D:{self.config.port}:Timeout") - if self._pwm_setup: - for channel,pwm in enumerate(self.PWMOutput): - if pwm is not None: - try: - pwm.duty_u16(0) - except Exception as e: # pylint: disable=broad-except - print(self._pwm_log_string(channel) + f"Off failed {e}") - self.PWMOutput[channel] = None # Tidy Up - # we keep retriggering in case anything else has corrupted the PWM outputs - - - def get_version(self) -> int: - """ Get the version of the app - this is used to determine if an upgrade is required. """ - return VERSION - - - def get_status(self) -> bool: - """ Get the current status of the app - True if the app is running and able to respond to commands, False if not. """ - return (self._pwm_setup) - - - def set_logging(self, state: bool): - """ Set the logging state - True to enable logging, False to disable logging. """ - self._logging = state - - - def set_power(self, state: bool) -> bool: - """ Turn the SMPSU on or off. Returns the new power state. - Note that just because the SMPSU is turned off does not mean that the outputs are NOT energised as there could be external battery power. """ - if (self.config is None) or (state == self._power_state): - return False - if self._logging: - print(f"D:{self.config.port}:Power={'On' if state else 'Off'}") - #if self.get_booster_power(): - # if the power detect pin is high then the SMPSU has a power source so enable it - try: - self._power_control.init(mode=Pin.OUT) - self._power_control.value(state) - except Exception as e: # pylint: disable=broad-except - print(f"D:{self.config.port}:power control failed {e}") - return False - self._power_state = state - return self._power_state - - - def get_power(self) -> bool: - """ Get the current state of the SMPSU enable pin. Returns True if enabled, False if disabled. """ - return self._power_state - - - def set_keep_alive(self, period: int): - """ Set the keep alive period in milliseconds: - This is the period of time that can elapse without any commands being received before the app automatically - turns off all outputs to prevent damage to motors or servos if something goes wrong. """ - self._keep_alive_period = period - - - def set_freq(self, freq: int, channel: int | None = None) -> bool: - """ Set the PWM frequency for a specific output, or all outputs if channel is None. Returns True if successful, False if failed. - Use 50 to 200 for Servos and 5000 to 20000 for motors. """ - if not self._pwm_setup: - return False - for this_channel, pwm in enumerate(self.PWMOutput): - if (channel is None or this_channel == channel) and pwm is not None: - try: - pwm.freq(freq) - if self._logging: - print(self._pwm_log_string(this_channel) + f"{freq}Hz set") - except Exception as e: # pylint: disable=broad-except - print(self._pwm_log_string(this_channel) + f"set freq {freq} failed {e}") - print(f"pwm: {pwm}") - return False - self._freq[this_channel] = freq - return True - - - def _pwm_log_string(self, channel: int | None) -> str: - """ Helper method to generate a log string for a PWM output change. """ - if channel is None: - return f"D:{self.config.port}:PWM:[All]:" - return f"D:{self.config.port}:PWM[{channel}]:" - - - def set_servoposition(self, channel: int | None = None, position: int | None = None) -> bool: - """ Set the position for a specific servo output, or all servo outputs if channel is None. Returns True if successful, False if failed. - The pulse width for a specific servo output is position + the centre offset (in us) - Based on standard RC servos with centre at 1500us and range of 1000-2000us. - The position is a signed value from -1000 to 1000 which is scaled to 500-2500us. - This is a very wide range and may not be suitable for all servos, some will - only be happy with 1000-2000us (i.e. position in the range -500 to 500). """ - if not self._pwm_setup: - return False - if position is None: - # position == None -> Turn off PWM (some servos will then turn off, others will stay in last position) - if channel is None: - # channel == None -> Turn off all PWM outputs - for ch, pwm in enumerate(self.PWMOutput): - if pwm is not None: - try: - pwm.duty_ns(0) - except Exception as e: # pylint: disable=broad-except - print(self._pwm_log_string(ch) + f"Off failed {e}") - if self._logging: - print(self._pwm_log_string(None) + "Off") - self._outputs_energised = False - return True - elif channel < 0 or channel >= self._hexdrive_type.servos: - return False - else: - try: - self.PWMOutput[channel].duty_ns(0) - if self._logging: - print(self._pwm_log_string(channel) + "Off") - except Exception as e: # pylint: disable=broad-except - print(self._pwm_log_string(channel) + f"Off failed {e}") - return False - # check if all channels are now off and set outputs_energised accordingly - self._check_outputs_energised() - elif channel is not None: - if channel < 0 or channel >= self._hexdrive_type.servos: - return False - if abs(position) > _MAX_SERVO_RANGE: - return False - pulse_width_in_ns = (self._servo_centre[channel] + position) * 1000 # convert from us to ns - if self.PWMOutput[channel] is None: - # Channel hasn't been setup yet so we need to initialise it from scratch - self._freq[channel] = self._freq[channel] if (0 < self._freq[channel]) and (self._freq[channel] <= _MAX_SERVO_FREQ) else _DEFAULT_SERVO_FREQ - try: - self.PWMOutput[channel] = PWM(self.config.pin[channel], freq = self._freq[channel], duty_ns = pulse_width_in_ns) - if self._logging: - print(self._pwm_log_string(channel) + f"{self.PWMOutput[channel]} init") - except Exception as e: # pylint: disable=broad-except - # There are a finite number of PWM resources so it is possible that we run out - print(self._pwm_log_string(channel) + f"PWM(init) failed {e}") - return False - else: - # Channel is already setup so we just need to change the duty cycle and possibly the frequency if it is too high for the servo - try: - if _MAX_SERVO_FREQ < self.PWMOutput[channel].freq(): - # Ensure the frequency is suitable for use with Servos - # otherwise the pulse width will not be accepted - self._freq[channel] = _DEFAULT_SERVO_FREQ - self.PWMOutput[channel].freq(_DEFAULT_SERVO_FREQ) - if self._logging: - print(self._pwm_log_string(channel) + f"{_DEFAULT_SERVO_FREQ}Hz for Servo") - except Exception as e: # pylint: disable=broad-except - print(self._pwm_log_string(channel) + f"set freq failed {e}") - return False - # Scale servo position to PWM duty cycle (500-2500us) - try: - if 2000 < abs(pulse_width_in_ns - self.PWMOutput[channel].duty_ns()): # allow tolerance of 2us to avoid unnecessary updates - if self._logging: - print(self._pwm_log_string(channel) + f"{pulse_width_in_ns}ns") - self.PWMOutput[channel].duty_ns(pulse_width_in_ns) - if self._logging: - print(self._pwm_log_string(channel) + f"{self.PWMOutput[channel]} duty") - except Exception as e: # pylint: disable=broad-except - print(self._pwm_log_string(channel) + f"set duty failed {e}") - return False - - self._outputs_energised = True - self._time_since_last_update = 0 - return True - - - def set_servocentre(self, centre: int, channel: int | None = None) -> bool: - """ Set the centre position for a specific servo output, or all servo outputs if channel is None. Returns True if successful, False if failed. - Note this does not change the current position of the servo. - It will only affect the position next time it is set. - You can use this to trim the centre position of the servo. """ - if not self._pwm_setup: - return False - if channel is not None and (channel < 0 or channel >= self._hexdrive_type.servos): - return False - if centre < (_SERVO_CENTRE - _SERVO_MAX_TRIM ) or centre > (_SERVO_CENTRE + _SERVO_MAX_TRIM): - return False - if channel is None: - self._servo_centre = [centre] * 4 - else: - self._servo_centre[channel] = centre - return True - - - # Set pairs of PWM duty cycles in one go using a signed value per motor channel (0-65535) - def set_motors(self, outputs: tuple[int, ...]) -> bool: - """ Set the motor outputs using a signed value for each motor channel. Returns True if successful, False if failed. - The outputs are signed values in a tuple from -65535 to 65535 which are scaled to the PWM duty cycle range of 0-65535. - A positive value will drive the motor in one direction, a negative value will drive it in the opposite direction, - and a value of 0 will stop the motor. """ - if not self._pwm_setup or len(outputs) != self._hexdrive_type.motors: - return False - for motor, output in enumerate(outputs): - if abs(output) > 65535: - return False - if output == self._motor_output[motor]: - # no change in output for this motor so skip to the next one - continue - try: - # if the output is changing direction then we need to switch which signal is being driven as the PWM output - # rather than test for change of direction and also test that PWMOutput to be disabled exists we just do the latter check. - output_to_enable = (motor<<1) if output > 0 else ((motor<<1)+1) - output_to_disable = (motor<<1)+1 if output > 0 else (motor<<1) - # switch off the currently active output before switching the other one on to prevent both outputs being on at the same time - if self.PWMOutput[output_to_disable] is not None: - # we need to set the frequency of the output that is to be enabled to match the frequency of the output that is to be disabled - self._freq[output_to_enable] = self._freq[output_to_disable] - self.PWMOutput[output_to_disable].deinit() - self.PWMOutput[output_to_disable] = None - self.config.pin[output_to_disable].value(0) - self._set_pwmoutput(output_to_enable, abs(output)) - except Exception as e: # pylint: disable=broad-except - print(f"D:{self.config.port}:Motor{motor}:{output} set failed {e}") - self._motor_output[motor] = output - self._check_outputs_energised() - self._time_since_last_update = 0 - return True - - - # Set all 4 PWM duty cycles in one go using a tuple (0-65535) - def set_pwm(self, duty_cycles: tuple[int, ...]) -> bool: - """ Set the PWM duty cycle for all outputs at once using a tuple of values. Returns True if successful, False if failed. - The duty_cycles are values from 0 to 65535. """ - if not self._pwm_setup: - return False - self._outputs_energised = any(duty_cycles) - for channel, duty_cycle in enumerate(duty_cycles): - if not self._set_pwmoutput(channel, duty_cycle): - return False - self._time_since_last_update = 0 - return True - - -# -------------------------------------------------- -# Private methods for internal use only. -# -------------------------------------------------- - - def _pwm_init(self) -> bool: - self._pwm_setup = False - # HS Pins - if self.config.pin is not None and len(self.config.pin) == 4: - # Allocate PWM generation to pins - for channel, _ in enumerate(self.config.pin): - self._freq[channel] = 0 - if self._hexdrive_type is not None: - if channel < (2 * self._hexdrive_type.motors): - # First channels are for motors (can be 0, 1 or 2 motors) - if 0 == channel % 2: - # initialise motor PWM output on even channel - self._motor_output[(channel>>1)] = 0 - self._freq[channel] = _DEFAULT_PWM_FREQ - #print(f"D:{self.config.port}:Motor PWM[{channel}]") - else: - # ignore the motor PWM output on odd channel - we will switch it on when needed - pass - elif channel < ((2 * self._hexdrive_type.motors) + self._hexdrive_type.servos): - # Remaining channels are for servos (can be 4, 2 or 0 servos - self._freq[channel] = _DEFAULT_SERVO_FREQ - #print(f"D:{self.config.port}:Servo PWM[{channel}]") - else: - # ignore the remaining channels - we will switch them on when needed - pass - if 0 < self._freq[channel]: - if not self._set_pwmoutput(channel, 0): - return False - self._pwm_setup = True - return self._pwm_setup - - - # De-initialise all PWM outputs - def _pwm_deinit(self): - for channel, pwm in enumerate(self.PWMOutput): - if pwm is not None: - try: - pwm.deinit() - except Exception: # pylint: disable=broad-except - pass - self.PWMOutput[channel] = None - self._freq[channel] = 0 - self._motor_output[(channel>>1)] = 0 - self._pwm_setup = False - - - # are any of the PWM outputs energised? - def _check_outputs_energised(self): - energised_output = False - for channel, pwm in enumerate(self.PWMOutput): - if pwm is not None: - try: - if 0 < pwm.duty_ns(): - energised_output = True - break - except Exception as e: # pylint: disable=broad-except - print(self._pwm_log_string(channel) + f"Check failed {e}") - if self._outputs_energised != energised_output: - if self._logging: - print(f"D:{self.config.port}:Outputs {'Energised' if energised_output else 'De-energised'}") - self._outputs_energised = energised_output - - - # Set a single PWM duty cycle (0-65535) for a specific output - # if the channel has not been setup yet then we initialise it from scratch, otherwise we just change the duty cycle - def _set_pwmoutput(self, channel: int, duty_cycle: int) -> bool: - if duty_cycle < 0 or duty_cycle > 65535: - return False - try: - if self.PWMOutput[channel] is None: - # Channel hasn't been setup yet so we need to initialise it from scratch - self.PWMOutput[channel] = PWM(self.config.pin[channel], freq = self._freq[channel], duty_u16 = duty_cycle) - if self._logging: - print(self._pwm_log_string(channel) + f"{self.PWMOutput[channel]} init") - elif duty_cycle != self.PWMOutput[channel].duty_u16(): - self.PWMOutput[channel].duty_u16(duty_cycle) - if self._logging: - print(self._pwm_log_string(channel) + f"{duty_cycle}") - except Exception as e: # pylint: disable=broad-except - print(self._pwm_log_string(channel) + f"set {duty_cycle} failed {e}") - return False - return True - - - def _check_port_for_hexdrive(self, port: int) -> HexDriveType | None: - #just read the part of the header which contains the PID - try: - pid_bytes = self.config.i2c.readfrom_mem(_EEPROM_ADDR, _PID_ADDR, 2, addrsize = (8*_EEPROM_NUM_ADDRESS_BYTES)) - except OSError as e: # pylint: disable=broad-except - # no EEPROM on this port - print(f"D:{port}:EEPROM error: {e}") - return None - # check the MSByte of PID for HexDrive Family - if len(pid_bytes) < 2: - return None - if pid_bytes[1] != _HEXDRIVE_PID_MSB: - return None - # check if this is a HexDrive header by scanning the HEXDRIVE_TYPES list - for _, hexpansion_type in enumerate(_HEXDRIVE_TYPES): - if pid_bytes[0] == hexpansion_type.pid: - return hexpansion_type - # we are not interested in this type of hexpansion - return None - - - def _parse_version(self, version): - """ Parse a version string, e.g. that of BadgeOS, into a list of components for comparison. Handles versions in the format v1.9.0-beta.1+build.123 - The version is split into components based on the delimiters '.' '-' and '+'.""" - #pre_components = ["final"] - #build_components = ["0", "000000z"] - #build = "" - components = [] - if "+" in version: - version, build = version.split("+", 1) # pylint: disable=unused-variable - # build_components = build.split(".") - if "-" in version: - version, pre_release = version.split("-", 1) # pylint: disable=unused-variable - # if pre_release.startswith("rc"): - # # Re-write rc as c, to support a1, b1, rc1, final ordering - # pre_release = pre_release[1:] - # pre_components = pre_release.split(".") - version = version.strip("v").split(".") - components = [int(item) if item.isdigit() else item for item in version] - #components.append([int(item) if item.isdigit() else item for item in pre_components]) - #components.append([int(item) if item.isdigit() else item for item in build_components]) - return components - - -__app_export__ = HexDriveApp diff --git a/README.md b/README.md index e863449..ada7594 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # BadgeBot app -Companion app for the HexDrive hexpansion. Supports 2 brushed DC motors, 4 RC servos, 1 motor + 2 servos. Features Logo-style motor programming, PID line following with automatic gain tuning, I²C sensor testing, servo test mode, and persistent settings management. +Companion app for the HexDrive hexpansion. Supports 2 brushed DC motors, 4 RC servos (2 for HexDrive2), 1 motor + 2 servos (1 for HexDrive2). Features Logo-style motor programming, PID line following with automatic gain tuning, I²C sensor testing, servo test mode, and persistent settings management. This guide is current for BadgeBot version 1.5 @@ -16,10 +16,9 @@ If your HexDrive software (stored on the EEPROM on the hexpansion) is not the la - 1 Motor and 2 Servos - Unknown -The board can drive 2 brushed DC motors, 4 RC servos, 1 DC motor and 2 servos. Once you have selected the desired 'flavour' - please confirm by pressing the "C" (confirm) button. - -There must be a HexDrive board plugged in and running the latest software to use the BadgeBot app. If this is not the case then you will see a warning that you need a HexDrive with a reference to this repo. + +There must be a HexDrive board plugged in and running the latest software to use the BadgeBot app. If this is not the case then you will see a warning that you need a HexDrive with a reference to this repo. ### Main Menu ### @@ -71,9 +70,9 @@ When running from badge power the current available is limited - the best way to The maximum allowed servo range is VERY WIDE - most Servos will not be able to cope with this, so you probably want to reduce the ```servo_range``` setting to suit your servos. -Each Servo or Motor driver requires a PWM signals to control it, so a single HexDrive takes up four PWM resources on the ESP32. As there are 8 such resources, the 'flavour' of your HexDrives will determine how many you can run simultaneously as long as you don't have any other hexpansions or applications using PWM resources. Two '4 Servo' or 'Unknown' flavour HexDrives will use up all the available PWM channels, whereas you can run up to 4 HexDrives in '2 Motor' flavour. (While each motor driver does actually require two PWM signals we have been able to reduce this to one by swapping it between the active signal when the motor direction changes.) +Each Servo or Motor driver requires a PWM signal to control it, so a single HexDrive can take upto four PWM resources on the ESP32. As there are 8 such resources, the 'flavour' of your HexDrives will determine how many you can run simultaneously as long as you don't have any other hexpansions or applications using PWM resources. Two '4 Servo' flavour HexDrives will use up all the available PWM channels, whereas you can run up to 4 HexDrives in '2 Motor' flavour. (While each motor driver does actually require two PWM signals we have been able to reduce this to one by swapping it between the active signal when the motor direction changes.) -If you unplug a HexDrive the PWM resources will be released immediately so you can move them around the badge easily. +If you unplug a HexDrive the PWM resources will be released immediately so you can move them around the badge easily. ### Install guide @@ -85,6 +84,7 @@ This repo contains lots of files that you don't need on your badge to use a HexD + metadata.json + app.py or app.mpy + EEPROM/hexdrive.mpy ++ EEPROM/hexdrive2.mpy + utils.mpy + hexpansion_mgr.mpy + motor_controller.mpy @@ -100,10 +100,7 @@ This repo contains lots of files that you don't need on your badge to use a HexD + sensors/__init__.mpy + sensors/sensor_base.mpy + sensors/vl53l0x.mpy -+ sensors/vl6180x.mpy -+ sensors/tcs3472.mpy -+ sensors/tcs3430.mpy -+ sensors/opt4048.mpy ++ sensors/opt4060.mpy ### Hexpansion Recovery ### @@ -111,7 +108,7 @@ This repo contains lots of files that you don't need on your badge to use a HexD If you have issues with a HexDrive, or for that matter any hexpansion fitted with an EEPROM, e.g. a software incompatibility with a particular badge software version, you can reset the EEPROM back to blank as follows: 1) Plug in the hexpansion to Slot 1 (will work with any slot but you have to change the "1" below to the slot number. 2) Connect your favourite Terminal program to the COM port presented by the Badge over USB. -3) Press "Ctrl" & "C" simultaneously. i.e. "Ctrl-C" +3) Press "Ctrl" & "C" simultaneously. i.e. "Ctrl-C" 4) You should now be presented with a prompt ">>>" which is called the python REPL. At this type in the following lines (the HexDrive EEPROM is 8kbytes so requires 16 bit addressing, hence the ```addrsize=16``` other hexpansions may use smaller EEPROMS where this is not required): ``` from machine import I2C @@ -214,6 +211,38 @@ PYTHONPATH=/path/to/badge-2024-software ../.venv-wsl310/bin/python -m pytest tes ### Best practise Run `isort` on in-app python files. Check `pylint` for linting errors. +### Minification + +Hexpansion apps stored on EEPROM are minified before being compiled to `.mpy` to reduce their on-badge footprint. The following files are minified: + +| Source | Artifact | +|--------|----------| +| `vendor/HexDrive2/hexdrive2.py` | `EEPROM/hexdrive2.mpy` | +| `vendor/HexDrive/hexdrive.py` | `EEPROM/hexdrive.mpy` | + +The pipeline uses `dev/minify.py` which: +1. Renames internal `self.*` attributes to short names via an AST transform (source stays readable) +2. Strips docstrings with `python-minifier` +3. Compiles with `mpy-cross -O2` + +Typical savings are ~5% compared with compiling from source directly. + +The minifier is invoked **automatically** by `dev/download_to_device.py` for any `ModuleSpec` that has `minify=True`. You do not need to run it manually during normal development. + +To run it standalone and see a before/after size comparison for all minified modules: +``` +python dev/minify.py +``` + +Or to minify a single file (as `download_to_device.py` does): +``` +python dev/minify.py --source vendor/HexDrive/hexdrive.py --artifact EEPROM/hexdrive.mpy +``` + +`python-minifier` is listed in `dev/dev_requirements.txt` and is installed as part of the standard dev-environment setup. + +Intermediate build artefacts (`*.min.py`, `*.renamed.py`) are listed in `.gitignore` and should not be committed. + ### Regenerating QR Code QR generation is a development-time task and is intentionally kept out of normal runtime loading for the app. diff --git a/app.py b/app.py index f2bd957..8c41ec7 100644 --- a/app.py +++ b/app.py @@ -2,7 +2,7 @@ import asyncio import sys import time -from math import cos, pi +from math import sin, cos, pi import ota import settings @@ -21,6 +21,8 @@ from machine import Pin import app +from .bluetooth_mgr import bluetooth, RobotBLE, ble_process_command, enable_ble_logging, disable_ble_logging, get_ble_motor_override + # If you could use hard=True in setting up a Pin IRQ hander, which you can't as of BadgeOS V1.10, then it is recommended to # allocate the emergency exception buffer to prevent crashes due to OSError: Out of memory when an interrupt occurs and # there is no memory available to handle the exception. @@ -28,14 +30,13 @@ #micropython.alloc_emergency_exception_buf(100) from .utils import draw_logo_animated, parse_version -from .EEPROM.hexdrive import VERSION as HEXDRIVE_APP_VERSION +HEXDRIVE_APP_VERSION = 6 +HEXDRIVE2_APP_VERSION = 1 -_SETTINGS_NAME_PREFIX = "badgebot." # Prefix for settings keys in EEPROM +SETTINGS_NAME_PREFIX = "badgebot." # Prefix for settings keys in EEPROM APP_VERSION = "1.5" # BadgeBot App Version Number -_DIAG_PORT = None # Hexpansion port to use for diagnostic timing measurements - # If you change the URL then you will need to regenerate the QR code # using the generate_qr_code.py script, and update the _QR_CODE constant below with the new code generated for your URL _QR_CODE = [ @@ -73,6 +74,7 @@ # Timings MOTOR_PWM_FREQ = 20000 # 20kHz is a good default for motors as it is above the audible range for most people and works with most motors and ESC +MOTOR_POWER_SCALE_FACTOR = 512 # Settings store motor power / acceleration divided by this; multiply back to get 0-65535 PWM values _LONG_PRESS_MS = 750 # Time for long button press to register, in ms _RUN_COUNTDOWN_MS = 5000 # Time after running program until drive starts, in ms _AUTO_REPEAT_MS = 200 # Time between auto-repeats, in ms @@ -103,6 +105,7 @@ #Misceallaneous Settings _LOGGING = False +_BLE_LOGGING = False _IS_SIMULATOR = sys.platform != "esp32" # True when running in the simulator, not on real badge hardware _FWD_DIR_DEFAULT = 0 _FRONT_FACE_DEFAULT = 0 @@ -151,14 +154,14 @@ def _try_import(module_name, *attr_names): return nones HexpansionMgr, HexpansionType, _hexpansion_init_settings = _try_import('hexpansion_mgr', 'HexpansionMgr', 'HexpansionType', 'init_settings') -SettingsMgr, MySetting = _try_import('settings_mgr', 'SettingsMgr', 'MySetting') -MotorMovesMgr, _motor_moves_init_settings = _try_import('motor_moves', 'MotorMovesMgr', 'init_settings') -ServoTestMgr, _servo_test_init_settings = _try_import('servo_test', 'ServoTestMgr', 'init_settings') -LineFollowMgr, _line_follow_init_settings = _try_import('line_follow', 'LineFollowMgr', 'init_settings') -(AutotuneMgr,) = _try_import('autotune_mgr', 'AutotuneMgr') -SensorTestMgr, _sensor_test_init_settings = _try_import('sensor_test', 'SensorTestMgr', 'init_settings') -AutoDriveMgr, _autodrive_init_settings = _try_import('autodrive', 'AutoDriveMgr', 'init_settings') - +SettingsMgr, MySetting = _try_import('settings_mgr', 'SettingsMgr', 'MySetting') +MotorMovesMgr, _motor_moves_init_settings = _try_import('motor_moves', 'MotorMovesMgr', 'init_settings') +ServoTestMgr, _servo_test_init_settings = _try_import('servo_test', 'ServoTestMgr', 'init_settings') +LineFollowMgr, _line_follow_init_settings = _try_import('line_follow', 'LineFollowMgr', 'init_settings') +(AutotuneMgr,) = _try_import('autotune_mgr', 'AutotuneMgr') +SensorTestMgr, _sensor_test_init_settings = _try_import('sensor_test', 'SensorTestMgr', 'init_settings') +AutoDriveMgr, _autodrive_init_settings = _try_import('autodrive', 'AutoDriveMgr', 'init_settings') +emit_diagnostics_output, set_diagnostics_output = _try_import('diagnostics', 'diagnostics_output', 'set_diagnostics_output') class BadgeBotApp(app.App): # pylint: disable=no-member """Main application class for BadgeBot. Manages overall state, user input, and delegates to functional area managers for specific features.""" @@ -187,8 +190,11 @@ def __init__(self): self.message: list = [] self.message_colours: list = [] self.message_type: str | None = None + self.message_return_state: int | None = None self.current_menu: str | None = None self.menu: Menu | None = None + self._main_menu_position: int = 0 + self._settings_menu_position: int = 0 self.scroll_mode_enabled: bool = False # Whether pressing the "C" button can toggle scroll mode on/off, which allows the user to scroll through lines on the display. self.scroll_ignore_next_c_button: bool = False # Used to ignore the "C" button event that triggers scroll mode on, otherwise it would immediately toggle scroll mode off again self.is_scroll: bool = False # Whether we are in scroll mode - this is displayed by a green border around the screen @@ -207,6 +213,7 @@ def __init__(self): # General settings self.settings['brightness'] = MySetting(self.settings, _BRIGHTNESS, 0.1, 1.0) self.settings['logging'] = MySetting(self.settings, _LOGGING, False, True) + self.settings['ble_logging'] = MySetting(self.settings, _BLE_LOGGING, False, True) # Direction settings self.settings['motor1_dir'] = MySetting(self.settings, _FWD_DIR_DEFAULT, 0, 1, labels=_MOTOR_DIRECTION_LABELS) self.settings['motor2_dir'] = MySetting(self.settings, _FWD_DIR_DEFAULT, 0, 1, labels=_MOTOR_DIRECTION_LABELS) @@ -233,7 +240,7 @@ def __init__(self): # make use of special characters if running on compatible badge s/w version version_triplet = tuple(part if isinstance(part, int) else 0 for part in (ver[:3] if ver is not None else [])) - if len(version_triplet) == 3 and version_triplet > (1, 10, 0): + if len(version_triplet) == 3 and version_triplet > (3, 0, 0): # font has not yet been updated... self.special_chars = { 'up': "\u25B2", # up arrow # 'down': "\u25BC", # down arrow - has always existed 'left': "\u25C0", # left arrow @@ -245,52 +252,44 @@ def __init__(self): # Hexpansion related - SEE ALSO hexpansion_mgr to update _SINGLE_PORT_HEXPANSION_REFS # pid name vid eeprom total size eeprom page size app mpy name app mpy version app name motors servos sensors sub_type assert HexpansionType is not None - self.HEXPANSION_TYPES = [HexpansionType(0xCBCB, "HexDrive", app_mpy_name="hexdrive.mpy", app_mpy_version=HEXDRIVE_APP_VERSION, app_name="HexDriveApp", motors=2, servos=4, sub_type="Uncommitted" ), - HexpansionType(0xCBCA, "HexDrive", app_mpy_name="hexdrive.mpy", app_mpy_version=HEXDRIVE_APP_VERSION, app_name="HexDriveApp", motors=2, sub_type="2 Motor" ), - HexpansionType(0xCBCC, "HexDrive", app_mpy_name="hexdrive.mpy", app_mpy_version=HEXDRIVE_APP_VERSION, app_name="HexDriveApp", servos=4, sub_type="4 Servo" ), - HexpansionType(0xCBCD, "HexDrive", app_mpy_name="hexdrive.mpy", app_mpy_version=HEXDRIVE_APP_VERSION, app_name="HexDriveApp", motors=1, servos=2, sub_type="1 Mot 2 Srvo" ), - HexpansionType(0x0100, "HexSense", vid=0xCBCB, eeprom_total_size=65536, eeprom_page_size=128, sensors=2, sub_type="2 Line Sensors"), - HexpansionType(0x0200, "HexDriveV2", vid=0xCBCB, eeprom_total_size=32768, eeprom_page_size= 64, app_mpy_name="hexdrive.mpy", app_mpy_version=HEXDRIVE_APP_VERSION, app_name="HexDriveApp", motors=2, servos=2, sub_type="Uncommitted" ), - HexpansionType(0x0201, "HexDriveV2", vid=0xCBCB, eeprom_total_size=32768, eeprom_page_size= 64, app_mpy_name="hexdrive.mpy", app_mpy_version=HEXDRIVE_APP_VERSION, app_name="HexDriveApp", motors=2, sub_type="2 Motor" ), - HexpansionType(0x0202, "HexDriveV2", vid=0xCBCB, eeprom_total_size=32768, eeprom_page_size= 64, app_mpy_name="hexdrive.mpy", app_mpy_version=HEXDRIVE_APP_VERSION, app_name="HexDriveApp", servos=2, sub_type="2 Servo" ), - HexpansionType(0x0300, "HexTest", vid=0xCBCB, eeprom_total_size=65536, eeprom_page_size=128), - HexpansionType(0x0400, "HexDiag", vid=0xCBCB, eeprom_total_size=65536, eeprom_page_size=128), - #HexpansionType(0x1295, "GPS", app_mpy_name="gps.mpy", app_mpy_version=1, app_name="GPSApp"), # eeprom_total_size= 2048, eeprom_page_size= 16), - #HexpansionType(0xD15C, "Flopagon", eeprom_total_size= 2048, eeprom_page_size= 16), # EEPROM too small for the app - #HexpansionType(0xCAFF, "Club Mate", eeprom_total_size= 8192, eeprom_page_size= 32, app_mpy_name="caffeine.mpy", app_name="CaffeineJitter"), - - HexpansionType(0x0000, "Unknown", sub_type=""), # Virtual type to represent unrecognised hexpansions - HexpansionType(0xFFFF, "Blank", sub_type="")] # Virtual type to represent blank EEPROMs + self.HEXPANSION_TYPES = [HexpansionType(0xCBCB, "HexDrive", app_mpy_name="hexdrive", app_mpy_version=HEXDRIVE_APP_VERSION, app_name="HexDriveApp", motors=2, servos=4, sub_type="Uncommitted" ), + HexpansionType(0xCBCA, "HexDrive", app_mpy_name="hexdrive", app_mpy_version=HEXDRIVE_APP_VERSION, app_name="HexDriveApp", motors=2, sub_type="2 Motor" ), + HexpansionType(0xCBCC, "HexDrive", app_mpy_name="hexdrive", app_mpy_version=HEXDRIVE_APP_VERSION, app_name="HexDriveApp", servos=4, sub_type="4 Servo" ), + HexpansionType(0xCBCD, "HexDrive", app_mpy_name="hexdrive", app_mpy_version=HEXDRIVE_APP_VERSION, app_name="HexDriveApp", motors=1, servos=2, sub_type="1 Mot 2 Srvo" ), + + HexpansionType(0x10C8, "HexDrive2", vid=0xCBCB, eeprom_total_size=32768, eeprom_page_size= 64, app_mpy_name="hexdrive2", app_mpy_version=HEXDRIVE2_APP_VERSION, app_name="HexDriveApp", motors=2, servos=2, sub_type="Uncommitted" ), + HexpansionType(0x10C9, "HexDrive2", vid=0xCBCB, eeprom_total_size=32768, eeprom_page_size= 64, app_mpy_name="hexdrive2", app_mpy_version=HEXDRIVE2_APP_VERSION, app_name="HexDriveApp", servos=2, sub_type="2 Servo" ), + HexpansionType(0x10CA, "HexDrive2", vid=0xCBCB, eeprom_total_size=32768, eeprom_page_size= 64, app_mpy_name="hexdrive2", app_mpy_version=HEXDRIVE2_APP_VERSION, app_name="HexDriveApp", motors=2, sub_type="2 Motor" ), + HexpansionType(0x11CE, "HexDrive2", vid=0xCBCB, eeprom_total_size=32768, eeprom_page_size= 64, app_mpy_name="hexdrive2", app_mpy_version=HEXDRIVE2_APP_VERSION, app_name="HexDriveApp", motors=1, sub_type="Left Motor" ), + HexpansionType(0x12CE, "HexDrive2", vid=0xCBCB, eeprom_total_size=32768, eeprom_page_size= 64, app_mpy_name="hexdrive2", app_mpy_version=HEXDRIVE2_APP_VERSION, app_name="HexDriveApp", motors=1, sub_type="Right Motor" ), + HexpansionType(0x10CF, "HexDrive2", vid=0xCBCB, eeprom_total_size=32768, eeprom_page_size= 64, app_mpy_name="hexdrive2", app_mpy_version=HEXDRIVE2_APP_VERSION, app_name="HexDriveApp", motors=1, servos=1, sub_type="1 Mot 1 Srvo" ), + + HexpansionType(0x2000, "HexSense", vid=0xCBCB, eeprom_total_size=65536, eeprom_page_size=128, sub_type="Line Follow" ), + HexpansionType(0x4000, "HexDiag", vid=0xCBCB, eeprom_total_size=65536, eeprom_page_size=128, sub_type="Scope Pins" ), + HexpansionType(0x5000, "HexAudio", vid=0xCBCB, eeprom_total_size=8192, eeprom_page_size= 32, sub_type="Output Only" )] self.HEXDRIVE_HEXPANSION_INDEX = 0 # Index in the HEXPANSION_TYPES list which corresponds to the basic HexDrive type - self.HEXDRIVE_V2_HEXPANSION_INDEX = 5 # Index in the HEXPANSION_TYPES list which corresponds to the basic HexDrive V2 type - self.HEXSENSE_HEXPANSION_INDEX = 4 # Index in the HEXPANSION_TYPES list which corresponds to the HexSense type - self.HEXTEST_HEXPANSION_INDEX = 8 # Index in the HEXPANSION_TYPES list which corresponds to the HexTest type - self.HEXDIAG_HEXPANSION_INDEX = 9 # Index in the HEXPANSION_TYPES list which corresponds to the HexDiag type - #self.HEXGPS_HEXPANSION_INDEX = 10 # Index in the HEXPANSION_TYPES list which corresponds to the HexGPS type - - self.UNRECOGNISED_HEXPANSION_INDEX = len(self.HEXPANSION_TYPES) - 2 # Index in the HEXPANSION_TYPES list which corresponds to unrecognised hexpansion types MUST BE LAST NON-BLANK ENTRY IN THE LIST - self.BLANK_HEXPANSION_INDEX = len(self.HEXPANSION_TYPES) - 1 # Index in the HEXPANSION_TYPES list which corresponds to blank EEPROMs + self.HEXDRIVE_V2_HEXPANSION_INDEX = 4 # Index in the HEXPANSION_TYPES list which corresponds to the basic HexDrive2 type + self.HEXSENSE_HEXPANSION_INDEX = 10 # Index in the HEXPANSION_TYPES list which corresponds to the HexSense type + self.HEXDIAG_HEXPANSION_INDEX = 11 # Index in the HEXPANSION_TYPES list which corresponds to the HexDiag type + self.HEXAUDIO_HEXPANSION_INDEX = 12 # Index in the HEXPANSION_TYPES list which corresponds to the HexAudio type + self.hexpansion_update_required: bool = False # flag from async to main loop - self.hexdrive_hexpansion_types = [0,1,2,3,5,6,7] # indices in the HEXPANSION_TYPES list which correspond to HexDrive variants - used to check if a detected hexpansion is a HexDrive and to set up the motor and servo counts accordingly + self.hexdrive_hexpansion_types = [0,1,2,3,4,5,6,7,8,9] # indices in the HEXPANSION_TYPES list which correspond to HexDrive variants - used to check if a detected hexpansion is a HexDrive and to set up the motor and servo counts accordingly # HexDrive hexpansion - has an app which we use to control the motors and servos self.hexdrive_ports = [] self.hexdrive_apps = [] - # HexSense hexpansion - only a prototype at present - self.hexsense_port = None # Store the HexpansionConfig of the HexSense that is providing the line sensors - - # HexTest hexpansion - a prototype hexpansion with phototransistors and IR LEDs that we use for testing and diagnostics - # including timing measurements for the rotation rate measurement feature in the Sensor Test - self.hextest_port = None + # HexAudio hexpansion + self.hexaudio_port = None # Store the HexpansionConfig of the HexAudio that is providing the audio output - # GPS hexpansion - #self.hexgps_port = None + # HexSense hexpansion - prototype line sensor expansion + self.hexsense_port = None # Diagnostics hexpansion - self.hexdiag_port = _DIAG_PORT + self.hexdiag_port = None self._diag_config = None self.hexdiag_setup() @@ -351,6 +350,46 @@ def __init__(self): # This version is compatible with the simulator asyncio.get_event_loop().create_task(self._gain_focus(RequestForegroundPushEvent(self))) + # BluetoothLE setup + self._ble = bluetooth.BLE() + self._ble_controller = RobotBLE(self._ble, name="BadgeBot") + # Register the command processor + self._ble_controller.on_write(ble_process_command) + + # Apply BLE logging setting now that _ble_controller exists + if self.ble_logging: + enable_ble_logging(self._ble_controller) + +# TESTING I2S START + if False: + from machine import I2S + SR = 44100; F_L = 882; F_R = 441 + n_l = SR // F_L + n_r = SR // F_R + # need to generate a buffer that is a multiple of both n_l and n_r to avoid stuttering in the output, so we take the least common multiple which for two integers is (a*b)//gcd(a,b) + n = (n_l * n_r) // 1 # gcd is 1 for these frequencies, so this is just n_l * n_r + Amplitude = 16000 + buf = bytearray(n * 4) # 16-bit stereo + for i in range(n): + l = int(cos(2 * pi * i / n_l) * Amplitude) + r = int(cos(2 * pi * i / n_r) * Amplitude) + buf[i*4:i*4+2] = l.to_bytes(2, 'little', True) + buf[i*4+2:i*4+4] = r.to_bytes(2, 'little', True) + + i2s = I2S(0, sck=Pin(37), ws=Pin(38), sd=Pin(35), + mode=I2S.TX, bits=16, format=I2S.STEREO, + rate=SR, ibuf=20000) + print("Testing I2S output") + for _ in range(1000): + for _ in range(200): + try: + i2s.write(buf) + except Exception as e: + print(f"I2S write error: {e}") + i2s.deinit() + +#TESTING I2S END + if self.logging: print(f"BadgeBot App V{self.app_version} Initialised") @@ -378,6 +417,14 @@ def logging(self): return True + @property + def ble_logging(self): + """Convenience property to access ble_logging setting.""" + if 'ble_logging' in self.settings: + return self.settings['ble_logging'].v + return False + + @property def front_face(self): """Convenience property to access front_face setting representing the forward direction for movement.""" @@ -431,9 +478,9 @@ async def background_task(self): while True: cur_time = time.ticks_ms() delta_ticks = time.ticks_diff(cur_time, last_time) - self.diagnostics_output(0, 1) + diagnostics_output(0, 1) self.background_update(delta_ticks) - self.diagnostics_output(0, 0) + diagnostics_output(0, 0) await asyncio.sleep_ms(max (1, self.update_period - (time.ticks_ms() - cur_time))) # sleep for the remainder of the update period, accounting for time taken by background_update last_time = cur_time @@ -444,10 +491,19 @@ def background_update(self, delta: int): """Background update function that is called at a regular interval from the background task loop. It dispatches to the appropriate manager based on the current state, and if motor outputs are returned, it sends them to the HexDrive app.""" bg_fn = self._state_background_dispatch.get(self.current_state) - if bg_fn is not None: - output = bg_fn(delta) - if output is not None and len(self.hexdrive_apps) > 0: - self.hexdrive_apps[0].set_motors(self.apply_motor_directions(output)) + output = bg_fn(delta) if bg_fn is not None else None + + if len(self.hexdrive_apps) > 0: + # BLE direction buttons override the state's motor output while held, + # regardless of whether the current state produced any output. + max_pwr = self.settings['max_power'].v * MOTOR_POWER_SCALE_FACTOR if 'max_power' in self.settings else 49152 + ble_override = get_ble_motor_override(max_pwr) + if ble_override is not None: + output = ble_override + if output is not None: + if not self.hexdrive_apps[0].set_motors(self.apply_motor_directions(output)): + if self.logging: + print("Failed to set motor outputs to HexDrive app") @property @@ -511,15 +567,23 @@ def update_settings(self): if self.logging: print("Updating settings from EEPROM") for s in self.settings: - self.settings[s].v = settings.get(f"{_SETTINGS_NAME_PREFIX}{s}", self.settings[s].d) + self.settings[s].v = settings.get(f"{SETTINGS_NAME_PREFIX}{s}", self.settings[s].d) if self.logging: print(f"Setting {s} = {self.settings[s].v}") def fast_settings_update(self): """Update fast access settings from the main settings dictionary.""" + if self.logging: + print("Updating fast access settings") self._motor1_reversed: bool = self.settings['motor1_dir'].v != 0 self._motor2_reversed: bool = self.settings['motor2_dir'].v != 0 + ble_ctrl = getattr(self, '_ble_controller', None) + if ble_ctrl is not None: + if self.ble_logging: + enable_ble_logging(ble_ctrl) + else: + disable_ble_logging() def hexdiag_setup(self): @@ -556,7 +620,7 @@ def _pattern_management(self): def update(self, delta: int): """Main update function called from the main loop. Handles state transitions, user input, and delegates to functional area managers.""" - self.diagnostics_output(1, 1) + diagnostics_output(1, 1) if self.notification: self.notification.update(delta) @@ -621,7 +685,7 @@ def update(self, delta: int): except OSError as e: if self.logging: print(f"Error writing to LEDs: {e}") - self.diagnostics_output(1, 0) + diagnostics_output(1, 0) @@ -678,6 +742,10 @@ def _update_state_message(self, delta: int): # pylint: disable=unused-argum self.button_states.clear() # Reboot has been acknowledged by the user - unfortunately we can't actually reboot the badge from Python. return # leave the message on screen. + elif self.message_return_state is not None: + self.button_states.clear() + self.current_state = self.message_return_state + #TODO rework to use the new message_return_state elif self.message_type == "error" or self.message_type == "warning" or self.message_type == "hexpansion": # Message has been acknowledged by the user self.button_states.clear() @@ -693,6 +761,7 @@ def _update_state_message(self, delta: int): # pylint: disable=unused-argum self.message = [] self.message_colours = [] self.message_type = None + self.message_return_state = None else: # "CANCEL" button is handled in common for all MINIMISE_VALID_STATES so no custom code here # Show the warning screen for 10 seconds @@ -704,6 +773,7 @@ def _update_state_message(self, delta: int): # pylint: disable=unused-argum self.message = [] self.message_colours = [] self.message_type = None + self.message_return_state = None self.refresh = True elif self.current_state == STATE_LOGO: # LED management - to match rotating logo: @@ -775,7 +845,7 @@ def scroll(self, enable: bool): def draw(self, ctx): """Main draw function called from the main loop. Handles drawing the current state, including any notifications.""" - self.diagnostics_output(2, 1) + diagnostics_output(2, 1) if self.current_state == STATE_MENU and self.menu is not None: # These need to be drawn every frame as they contain animations @@ -828,7 +898,7 @@ def draw(self, ctx): if self.notification: self.notification.draw(ctx) - self.diagnostics_output(2, 0) + diagnostics_output(2, 0) @@ -843,8 +913,8 @@ def apply_motor_directions(self, output: tuple) -> tuple: """Negate individual motor outputs as per settings.""" output1, output2 = output output = (-output1 if self._motor1_reversed else output1, -output2 if self._motor2_reversed else output2) - if self.logging: - print(f"M:{output}") + #if self.logging: + # print(f"M:{output}") return output @@ -906,7 +976,7 @@ def return_to_menu(self, menu_name: str | None = None): self.refresh = True - def show_message(self, msg_content, msg_colours, msg_type = None): + def show_message(self, msg_content, msg_colours, msg_type = None, return_state: int | None = None): """Utility function to set the current state to the message display, and populate the message content and colours. The message_type can be used to indicate whether this is an 'error' (red) or 'warning' (green) message, which can affect both the display and the behaviour when the user acknowledges the message.""" if self.logging: print(f"Showing message: '{msg_content}' with type {msg_type}") @@ -914,6 +984,7 @@ def show_message(self, msg_content, msg_colours, msg_type = None): self.message = msg_content self.message_colours = msg_colours self.message_type = msg_type + self.message_return_state = return_state self.current_state = STATE_MESSAGE self.refresh = True @@ -994,6 +1065,7 @@ def set_menu(self, menu_name: str | None = "main"): #: Literal["main"]): does i menu_items, select_handler=self._main_menu_select_handler, back_handler=self._menu_back_handler, + position=self._main_menu_position, ) elif menu_name == MAIN_MENU_ITEMS[MENU_ITEM_SETTINGS] and self._settings_mgr is not None: # "Settings" # construct the settings menu @@ -1005,13 +1077,15 @@ def set_menu(self, menu_name: str | None = "main"): #: Literal["main"]): does i _settings_menu_items, select_handler=self._settings_menu_select_handler, back_handler=self._menu_back_handler, + position=self._settings_menu_position, ) # this appears to be able to be called at any time def _main_menu_select_handler(self, item: str, idx: int): if self.logging: - print(f"H:Main Menu {item} at index {idx}") + print(f"H:Main Menu {item} at index {idx} position {self.menu.position if self.menu else 'N/A'}") + self._main_menu_position = self.menu.position if self.menu else 0 if item == MAIN_MENU_ITEMS[MENU_ITEM_LINE_FOLLOWER]: # Line Follower # Check for required hardware and show message if not present, otherwise start the line follower manager and switch to follower state if self.num_motors == 0: @@ -1108,9 +1182,23 @@ def _settings_menu_select_handler(self, item: str, idx: int): def _menu_back_handler(self): if self.current_menu == "main": + self._main_menu_position = self.menu.position if self.menu else 0 self.minimise() # for submenus, just return to the main menu + if self.current_menu == MAIN_MENU_ITEMS[MENU_ITEM_SETTINGS]: + self._settings_menu_position = self.menu.position if self.menu else 0 self.set_menu() +def diagnostics_output(index: int, value: int): + """Output diagnostic values to the HS pins on the diagnostics hexpansion, for measurement with an oscilloscope""" + if emit_diagnostics_output is not None: + emit_diagnostics_output(index, value) + + +def __app_init__(app_instance): + """Register the active app instance as the shared diagnostics sink.""" + if set_diagnostics_output is not None: + set_diagnostics_output(app_instance.diagnostics_output) + __app_export__ = BadgeBotApp diff --git a/autodrive.py b/autodrive.py index 393d54c..7f6a529 100644 --- a/autodrive.py +++ b/autodrive.py @@ -99,12 +99,12 @@ def __init__(self, app, logging: bool = False): # ------------------------------------------------------------------ - + @property def logging(self) -> bool: """Whether to print debug logs from the AutoDriveMgr.""" return self._logging - + @logging.setter def logging(self, value: bool): self._logging = value @@ -128,8 +128,6 @@ def start(self) -> bool: for port in app.hexdrive_ports: if port != sensor_test.port_selected: ports_to_try.append(port) - if app.hexsense_port is not None and app.hexsense_port != sensor_test.port_selected: - ports_to_try.append(app.hexsense_port) for probe_port in ports_to_try: if sensor_test.open_sensor_port(probe_port): sensor_test.port_selected = probe_port @@ -259,7 +257,7 @@ def stop(self): self._mc_task.cancel() self._mc_task = None if self._mc is not None: - self._mc.stop() + self._mc.stop() self._active = False self.motor_output = (0, 0) self.target_output = (0, 0) diff --git a/autotune_mgr.py b/autotune_mgr.py index 351a9c9..2c6814c 100644 --- a/autotune_mgr.py +++ b/autotune_mgr.py @@ -51,7 +51,7 @@ def __init__(self, app, follower, logging: bool = False): self.follower = follower self.autotuner = None self._logging: bool = logging - if self._logging: + if self._logging: print("AutotuneMgr initialised") # ------------------------------------------------------------------ @@ -104,7 +104,7 @@ def start(self) -> bool: print("H:Failed to initialise HexDrive for autotune") app.notification = Notification("HexDrive Init Failed") return False - + # ------------------------------------------------------------------ # Begin tuning (called after countdown completes) @@ -160,12 +160,12 @@ def update(self, delta) -> bool: # pylint: disable=unused-argument app.current_state = STATE_COUNTDOWN app.refresh = True - if (self.follower.sample_time > 1000): + if self.follower.sample_time > 1000: sample_count = self.follower.line_sensors.sample_count_and_reset() self.follower.sensor_rate = int(((self.follower.sample_time / self.follower.line_sensors.num_sensors) * sample_count) // self.follower.sample_time) self.follower.sample_time = 0 - app.refresh = True - + app.refresh = True + return True @@ -178,7 +178,7 @@ def background_update(self, delta) -> tuple[int, int] | None: Returns motor output tuple, or None if not active.""" if self.autotuner is not None and self.autotuner.is_running: #self.follower.line_sensors.read() - self.follower.line_sensors.read_blocking() # wait for sensor reading + self.follower.line_sensors.read_blocking() # wait for sensor reading left_raw = self.follower.line_sensors.raw_value(0) right_raw = self.follower.line_sensors.raw_value(1) error = self.follower.compute_error(left_raw, right_raw) @@ -193,20 +193,21 @@ def background_update(self, delta) -> tuple[int, int] | None: def autotune_complete(self): - app = self._app - app.refresh = True - if self.autotuner.is_complete: - gains = self.autotuner.get_gains() - if gains is not None: - app.settings['pid_kp'].v = int(1000 * gains[0]) - app.settings['pid_ki'].v = int(1000 * gains[1]) - app.settings['pid_kd'].v = int(1000 * gains[2]) - app.settings['pid_kp'].persist() - app.settings['pid_ki'].persist() - app.settings['pid_kd'].persist() - if self._logging: - print(f"AUTOTUNE: Gains saved to settings: Kp={gains[0]:.4f} Ki={gains[1]:.6f} Kd={gains[2]:.4f}") - app.notification = Notification(" Tuning Complete") + if self.autotuner is not None: + app = self._app + app.refresh = True + if self.autotuner.is_complete: + gains = self.autotuner.get_gains() + if gains is not None: + app.settings['pid_kp'].v = int(1000 * gains[0]) + app.settings['pid_ki'].v = int(1000 * gains[1]) + app.settings['pid_kd'].v = int(1000 * gains[2]) + app.settings['pid_kp'].persist() + app.settings['pid_ki'].persist() + app.settings['pid_kd'].persist() + if self._logging: + print(f"AUTOTUNE: Gains saved to settings: Kp={gains[0]:.4f} Ki={gains[1]:.6f} Kd={gains[2]:.4f}") + app.notification = Notification(" Tuning Complete") # ------------------------------------------------------------------ @@ -229,13 +230,13 @@ def draw(self, ctx) -> bool: app.draw_message(ctx, ["PID Auto Tune:", status, f"Cross: {diag['crossings']}/{diag['target']}", - f"T={diag['elapsed']//1000}s", + f"T={int(diag['elapsed'])//1000}s", f"Rate: {self.follower.sensor_rate} sps"], [(1, 1, 0), (1, 1, 0), (0, 1, 1), (0.7, 0.7, 0.7), (1, 0, 1)], label_font_size) button_labels(ctx, cancel_label="Stop") elif self.autotuner.is_complete: diag = self.autotuner.get_diagnostics() - q = diag['quality'] + q = int(diag['quality']) q_colour = (0, 1, 0) if q >= 60 else (1, 1, 0) if q >= 30 else (1, 0, 0) app.draw_message(ctx, ["Tune Complete", diff --git a/bluetooth_mgr.py b/bluetooth_mgr.py new file mode 100644 index 0000000..0282cac --- /dev/null +++ b/bluetooth_mgr.py @@ -0,0 +1,225 @@ +# MicroPython BLE Robot Control + +import bluetooth +import struct +import sys +import time +from micropython import const + +# --- BLE Constants for Nordic UART Service (NUS) --- +_ADV_TYPE_FLAGS = const(0x01) +_ADV_TYPE_NAME = const(0x09) +_ADV_TYPE_UUID128_COMPLETE = const(0x07) + +_UART_UUID = bluetooth.UUID("6E400001-B5A3-F393-E0A9-E50E24DCCA9E") +_UART_TX = ( + bluetooth.UUID("6E400003-B5A3-F393-E0A9-E50E24DCCA9E"), + bluetooth.FLAG_NOTIFY, +) +_UART_RX = ( + bluetooth.UUID("6E400002-B5A3-F393-E0A9-E50E24DCCA9E"), + bluetooth.FLAG_WRITE | bluetooth.FLAG_WRITE_NO_RESPONSE, +) + +_UART_SERVICE = (_UART_UUID, (_UART_TX, _UART_RX)) + + +class RobotBLE: + def __init__(self, ble, name="Robot-Control"): + self._ble = ble + self._ble.active(True) + self._ble.irq(self._irq) + ((self._handle_tx, self._handle_rx),) = self._ble.gatts_register_services((_UART_SERVICE,)) + self._connections = set() + self._write_callback = None + self._payload = self._advertising_payload(name=name, services=[_UART_UUID]) + self._advertise() + + def _irq(self, event, data): + # Track connections and handle data reception + if event == 1: # _IRQ_CENTRAL_CONNECT + conn_handle, _, _ = data + self._connections.add(conn_handle) + print("BLE:Connected") + elif event == 2: # _IRQ_CENTRAL_DISCONNECT + conn_handle, _, _ = data + self._connections.remove(conn_handle) + self._advertise() + print("BLE:Disconnected") + elif event == 3: # _IRQ_GATTS_WRITE + conn_handle, value_handle = data + value = self._ble.gatts_read(value_handle) + if value_handle == self._handle_rx and self._write_callback: + self._write_callback(value) + + def _advertise(self, interval_us=500000): + print("BLE:Advertising...") + self._ble.gap_advertise(interval_us, adv_data=self._payload) + + def send_telemetry(self, text): + """Sends sensor data or diagnostic logs back to the phone app.""" + for conn_handle in self._connections: + try: + # Transmit data via the TX characteristic + self._ble.gatts_notify(conn_handle, self._handle_tx, text + "\n") + except Exception: + pass + + def on_write(self, callback): + self._write_callback = callback + + def is_connected(self): + """Returns True if at least one BLE central is connected.""" + return len(self._connections) > 0 + + def _advertising_payload(self, name=None, services=None): + + payload = bytearray() + + def _append(adv_type, value): + nonlocal payload + payload.append(len(value) + 1) + payload.append(adv_type) + payload.extend(value) + + _append(_ADV_TYPE_FLAGS, struct.pack("B", 0x06)) + + if name: + _append(_ADV_TYPE_NAME, name.encode('utf-8')) + + if services: + for s in services: + _append(_ADV_TYPE_UUID128_COMPLETE, bytes(s)) + + return payload + + +# --- Robot Logic --- + +# Direction buttons that can override motor output from the current state. +# '4' = stop, '5' = forward, '6' = backward, '7' = left, '8' = right. +_DRIVE_BUTTONS = frozenset('45678') + +# Currently-held BLE drive button, or None when no button is pressed. +_ble_active_button = None + + +def ble_process_command(data): + """ + Bluefruit Connect Control Pad sends data in the format: + !B <1=pressed/0=released> + Example: b'!B516' is Up Button Pressed + """ + global _ble_active_button + + command = data.decode().strip() + if not command.startswith("!B"): + return + + # Check button number and press state + button = command[2] + action = command[3] # '1' for press, '0' for release + + if button not in _DRIVE_BUTTONS: + return + + if action == '1': # Button pressed + _ble_active_button = button + if button == '4': + print("BLE: Stop") + elif button == '5': + print("BLE: Forward") + elif button == '6': + print("BLE: Backward") + elif button == '7': + print("BLE: Left") + elif button == '8': + print("BLE: Right") + else: # Button released — clear override only if it's the button we're tracking + if _ble_active_button == button: + _ble_active_button = None + print("BLE: Release") + + +def get_ble_motor_override(max_power: int): + """Return a (left, right) motor override tuple if a BLE drive button is + currently held, or None to let the current state control the motors. + + max_power should be the full-scale PWM value (0-65535). + """ + btn = _ble_active_button + if btn is None: + return None + if btn == '4': # Stop + return (0, 0) + if btn == '5': # Forward + return (max_power, max_power) + if btn == '6': # Backward + return (-max_power, -max_power) + if btn == '7': # Left + return (-max_power, max_power) + if btn == '8': # Right + return (max_power, -max_power) + return None + + +# --------------------------------------------------------------------------- +# BLE Logging - redirect sys.stdout so all print() calls are also forwarded +# over BLE when explicitly enabled. The rest of the codebase is untouched. +# --------------------------------------------------------------------------- + +class BleLogStream: + """Proxy for sys.stdout that tees complete log lines to a RobotBLE instance.""" + + def __init__(self, ble_controller, original_stdout): + self._ble = ble_controller + self._orig = original_stdout + self._line_buf = [] + + def write(self, text): + # Always write to the original stdout (USB/UART serial) + self._orig.write(text) + # Buffer characters and send each complete line to BLE + if '\n' in text: + parts = text.split('\n') + # First segment completes whatever is already in the buffer + self._line_buf.append(parts[0]) + line = ''.join(self._line_buf) + if line: + self._ble.send_telemetry(line) + # Any middle segments are self-contained complete lines + for part in parts[1:-1]: + if part: + self._ble.send_telemetry(part) + # The trailing segment starts a new partial line + self._line_buf = [parts[-1]] if parts[-1] else [] + else: + self._line_buf.append(text) + + def flush(self): + try: + self._orig.flush() + except AttributeError: + pass + + +_ble_log_stream = None +_orig_stdout = None + + +def enable_ble_logging(ble_controller): + """Redirect sys.stdout through BleLogStream so every print() is also sent via BLE.""" + global _ble_log_stream, _orig_stdout + if _ble_log_stream is None: + _orig_stdout = sys.stdout + _ble_log_stream = BleLogStream(ble_controller, _orig_stdout) + sys.stdout = _ble_log_stream + + +def disable_ble_logging(): + """Restore sys.stdout to serial-only output.""" + global _ble_log_stream, _orig_stdout + if _ble_log_stream is not None: + sys.stdout = _orig_stdout + _ble_log_stream = None + _orig_stdout = None \ No newline at end of file diff --git a/dev/build_release.py b/dev/build_release.py index ee239df..e1a6164 100644 --- a/dev/build_release.py +++ b/dev/build_release.py @@ -2,21 +2,29 @@ import os import subprocess import sys +from dataclasses import dataclass from pathlib import Path import mpy_cross + +@dataclass(frozen=True) +class ModuleSpec: + source: Path + artifact: Path + RUNTIME_MODULES = { "app", - "EEPROM/hexdrive", "autotune", "autotune_mgr", "settings_mgr", "hexpansion_mgr", + "bluetooth_mgr", "line_follow", "motor_moves", "servo_test", "utils", + "diagnostics", "motor_controller", "sensor_manager", "sensor_test", @@ -27,17 +35,22 @@ SENSOR_MODULES = { "sensors/__init__", "sensors/sensor_base", - "sensors/tcs3430", - "sensors/tcs3472", + #"sensors/tcs3430", + #"sensors/tcs3472", "sensors/vl53l0x", - "sensors/vl6180x", - "sensors/opt4048", + #"sensors/vl6180x", + "sensors/opt4060", "sensors/ina226", } files_to_mpy = {Path(f"{module}.py") for module in RUNTIME_MODULES} files_to_mpy.update({Path(f"{module}.py") for module in SENSOR_MODULES}) +EXTERNAL_MODULES = ( + ModuleSpec(Path("vendor/HexDrive/hexdrive.py"), Path("EEPROM/hexdrive.mpy")), + ModuleSpec(Path("vendor/HexDrive2/hexdrive2.py"), Path("EEPROM/hexdrive2.mpy")), +) + files_to_keep = { Path("app.py"), Path("tildagon.toml"), @@ -45,10 +58,26 @@ } files_to_keep.update({Path(f"{module}.mpy") for module in RUNTIME_MODULES}) files_to_keep.update({Path(f"{module}.mpy") for module in SENSOR_MODULES}) +files_to_keep.update({spec.artifact for spec in EXTERNAL_MODULES}) + +IGNORED_SOURCE_DIRS = (Path("vendor/HexDrive"), Path("vendor/HexDrive2")) def _construct_filepaths(dirname, filenames): return [Path(dirname, filename) for filename in filenames] +def _normalise_parts(path: Path) -> tuple[str, ...]: + return tuple(part for part in path.parts if part not in (".", "")) + +def _is_ignored_dir(dirname: str) -> bool: + parts = _normalise_parts(Path(dirname)) + if ".git" in parts: + return True + for ignored_dir in IGNORED_SOURCE_DIRS: + ignored_parts = _normalise_parts(ignored_dir) + if parts[: len(ignored_parts)] == ignored_parts: + return True + return False + def find_files(top_level_dir): walkerator = iter(os.walk(top_level_dir)) dirname, _, filenames = next(walkerator) @@ -56,8 +85,7 @@ def find_files(top_level_dir): all_files = _construct_filepaths(dirname, filenames) for dirname, _, filenames in walkerator: - # if dirname not in dirs_to_keep: - if dirname != "./.git" and ".git/" not in dirname: + if not _is_ignored_dir(dirname): all_files.extend(_construct_filepaths(dirname, filenames)) return all_files @@ -98,10 +126,15 @@ def find_files(top_level_dir): print(f"Mpy-ing file: {file}") mpy_cross.run(file, "-v") + for spec in EXTERNAL_MODULES: + print(f"Mpy-ing file: {spec.source} -> {spec.artifact}") + spec.artifact.parent.mkdir(parents=True, exist_ok=True) + mpy_cross.run(str(spec.source), "-v", "-o", str(spec.artifact)) + if not files_to_keep.issubset(found_files): raise FileNotFoundError(f"Some of {files_to_keep} are not found so assuming wrong directory. " "Please run this script from BadgeBot dir.") - + files_to_remove = found_files.difference(files_to_keep) if not force_mode: if input(f"About to remove {len(files_to_remove)} files from {os.getcwd()}, continue? y/n") != "y": diff --git a/dev/dev_requirements.txt b/dev/dev_requirements.txt index b1a9b1b..b2eaea4 100644 --- a/dev/dev_requirements.txt +++ b/dev/dev_requirements.txt @@ -1,5 +1,9 @@ pylint isort pytest +mpremote mpy-cross -micropython-esp32-stubs==1.27.0.post1 \ No newline at end of file +python-minifier +micropython-esp32-stubs==1.27.0.post1 +pygame +wasmtime diff --git a/dev/download_to_device.py b/dev/download_to_device.py index 0ca1f71..97f5f06 100644 --- a/dev/download_to_device.py +++ b/dev/download_to_device.py @@ -16,12 +16,14 @@ import json import os import re +import shutil import subprocess +import sys from dataclasses import dataclass from pathlib import Path -DEFAULT_APP_DIR_ON_DEVICE = ":apps/TeamRobotMad_BadgeBot" +DEFAULT_APP_DIR_ON_DEVICE = ":apps/TeamRobotmad_BadgeBot" STATE_DIR = Path(".deploy_state") STATE_PATH = STATE_DIR / "test_device_download_state.json" MPREMOTE_COMMAND_TIMEOUT = 20 @@ -33,17 +35,21 @@ class ModuleSpec: source: Path artifact: Path + minify: bool = False # Add new runtime modules here as the project grows. MODULES: tuple[ModuleSpec, ...] = ( - ModuleSpec(Path("EEPROM/hexdrive.py"), Path("EEPROM/hexdrive.mpy")), + ModuleSpec(Path("vendor/HexDrive/hexdrive.py"), Path("EEPROM/hexdrive.mpy"), minify=False), + ModuleSpec(Path("vendor/HexDrive2/hexdrive2.py"), Path("EEPROM/hexdrive2.mpy"), minify=False), ModuleSpec(Path("app.py"), Path("app.mpy")), ModuleSpec(Path("autotune.py"), Path("autotune.mpy")), ModuleSpec(Path("autotune_mgr.py"), Path("autotune_mgr.mpy")), ModuleSpec(Path("utils.py"), Path("utils.mpy")), + ModuleSpec(Path("diagnostics.py"), Path("diagnostics.mpy")), ModuleSpec(Path("settings_mgr.py"), Path("settings_mgr.mpy")), ModuleSpec(Path("hexpansion_mgr.py"), Path("hexpansion_mgr.mpy")), + ModuleSpec(Path("bluetooth_mgr.py"), Path("bluetooth_mgr.mpy")), ModuleSpec(Path("line_follow.py"), Path("line_follow.mpy")), ModuleSpec(Path("motor_moves.py"), Path("motor_moves.mpy")), ModuleSpec(Path("servo_test.py"), Path("servo_test.mpy")), @@ -53,11 +59,11 @@ class ModuleSpec: ModuleSpec(Path("autodrive.py"), Path("autodrive.mpy")), ModuleSpec(Path("sensors/__init__.py"), Path("sensors/__init__.mpy")), ModuleSpec(Path("sensors/sensor_base.py"), Path("sensors/sensor_base.mpy")), - ModuleSpec(Path("sensors/tcs3430.py"), Path("sensors/tcs3430.mpy")), - ModuleSpec(Path("sensors/tcs3472.py"), Path("sensors/tcs3472.mpy")), + #ModuleSpec(Path("sensors/tcs3430.py"), Path("sensors/tcs3430.mpy")), + #ModuleSpec(Path("sensors/tcs3472.py"), Path("sensors/tcs3472.mpy")), ModuleSpec(Path("sensors/vl53l0x.py"), Path("sensors/vl53l0x.mpy")), - ModuleSpec(Path("sensors/vl6180x.py"), Path("sensors/vl6180x.mpy")), - ModuleSpec(Path("sensors/opt4048.py"), Path("sensors/opt4048.mpy")), + #ModuleSpec(Path("sensors/vl6180x.py"), Path("sensors/vl6180x.mpy")), + ModuleSpec(Path("sensors/opt4060.py"), Path("sensors/opt4060.mpy")), ModuleSpec(Path("sensors/ina226.py"), Path("sensors/ina226.mpy")), ) @@ -77,6 +83,7 @@ class CommandFailed(RuntimeError): # Set to True by main() when --verbose is passed. _verbose: bool = False +_tool_commands: dict[str, str] = {} def _log(level: str, message: str) -> None: @@ -129,6 +136,64 @@ def _save_state(path: Path, state: dict[str, dict[str, str]]) -> None: file.write("\n") +def _resolve_tool_command(tool_name: str, *, repo_root: Path) -> str: + resolved = _tool_commands.get(tool_name) + if resolved is not None: + return resolved + + executable_dir = Path(sys.executable).resolve().parent + venv_dir_name = "Scripts" if os.name == "nt" else "bin" + alt_venv_dir_name = "bin" if os.name == "nt" else "Scripts" + candidate_dirs = [ + executable_dir, + repo_root / ".venv" / venv_dir_name, + repo_root / ".venv" / alt_venv_dir_name, + ] + candidate_names = [tool_name] + if os.name == "nt": + candidate_names = [f"{tool_name}.exe", f"{tool_name}.cmd", f"{tool_name}.bat", tool_name] + + searched_dirs: list[str] = [] + seen_dirs: set[str] = set() + for directory in candidate_dirs: + directory_key = str(directory).lower() if os.name == "nt" else str(directory) + if directory_key in seen_dirs: + continue + seen_dirs.add(directory_key) + searched_dirs.append(str(directory)) + + for candidate_name in candidate_names: + candidate_path = directory / candidate_name + if candidate_path.exists(): + resolved = str(candidate_path) + _tool_commands[tool_name] = resolved + return resolved + + for candidate_name in candidate_names: + resolved = shutil.which(candidate_name) + if resolved is not None: + _tool_commands[tool_name] = resolved + return resolved + + raise RuntimeError( + f"Could not find required tool '{tool_name}'. Checked {', '.join(searched_dirs)} and PATH. " + "Create the project .venv or install the tool globally." + ) + + +def _tool(tool_name: str) -> str: + resolved = _tool_commands.get(tool_name) + if resolved is None: + raise RuntimeError(f"Tool '{tool_name}' has not been initialised") + return resolved + + +def _initialise_tool_commands(repo_root: Path) -> None: + for tool_name in ("mpy-cross", "mpremote"): + resolved = _resolve_tool_command(tool_name, repo_root=repo_root) + _log("INFO", f"using {tool_name}: {resolved}") + + def _format_command(command: list[str]) -> str: return " ".join(f'"{part}"' if " " in part else part for part in command) @@ -153,6 +218,16 @@ def _run_command( check=False, timeout=timeout, ) + except FileNotFoundError as exc: + raise CommandFailed( + "\n".join( + [ + "Command could not be started because the executable was not found", + f"Command: {quoted}", + f"Missing executable: {exc.filename or command[0]}", + ] + ) + ) from exc except subprocess.TimeoutExpired as exc: stdout = (exc.stdout or "").rstrip() or "" stderr = (exc.stderr or "").rstrip() or "" @@ -195,7 +270,7 @@ def _find_connect_arg(mpremote_args: list[str]) -> int | None: def _list_mpremote_devices() -> list[str]: completed = _run_command( - ["mpremote", "devs"], + [_tool("mpremote"), "devs"], dry_run=False, timeout=MPREMOTE_PROBE_TIMEOUT, ) @@ -215,7 +290,7 @@ def _list_mpremote_devices() -> list[str]: def _probe_mpremote_device(port: str) -> bool: command = [ - "mpremote", + _tool("mpremote"), "connect", port, "exec", @@ -305,7 +380,7 @@ def _ensure_device_dir(dir_path: str, *, mpremote_args: list[str], dry_run: bool " os.mkdir(cur)" ) _run_command( - ["mpremote", *mpremote_args, "exec", exec_code], + [_tool("mpremote"), *mpremote_args, "exec", exec_code], dry_run=dry_run, timeout=MPREMOTE_COMMAND_TIMEOUT, ) @@ -357,11 +432,20 @@ def _compile_changed_modules( _log("SKP", f"compile {spec.source} (source unchanged)") continue - _log("INFO", f"compile {spec.source} -> {spec.artifact}") - _run_command( - ["mpy-cross", "-v", str(spec.source), "-o", str(spec.artifact)], - dry_run=dry_run, - ) + if spec.minify: + _log("INFO", f"minify+compile {spec.source} -> {spec.artifact}") + _run_command( + [sys.executable, "dev/minify.py", + "--source", str(spec.source), + "--artifact", str(spec.artifact)], + dry_run=dry_run, + ) + else: + _log("INFO", f"compile {spec.source} -> {spec.artifact}") + _run_command( + [_tool("mpy-cross"), "-v", str(spec.source), "-o", str(spec.artifact)], + dry_run=dry_run, + ) if not dry_run and not spec.artifact.exists(): raise RuntimeError(f"mpy-cross did not produce {spec.artifact}") @@ -411,7 +495,7 @@ def _get_device_files( f"print(json.dumps(_ls('{safe_path}')))" ) - command = ["mpremote", *mpremote_args, "exec", exec_code] + command = [_tool("mpremote"), *mpremote_args, "exec", exec_code] quoted = " ".join(f'"{p}"' if " " in p else p for p in command) _log("CMD", quoted) @@ -490,7 +574,7 @@ def _upload_changed_artifacts( _log("INFO", f"upload {spec.artifact} -> {app_dir}/{spec.artifact.as_posix()}") destination = f"{app_dir}/{spec.artifact.as_posix()}" - command = ["mpremote", *mpremote_args, "cp", str(spec.artifact), destination] + command = [_tool("mpremote"), *mpremote_args, "cp", str(spec.artifact), destination] _run_command(command, dry_run=dry_run, timeout=MPREMOTE_COMMAND_TIMEOUT) state["uploaded"][artifact_key] = artifact_hash @@ -537,7 +621,7 @@ def _upload_changed_static_files( _log("INFO", f"upload {path} -> {app_dir}/{path.as_posix()}") destination = f"{app_dir}/{path.as_posix()}" - command = ["mpremote", *mpremote_args, "cp", str(path), destination] + command = [_tool("mpremote"), *mpremote_args, "cp", str(path), destination] _run_command(command, dry_run=dry_run, timeout=MPREMOTE_COMMAND_TIMEOUT) state["uploaded"][file_key] = file_hash @@ -615,6 +699,7 @@ def main() -> int: try: _validate_sources() + _initialise_tool_commands(repo_root) if options.clear_state and STATE_PATH.exists(): _log("INF", f"clearing state file {STATE_PATH}") diff --git a/dev/minify.py b/dev/minify.py new file mode 100644 index 0000000..7d5c2e3 --- /dev/null +++ b/dev/minify.py @@ -0,0 +1,283 @@ +"""Minify and compile vendor modules for MicroPython deployment. + +Pipeline: + 1. Rename internal instance attributes to short names via AST transform + (source stays readable; only the build artefact is shrunk) + 2. Strip docstrings via python-minifier + (--remove-literal-statements --no-hoist-literals) + 3. Compile with mpy-cross -O2 + +Standalone – minify all configured vendor modules and show size comparison: + python dev/minify.py + +Per-file – used by download_to_device.py for incremental builds: + python dev/minify.py --source vendor/HexDrive2/hexdrive2.py --artifact EEPROM/hexdrive2.mpy +""" +import argparse +import ast +import string +import subprocess +import sys +from collections import Counter +from dataclasses import dataclass +from pathlib import Path + +HERE = Path(__file__).parent +ROOT = HERE.parent # sim/apps/BadgeBot/ +MPY_CROSS = ( + ROOT / ".venv" / "Lib" / "site-packages" / "mpy_cross" / "archive" / "v1.20.0" / "mpy-cross.exe" +) + + +# ── per-file preserve sets ──────────────────────────────────────────────────── +# Names that must NOT be renamed – framework hooks and externally visible API. + +_PRESERVE_HEXDRIVE2: frozenset[str] = frozenset({ + # BadgeOS app lifecycle + "background_update", + "__init__", + # Public API called by BadgeBot + "initialise", "get_status", "set_logging", "set_power", + "set_dist_xshut", "set_sensor_led", "set_keep_alive", + "set_freq", "set_servoposition", "set_servocentre", "set_motors", + # Public state + "config", "VERSION", "PWMOutput", + # HexDriveType fields accessed externally + "pid", "name", "motors", "servos", "servo_pin_map", + "__app_export__", +}) + +_PRESERVE_HEXDRIVE: frozenset[str] = frozenset({ + # BadgeOS app lifecycle + "background_update", + "__init__", "__app_export__", + # Public API called by BadgeBot + "initialise", "get_status", "set_logging", "set_power", + "set_keep_alive", "set_freq", "set_servoposition", "set_servocentre", "set_motors", + # Public state + "config", "VERSION", "PWMOutput", + # HexDriveType fields accessed externally + "pid", "name", "motors", "servos", "servo_pin_map", "hw_ver", +}) + +_PRESERVE_HEXTEST: frozenset[str] = frozenset({ + # BadgeOS app lifecycle (uses background_task, not background_update) + "update", "draw", "background_task", + "__init__", "__app_export__", + # Public state accessed by BadgeBot + "config", "VERSION", "settings", "hexdrive_app", "logging", + "auto_repeat_level", "refresh", "current_state", + # Public methods called by BadgeBot + "update_settings", "set_logging", "deinitialise", "show_message", + "return_to_menu", "set_menu", "auto_repeat_check", "auto_repeat_clear", +}) + + +@dataclass(frozen=True) +class MinifySpec: + source: Path # relative to ROOT + artifact: Path # relative to ROOT + preserve: frozenset[str] + + +# All vendor modules this script knows how to minify. +MINIFIABLE: tuple[MinifySpec, ...] = ( + MinifySpec( + ROOT / "vendor" / "HexDrive2" / "hexdrive2.py", + ROOT / "EEPROM" / "hexdrive2.mpy", + _PRESERVE_HEXDRIVE2, + ), + MinifySpec( + ROOT / "vendor" / "HexDrive" / "hexdrive.py", + ROOT / "EEPROM" / "hexdrive.mpy", + _PRESERVE_HEXDRIVE, + ), + MinifySpec( + ROOT / "EEPROM" / "hextest.py", + ROOT / "EEPROM" / "hextest.mpy", + _PRESERVE_HEXTEST, + ), +) + + +# ── short-name generator: _a, _b, … _z, _aa, _ab, … ───────────────────────── +def _short_names(): + for c in string.ascii_lowercase: + yield f"_{c}" + for c1 in string.ascii_lowercase: + for c2 in string.ascii_lowercase: + yield f"_{c1}{c2}" + + +# ── build rename map ────────────────────────────────────────────────────────── +def _build_rename_map(tree: ast.AST, preserve: frozenset[str]): + """Return ({old: short}, Counter) for self.xxx attributes worth renaming.""" + counts: Counter = Counter() + for node in ast.walk(tree): + if ( + isinstance(node, ast.Attribute) + and isinstance(node.value, ast.Name) + and node.value.id == "self" + ): + counts[node.attr] += 1 + + candidates = sorted( + [(name, cnt) for name, cnt in counts.items() + if name not in preserve and len(name) > 3], + key=lambda x: -(len(x[0]) - 2) * x[1], + ) + + gen = _short_names() + used = set(counts.keys()) + mapping: dict[str, str] = {} + + for name, _cnt in candidates: + while True: + short = next(gen) + if short not in used: + break + mapping[name] = short + used.add(short) + + return mapping, counts + + +# ── AST transformer ─────────────────────────────────────────────────────────── +class _AttrRenamer(ast.NodeTransformer): + def __init__(self, mapping: dict[str, str]): + self.mapping = mapping + + def visit_Attribute(self, node: ast.Attribute): + self.generic_visit(node) + if ( + isinstance(node.value, ast.Name) + and node.value.id == "self" + and node.attr in self.mapping + ): + node.attr = self.mapping[node.attr] + return node + + def visit_FunctionDef(self, node: ast.FunctionDef): + self.generic_visit(node) + if node.name in self.mapping: + node.name = self.mapping[node.name] + return node + + visit_AsyncFunctionDef = visit_FunctionDef # type: ignore[assignment] + + +# ── core pipeline ───────────────────────────────────────────────────────────── +def minify_file( + source: Path, + artifact: Path, + preserve: frozenset[str], + *, + verbose: bool = False, +) -> int: + """AST-rename + minify + mpy-cross compile source → artifact. + + Returns the artifact size in bytes, or -1 on failure. + Temp files are cleaned up on both success and failure. + """ + source_text = source.read_text(encoding="utf-8") + tree = ast.parse(source_text) + + mapping, counts = _build_rename_map(tree, preserve) + + if verbose: + print(f"Renaming {len(mapping)} attributes/methods in {source.name}:") + for old, new in sorted(mapping.items(), key=lambda x: -(len(x[0]) - len(x[1])) * counts[x[0]]): + est = (len(old) - len(new)) * counts[old] + print(f" self.{old:35s} -> self.{new:<5s} (×{counts[old]:3d}, ~{est:+d} chars)") + + renamed_tree = _AttrRenamer(mapping).visit(tree) + ast.fix_missing_locations(renamed_tree) + + temp_renamed = source.parent / (source.stem + ".renamed.py") + temp_min = source.parent / (source.stem + ".min.py") + + temp_renamed.write_text(ast.unparse(renamed_tree), encoding="utf-8") + try: + cmd = [ + sys.executable, "-m", "python_minifier", + "--remove-literal-statements", "--no-hoist-literals", + "--output", str(temp_min), str(temp_renamed), + ] + r = subprocess.run(cmd, capture_output=True, text=True) + #temp_renamed.unlink() + if r.returncode != 0: + print(f"[FAIL] python-minifier on {source.name}: {r.stderr}", file=sys.stderr) + return -1 + + artifact.parent.mkdir(parents=True, exist_ok=True) + #cmd = [str(MPY_CROSS), "-O2", "-o", str(artifact), str(temp_min)] + cmd = [str(MPY_CROSS), "-O2", "-o", str(artifact), str(source)] + + r = subprocess.run(cmd, capture_output=True, text=True) + #temp_min.unlink() + if r.returncode != 0: + print(f"[FAIL] mpy-cross on {source.name}: {r.stderr}", file=sys.stderr) + return -1 + + return artifact.stat().st_size + + except Exception: + temp_renamed.unlink(missing_ok=True) + temp_min.unlink(missing_ok=True) + raise + + +# ── entry point ─────────────────────────────────────────────────────────────── +def main() -> int: + parser = argparse.ArgumentParser( + description="Minify and compile vendor modules for MicroPython deployment.", + ) + parser.add_argument("--source", type=Path, + help="Source .py file to minify (relative to repo root).") + parser.add_argument("--artifact", type=Path, + help="Output .mpy file (relative to repo root).") + parser.add_argument("--verbose", action="store_true", + help="Show attribute rename table.") + args = parser.parse_args() + + if args.source or args.artifact: + # ── CLI mode: single file, called by download_to_device.py ────────── + if not (args.source and args.artifact): + print("--source and --artifact must be provided together.", file=sys.stderr) + return 1 + source = ROOT / args.source + artifact = ROOT / args.artifact + preserve = next( + (spec.preserve for spec in MINIFIABLE if spec.source.stem == source.stem), + frozenset(), + ) + size = minify_file(source, artifact, preserve, verbose=args.verbose) + return 0 if size >= 0 else 1 + + # ── Standalone mode: minify all configured modules, show comparison ────── + for spec in MINIFIABLE: + if not spec.source.exists(): + print(f" Skipping {spec.source.name}: not found") + continue + + # Compile original for baseline comparison + orig_mpy = spec.source.parent / (spec.source.stem + ".orig.mpy") + cmd = [str(MPY_CROSS), "-O2", "-o", str(orig_mpy), str(spec.source)] + subprocess.run(cmd, capture_output=True, text=True) + orig_size = orig_mpy.stat().st_size if orig_mpy.exists() else 0 + orig_mpy.unlink(missing_ok=True) + + min_size = minify_file(spec.source, spec.artifact, spec.preserve, verbose=True) + + if min_size >= 0 and orig_size: + saving = orig_size - min_size + print(f"\n {spec.source.name}:") + print(f" original: {orig_size:6d} bytes") + print(f" minified: {min_size:6d} bytes") + print(f" saving: {saving:+d} bytes ({100 * saving / orig_size:.1f}%)\n") + + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/diagnostics.py b/diagnostics.py new file mode 100644 index 0000000..0b98440 --- /dev/null +++ b/diagnostics.py @@ -0,0 +1,15 @@ +"""Shared development diagnostics output hooks for BadgeBot.""" + +_diagnostics_state = {"sink": None} + + +def set_diagnostics_output(sink): + """Register a callable that receives diagnostic pin updates.""" + _diagnostics_state["sink"] = sink + + +def diagnostics_output(index: int, value: int): + """Emit a diagnostic output update if a sink is registered.""" + sink = _diagnostics_state["sink"] + if sink is not None: + sink(index, value) diff --git a/download.bat b/download.bat index 9a4e168..181f53a 100644 --- a/download.bat +++ b/download.bat @@ -1,8 +1,25 @@ @echo off setlocal +cd /d "%~dp0" REM Incremental compile + upload with detailed logging and error reporting. -python dev\download_to_device.py %* +if exist ".venv\Scripts\python.exe" ( + ".venv\Scripts\python.exe" dev\download_to_device.py %* +) else ( + where python >nul 2>nul + if not errorlevel 1 ( + python dev\download_to_device.py %* + ) else ( + where py >nul 2>nul + if not errorlevel 1 ( + py -3 dev\download_to_device.py %* + ) else ( + echo. + echo download failed: could not find Python. Install Python or create .venv. + exit /b 1 + ) + ) +) set "EXIT_CODE=%ERRORLEVEL%" if not "%EXIT_CODE%"=="0" ( diff --git a/download.sh b/download.sh index 1d3bd07..f567666 100755 --- a/download.sh +++ b/download.sh @@ -1,7 +1,22 @@ #!/bin/bash # Incremental compile + upload with detailed logging and error reporting. cd "$(dirname "$0")" -python dev/download_to_device.py "$@" + +if [ -x ".venv/bin/python" ]; then + PYTHON_CMD=".venv/bin/python" +elif [ -x ".venv/Scripts/python.exe" ]; then + PYTHON_CMD=".venv/Scripts/python.exe" +elif command -v python3 >/dev/null 2>&1; then + PYTHON_CMD="python3" +elif command -v python >/dev/null 2>&1; then + PYTHON_CMD="python" +else + echo + echo "download failed: could not find Python. Install Python or create .venv." + exit 1 +fi + +"$PYTHON_CMD" dev/download_to_device.py "$@" EXIT_CODE=$? if [ "$EXIT_CODE" != "0" ]; then diff --git a/hexpansion_mgr.py b/hexpansion_mgr.py index ddbee5c..b8cf374 100644 --- a/hexpansion_mgr.py +++ b/hexpansion_mgr.py @@ -20,12 +20,13 @@ from events.input import BUTTON_TYPES from machine import I2C from system.eventbus import eventbus -from system.hexpansion.events import HexpansionInsertionEvent +from system.hexpansion import app as hexpansion_app +from system.hexpansion.events import HexpansionInsertionEvent, HexpansionRemovalEvent from system.hexpansion.header import HexpansionHeader, write_header from system.hexpansion.util import get_hexpansion_block_devices, detect_eeprom_addr from system.scheduler import scheduler -_NUM_HEXPANSION_SLOTS = 6 +_SLOTS = 6 # HexDrive Hexpansion constants # EEPROM Constants @@ -86,11 +87,12 @@ _MODE_UPDATE = 2 # Normal mode for responding to hexpansion-related events (insertion/removal) _MODE_INTERACTIVE = 3 # Interactive mode for user interactions for initialisation/upgrade/erasure of hexpansions + +# WHEN Badge hexpansion handling has stabillised - revisit this and make much much simpler... _SINGLE_PORT_HEXPANSION_REFS = ( ("hexsense_port", "HexSense", "HEXSENSE_HEXPANSION_INDEX"), - ("hextest_port", "HexTest", "HEXTEST_HEXPANSION_INDEX"), - ("hexdiag_port", "HexDiag", "HEXDIAG_HEXPANSION_INDEX"), - #("hexgps_port", "HexGPS", "HEXGPS_HEXPANSION_INDEX"), + ("hexdiag_port", "HexDiag", "HEXDIAG_HEXPANSION_INDEX"), + ("hexaudio_port", "HexAudio", "HEXAUDIO_HEXPANSION_INDEX"), ) # ---- Settings initialisation ----------------------------------------------- @@ -127,10 +129,10 @@ def __init__(self, app, logging: bool = False): self._port_detail_page: int = 0 # 0=vid/pid, 1=eeprom, 2=details (conditional) self._port_detail_page_count: int = 2 # 2 or 3 depending on whether details page is available self._hexpansion_app_startup_timer: int = 0 - self._hexpansion_type_by_slot: list[int | None] = [None]*_NUM_HEXPANSION_SLOTS - self._hexpansion_state_by_slot: list[int] = [_HEXPANSION_STATE_UNKNOWN]*_NUM_HEXPANSION_SLOTS - self._hexpansion_eeprom_addr_len: list[int | None] = [None]*_NUM_HEXPANSION_SLOTS - self._hexpansion_eeprom_addr: list[int | None] = [None]*_NUM_HEXPANSION_SLOTS + self._hexpansion_type_by_slot: list[int | None] = [None]*_SLOTS + self._hexpansion_state_by_slot: list[int] = [_HEXPANSION_STATE_UNKNOWN]*_SLOTS + self._hexpansion_eeprom_addr_len: list[int | None] = [None]*_SLOTS + self._hexpansion_eeprom_addr: list[int | None] = [None]*_SLOTS self._hexpansion_init_type: int = 0 self._detected_port: int | None = None self._waiting_app_port: int | None = None @@ -150,14 +152,12 @@ def __init__(self, app, logging: bool = False): def register_events(self): """Register hexpansion insertion/removal event handlers directly.""" - from system.hexpansion.events import HexpansionRemovalEvent eventbus.on_async(HexpansionInsertionEvent, self._handle_insertion, self._app) eventbus.on_async(HexpansionRemovalEvent, self._handle_removal, self._app) def unregister_events(self): """Unregister hexpansion event handlers.""" - from system.hexpansion.events import HexpansionRemovalEvent eventbus.remove(HexpansionInsertionEvent, self._handle_insertion, self._app) eventbus.remove(HexpansionRemovalEvent, self._handle_removal, self._app) @@ -180,7 +180,6 @@ def _clear_single_port_hexpansion_refs(self, port: int | None): """Clear app references for single-port hexpansions assigned to *port*.""" if port is None: return - app = self._app for attr_name, display_name, _ in _SINGLE_PORT_HEXPANSION_REFS: if getattr(app, attr_name, None) == port: @@ -194,6 +193,7 @@ def _has_single_port_hexpansion_on_port(self, port: int) -> bool: app = self._app for attr_name, _, _ in _SINGLE_PORT_HEXPANSION_REFS: if getattr(app, attr_name, None) == port: + print(f"H:Found hexpansion {attr_name} on port {port}") return True return False @@ -203,6 +203,7 @@ def _refresh_single_port_hexpansion_assignments(self): app = self._app previous_ports = {} + print("H:Refreshing hexpansion assignments...") for attr_name, _, type_index_attr in _SINGLE_PORT_HEXPANSION_REFS: previous_port = getattr(app, attr_name, None) previous_ports[attr_name] = previous_port @@ -213,9 +214,6 @@ def _refresh_single_port_hexpansion_assignments(self): if previous_ports["hexdiag_port"] != app.hexdiag_port: app.hexdiag_setup() - if previous_ports["hextest_port"] != app.hextest_port and app.sensor_test_mgr is not None: - app.sensor_test_mgr.hextest_setup(app.hextest_port) - def _should_claim_single_port_hexpansion(self, type_index: int) -> bool: """Return True if a detected single-port hexpansion type is not yet assigned.""" @@ -230,28 +228,33 @@ def _should_claim_single_port_hexpansion(self, type_index: int) -> bool: # Async event handlers (registered directly on eventbus) # ------------------------------------------------------------------ - async def _handle_removal(self, event): + async def _handle_removal(self, event: HexpansionRemovalEvent): app = self._app - self._hexpansion_type_by_slot[event.port - 1] = None - self._hexpansion_state_by_slot[event.port - 1] = _HEXPANSION_STATE_EMPTY - if event.port in self._ports_to_initialise: - self._ports_to_initialise.remove(event.port) - self._ports_to_check_app.discard(event.port) - - if (self._detected_port is not None and event.port == self._detected_port) or \ - (self._upgrade_port is not None and event.port == self._upgrade_port) or \ - (self._waiting_app_port is not None and event.port == self._waiting_app_port) or \ - (self._erase_port is not None and event.port == self._erase_port) or \ - (self._port_selected != 0 and event.port == self._port_selected) or \ - (event.port in app.hexdrive_ports) or \ - self._has_single_port_hexpansion_on_port(event.port): + port = event.port + self._hexpansion_type_by_slot[port - 1] = None + self._hexpansion_state_by_slot[port - 1] = _HEXPANSION_STATE_EMPTY + if port in self._ports_to_initialise: + self._ports_to_initialise.remove(port) + self._ports_to_check_app.discard(port) + + if (self._detected_port is not None and port == self._detected_port) or \ + (self._upgrade_port is not None and port == self._upgrade_port) or \ + (self._waiting_app_port is not None and port == self._waiting_app_port) or \ + (self._erase_port is not None and port == self._erase_port) or \ + (self._port_selected != 0 and port == self._port_selected) or \ + (port in app.hexdrive_ports) or \ + self._has_single_port_hexpansion_on_port(port): # The port from which a hexpansion has been removed is significant + self._hexpansion_eeprom_addr_len[port - 1] = None + self._hexpansion_eeprom_addr[port - 1] = None app.hexpansion_update_required = True if self._logging: - print(f"H:Hexpansion removed from port {event.port}") + print(f"H:Hexpansion removed from port {port}") - async def _handle_insertion(self, event): + # Although the Badge S/W now provides HexpansionMountedEvent which is emitted after a hexpansion is inserted and successfully mounted, + # we still want to listen for the raw insertion event as we want to know about hexpansions with blank eeproms. + async def _handle_insertion(self, event: HexpansionInsertionEvent): if self._check_port_for_known_hexpansions(event.port) or event.port == self._port_selected: # A known hexpansion type has been detected on the inserted port, so trigger an update of # the hexpansion management state machine to handle it. Or the inserted port is the one @@ -304,23 +307,21 @@ def _read_port_header(self, port: int): def _update_detail_page_count(self): """Set page count to 3 if the selected port has a recognised type with sub_type or app_name, else 2, or 1 if blank EEPROM.""" - app = self._app - type_idx = self._hexpansion_type_by_slot[self._port_selected - 1] if 1 <= self._port_selected <= _NUM_HEXPANSION_SLOTS else None - if type_idx is not None and 0 <= type_idx <= app.BLANK_HEXPANSION_INDEX: - ht = app.HEXPANSION_TYPES[type_idx] - if ht.sub_type or ht.app_name: - # Recognised type with sub_type or app_name, so show details page + state_idx = self._hexpansion_state_by_slot[self._port_selected - 1] if 1 <= self._port_selected <= _SLOTS else None + if state_idx is not None: + if state_idx == _HEXPANSION_STATE_UNRECOGNISED: + # Unrecognised type - show vid/pid page and EEPROM page but not details page + self._port_detail_page_count = 2 + self._port_detail_page = self._PAGE_VID_PID + elif state_idx >= _HEXPANSION_STATE_RECOGNISED: + # Recognised type - show vid/pid page and details page self._port_detail_page_count = 3 self._port_detail_page = self._PAGE_DETAILS - elif type_idx == app.BLANK_HEXPANSION_INDEX: - # Blank EEPROM - no details to show, so only show the EEPROM page + else: + # Empty, Faulty or Blank self._port_detail_page_count = 0 - elif type_idx == app.UNRECOGNISED_HEXPANSION_INDEX: - # Unrecognised type - show vid/pid page and EEPROM page - self._port_detail_page_count = 2 - self._port_detail_page = self._PAGE_VID_PID else: - # Empty + # No state information self._port_detail_page_count = 0 @@ -328,7 +329,7 @@ def _update_detail_page_count(self): # Per-tick update (state machine for hexpansion management) # ------------------------------------------------------------------ - def update(self, delta) -> bool: + def update(self, delta: int) -> bool: """Per-tick update for hexpansion management state machine.""" app = self._app @@ -390,9 +391,9 @@ def update(self, delta) -> bool: app.hexpansion_update_required = False #to avoid beign called immediately self._sub_state = _SUB_EXIT # exit to menu on next call (when user accepts warning) elif self._sub_state == _SUB_EXIT: + print("H:EXIT") app.hexpansion_update_required = False self._message_being_shown = False - print("H:EXIT") app.initialise_settings() app.return_to_menu() self._mode = _MODE_IDLE @@ -404,7 +405,7 @@ def update(self, delta) -> bool: # Individual state handlers # ------------------------------------------------------------------ - def _update_state_programming(self, delta): # pylint: disable=unused-argument + def _update_state_programming(self, delta: int): # pylint: disable=unused-argument app = self._app if self._upgrade_port is not None: @@ -429,12 +430,15 @@ def _update_state_programming(self, delta): # pylint: disable=unused-argumen self._message_being_shown = True self._sub_state = _SUB_CHECK else: + #Easisest way to cope with there being a new EEPROM image is to ask the user to reboop + #otherwise we actually need to do a lot to get the old module and mount removed first... #upgrade_text = "Upgraded" if result == _APP_EEPROM_RESULT_SUCCESSFUL_UPGRADE else "Programmed" #app.notification = Notification(upgrade_text, port=self._upgrade_port) # No point showing "Programmed" vs "Upgraded" as the Hexpansion Insertion Notification will cover it up - eventbus.emit(HexpansionInsertionEvent(self._upgrade_port)) + #eventbus.emit(HexpansionInsertionEvent(self._upgrade_port)) #app.show_message([f"{upgrade_text}:", "Please", "reboop"], [(0,1,0),(1,1,1),(1,1,1)], "reboop") - #self._reboop_required = True + self._reboop_required = True + self._hexpansion_state_by_slot[self._upgrade_port - 1] = _HEXPANSION_STATE_RECOGNISED_APP_OK self._sub_state = _SUB_CHECK self._upgrade_port = None elif self._detected_port is not None: @@ -466,7 +470,7 @@ def _update_state_programming(self, delta): # pylint: disable=unused-argumen self._sub_state = _SUB_INIT if self._mode == _MODE_INIT else _SUB_CHECK - def _update_state_detected(self, delta): # pylint: disable=unused-argument + def _update_state_detected(self, delta: int): # pylint: disable=unused-argument """ Allow User to select which sub-type they want to initialise the hexpansion as (if there are multiple sub-types with the same PID), and confirm or cancel the initialisation.""" app = self._app if app.button_states.get(BUTTON_TYPES["CONFIRM"]): @@ -480,25 +484,15 @@ def _update_state_detected(self, delta): # pylint: disable=unused-argumen self._sub_state = _SUB_INIT if self._mode == _MODE_INIT else _SUB_CHECK elif app.button_states.get(BUTTON_TYPES["UP"]): app.button_states.clear() - self._hexpansion_init_type = (self._hexpansion_init_type + 1) % app.UNRECOGNISED_HEXPANSION_INDEX + self._hexpansion_init_type = (self._hexpansion_init_type + 1) % len(app.HEXPANSION_TYPES) app.refresh = True elif app.button_states.get(BUTTON_TYPES["DOWN"]): app.button_states.clear() - self._hexpansion_init_type = (self._hexpansion_init_type - 1) % app.UNRECOGNISED_HEXPANSION_INDEX - app.refresh = True - elif app.button_states.get(BUTTON_TYPES["LEFT"]): - app.button_states.clear() - # "Left" is a shortcut button to HexDrive - self._hexpansion_init_type = app.HEXDRIVE_HEXPANSION_INDEX - app.refresh = True - elif app.button_states.get(BUTTON_TYPES["RIGHT"]): - app.button_states.clear() - # "Right" is a shortcut button to HexSense - self._hexpansion_init_type = app.HEXSENSE_HEXPANSION_INDEX + self._hexpansion_init_type = (self._hexpansion_init_type - 1) % len(app.HEXPANSION_TYPES) app.refresh = True - def _update_state_erase_confirm(self, delta): # pylint: disable=unused-argument + def _update_state_erase_confirm(self, delta: int): # pylint: disable=unused-argument """ Allow User to confirm or cancel EEPROM erasure.""" # not used in _MODE_INIT app = self._app @@ -512,27 +506,30 @@ def _update_state_erase_confirm(self, delta): # pylint: disable=unused-arg print("H:Erase Cancelled") app.button_states.clear() self._erase_port = None - self._sub_state = _SUB_PORT_SELECT if self._mode == _MODE_INTERACTIVE else _SUB_CHECK + self._sub_state = _SUB_CHECK - def _update_state_erase(self, delta): # pylint: disable=unused-argument + def _update_state_erase(self, delta: int): # pylint: disable=unused-argument """ Perform EEPROM erasure, and update app state accordingly (e.g. if the erased hexpansion is currently in use or being initialised/upgraded, reset those states). Unresponsive to buttons during the erasure process.""" # not used in _MODE_INIT app = self._app erase_port = self._erase_port if erase_port is None: - self._sub_state = _SUB_PORT_SELECT if self._mode == _MODE_INTERACTIVE else _SUB_CHECK + self._sub_state = _SUB_CHECK return if self._logging: print(f"H:Erasing EEPROM on port {erase_port}") + existing_type = self._hexpansion_type_by_slot[erase_port - 1] + if existing_type is not None: + self._hexpansion_init_type = existing_type eeprom_page_size=app.HEXPANSION_TYPES[self._hexpansion_init_type].eeprom_page_size if self._hexpansion_init_type > 0 else _DEFAULT_EEPROM_PAGE_SIZE eeprom_total_size=app.HEXPANSION_TYPES[self._hexpansion_init_type].eeprom_total_size if self._hexpansion_init_type > 0 else _DEFAULT_EEPROM_TOTAL_SIZE erase_addr_len = self._hexpansion_eeprom_addr_len[erase_port - 1] erase_addr = self._hexpansion_eeprom_addr[erase_port - 1] if erase_addr_len is None or erase_addr is None: app.notification = Notification("Failed", port=erase_port) - self._sub_state = _SUB_PORT_SELECT if self._mode == _MODE_INTERACTIVE else _SUB_CHECK + self._sub_state = _SUB_CHECK return if self._logging: print(f"H:Erase {self._hexpansion_init_type} page size: {eeprom_page_size} bytes, total size: {eeprom_total_size} bytes, addr_len: {erase_addr_len}, addr: {hex(erase_addr)}") @@ -543,7 +540,6 @@ def _update_state_erase(self, delta): # pylint: disable=unused-argument eeprom_total_size, eeprom_page_size): app.notification = Notification("Erased", port=erase_port) - self._hexpansion_type_by_slot[erase_port - 1] = app.BLANK_HEXPANSION_INDEX self._hexpansion_state_by_slot[erase_port - 1] = _HEXPANSION_STATE_BLANK hexpansion_type = self._type_name_for_port(erase_port) app.show_message([hexpansion_type, f"in slot {erase_port}:", "Erased"], [(1,1,0), (1,1,1), (0,1,0)], "hexpansion") @@ -553,7 +549,7 @@ def _update_state_erase(self, delta): # pylint: disable=unused-argument app.notification = Notification("Failed", port=erase_port) app.show_message(["EEPROM", "erasure", "failed", "Protected?"], [(1,0,0),(1,0,0),(1,0,0),(1,0,0)], "warning") self._message_being_shown = True - self._sub_state = _SUB_PORT_SELECT if self._mode == _MODE_INTERACTIVE else _SUB_CHECK + self._sub_state = _SUB_CHECK #self._reboop_required = True @@ -571,12 +567,14 @@ def _update_state_erase(self, delta): # pylint: disable=unused-argument self._erase_port = None - def _update_state_upgrade(self, delta): # pylint: disable=unused-argument + def _update_state_upgrade(self, delta: int): # pylint: disable=unused-argument """ Allow User to confirm or cancel App upgrade.""" app = self._app upgrade_port = self._upgrade_port if upgrade_port is None: - self._sub_state = _SUB_PORT_SELECT if self._mode == _MODE_INTERACTIVE else (_SUB_INIT if self._mode == _MODE_INIT else _SUB_CHECK) + if self.logging: + print("H:Error - no port to upgrade") + self._sub_state = _SUB_INIT if self._mode == _MODE_INIT else _SUB_CHECK return if app.button_states.get(BUTTON_TYPES["CONFIRM"]): app.button_states.clear() @@ -586,14 +584,14 @@ def _update_state_upgrade(self, delta): # pylint: disable=unused-argument if self._logging: print("H:Upgrade Cancelled") app.button_states.clear() - self._hexpansion_state_by_slot[upgrade_port - 1] = _HEXPANSION_STATE_RECOGNISED_OLD_APP + #self._hexpansion_state_by_slot[upgrade_port - 1] = _HEXPANSION_STATE_RECOGNISED_OLD_APP self._upgrade_port = None - self._sub_state = _SUB_PORT_SELECT if self._mode == _MODE_INTERACTIVE else (_SUB_INIT if self._mode == _MODE_INIT else _SUB_CHECK) + self._sub_state = _SUB_INIT if self._mode == _MODE_INIT else _SUB_CHECK - def _get_hexpansion_by_type(self, hexpansion_type) -> int | None: + def _get_hexpansion_by_type(self, hexpansion_type: int) -> int | None: """ Return the port number of a hexpansion of the given type, or None if no such hexpansion is currently detected.""" - for port in range(0, _NUM_HEXPANSION_SLOTS): + for port in range(0, _SLOTS): if self._hexpansion_type_by_slot[port] == hexpansion_type: return port+1 return None @@ -604,20 +602,20 @@ def _report_hexpansion_states(self): if not self._logging: return app = self._app - for port in range(0, _NUM_HEXPANSION_SLOTS): + print("H:Current Hexpansion States:") + for port in range(0, _SLOTS): type_idx = self._hexpansion_type_by_slot[port] type_name = app.HEXPANSION_TYPES[type_idx].name if type_idx is not None else "None" state_name = _HEXPANSION_STATE_NAMES[self._hexpansion_state_by_slot[port]] print(f"Port {port+1}: Type={type_name}, State={state_name}") - print(f"Ports to initialise: {self._ports_to_initialise}") - print(f"Ports to check app: {self._ports_to_check_app}") - print(f"hexsense_port:{app.hexsense_port}") - print(f"hextest_port:{app.hextest_port}") - #print(f"hexgps_port:{app.hexgps_port}") - print(f"hexdiag_port:{app.hexdiag_port}") - print(f"hexdrive_ports:{app.hexdrive_ports}") - print(f"hexpansion_update_required = {app.hexpansion_update_required}") - print(f"mode = {self._mode}") + print(f"\tPorts to initialise: {self._ports_to_initialise}") + print(f"\tPorts to check app: {self._ports_to_check_app}") + print(f"\thexsense_port:{app.hexsense_port}") + print(f"\thexdiag_port:{app.hexdiag_port}") + print(f"\thexaudio_port:{app.hexaudio_port}") + print(f"\thexdrive_ports:{app.hexdrive_ports}") + print(f"\thexpansion_update_required = {app.hexpansion_update_required}") + print(f"\tmode = {self._mode}") @@ -625,7 +623,7 @@ def _check_hexpansion(self, port: int | None, type_index: int) -> tuple[int | No """ Check if the currently configured hexpansion of the given type is still present, and if not, check if there is another hexpansion of the same type that can be switched to, or if the hexpansion has been removed entirely.""" app = self._app - hexpansion_app = None + check_hexpansion_app = None hexpansion_was_present = port is not None old_port = port if hexpansion_was_present: @@ -647,7 +645,7 @@ def _check_hexpansion(self, port: int | None, type_index: int) -> tuple[int | No port = new_port if app.HEXPANSION_TYPES[type_index].app_name is not None: self._hexpansion_state_by_slot[port - 1] = _HEXPANSION_STATE_RECOGNISED # may be updated to _RECOGNISED_APP_OK or _RECOGNISED_OLD_APP after checking for the correct app - hexpansion_app = self._check_hexpansion_app_on_port(port, type_index) + check_hexpansion_app = self._check_hexpansion_app_on_port(port, type_index) else: self._hexpansion_state_by_slot[port - 1] = _HEXPANSION_STATE_RECOGNISED_NO_APP elif hexpansion_was_present: @@ -656,9 +654,10 @@ def _check_hexpansion(self, port: int | None, type_index: int) -> tuple[int | No if self._mode == _MODE_UPDATE: app.show_message([f"{name}","removed.","Please reinsert"], [(1,1,0),(1,1,1),(1,1,1)], "error") self._message_being_shown = True + assert old_port is not None app.notification = Notification(f"{name} removed", port=old_port) - return port, hexpansion_app + return port, check_hexpansion_app def _update_state_check(self, delta): # pylint: disable=unused-argument @@ -667,11 +666,12 @@ def _update_state_check(self, delta): # pylint: disable=unused-argument self._report_hexpansion_states() + # For hexpansions of which we only need to know where one is we track movements between ports and update the assigned port accordingly self._refresh_single_port_hexpansion_assignments() - # Build a new list of ports with HexDrives: + # Build a new list of ports with HexDrives - to allow for more than one being present and used: new_hexdrive_ports = [] - for port in range(1, _NUM_HEXPANSION_SLOTS + 1): + for port in range(1, _SLOTS + 1): # check if there is a hexpansion of a type that can be a HexDrive on this port type_idx = self._hexpansion_type_by_slot[port-1] if type_idx is not None and type_idx in app.hexdrive_hexpansion_types: @@ -682,23 +682,8 @@ def _update_state_check(self, delta): # pylint: disable=unused-argument if set(new_hexdrive_ports) != set(app.hexdrive_ports): if self._logging: print(f"H:HexDrive ports changed from {app.hexdrive_ports} to {new_hexdrive_ports}") - #if len(new_hexdrive_ports) == len(app.hexdrive_ports): - # app.show_message(["HexDrive moved", f"to {new_hexdrive_ports}"], [(1,1,0),(1,1,1)], "hexpansion") - # self._message_being_shown = True - #elif len(new_hexdrive_ports) > len(app.hexdrive_ports): - # added_ports = set(new_hexdrive_ports) - set(app.hexdrive_ports) - # if self._mode != _MODE_INIT: - # app.show_message(["HexDrive inserted", f"on port {added_ports}"], [(0,1,0),(1,1,1)], "hexpansion") - # self._message_being_shown = True - #else: - # removed_ports = set(app.hexdrive_ports) - set(new_hexdrive_ports) - # if len(new_hexdrive_ports) > 0: - # # no point showing this message if there are no Hexdrives left as user will get the "HexDrive required" message instead - # app.show_message(["HexDrive removed", f"from port {removed_ports}"], [(1,0,0),(1,1,1)], "hexpansion") - # self._message_being_shown = True app.hexdrive_ports = new_hexdrive_ports app.hexdrive_apps = [] - app.num_motors = 0 app.num_servos = 0 for port in app.hexdrive_ports: @@ -710,7 +695,8 @@ def _update_state_check(self, delta): # pylint: disable=unused-argument if len(app.hexdrive_ports) != len (app.hexdrive_apps): hexdrive_apps = [] for port in app.hexdrive_ports: - print(f"H:Checking HexDrive app on port {port}, current state: {_HEXPANSION_STATE_NAMES[self._hexpansion_state_by_slot[port - 1]]}") + if self._logging: + print(f"H:Checking HexDrive app on port {port}, current state: {_HEXPANSION_STATE_NAMES[self._hexpansion_state_by_slot[port - 1]]}") if self._hexpansion_state_by_slot[port - 1] == _HEXPANSION_STATE_RECOGNISED_APP_OK: # already checked and app is OK, so just add it to the list hexdrive_app = self._find_hexpansion_app(port) @@ -721,14 +707,15 @@ def _update_state_check(self, delta): # pylint: disable=unused-argument type_idx = self._hexpansion_type_by_slot[port - 1] if app.HEXPANSION_TYPES[type_idx].app_name is not None: # Yes this port should have an app, but we haven't checked it yet, so check if the correct app is running on this port - print(f"H:Request Check for {app.HEXPANSION_TYPES[type_idx].app_name} app on port {port}") if port not in self._ports_to_check_app: + if self._logging: + print(f"H:Request Check for {app.HEXPANSION_TYPES[type_idx].app_name} app on port {port}") self._ports_to_check_app.add(port) if len(hexdrive_apps) > 0: app.hexdrive_apps = hexdrive_apps if self._logging: - print(f"H:Updated HexDrive apps: {app.hexdrive_apps}") + print(f"H:Latest HexDrive apps: {app.hexdrive_apps}") # Create the high-level MotorController for IMU-aided driving # (only when the HexDrive has motors) @@ -751,7 +738,7 @@ def _update_state_check(self, delta): # pylint: disable=unused-argument if len(self._ports_to_check_app) > 0: # there are outstandind apps to check if self._logging: - print(f"H:Checking apps on ports: {self._ports_to_check_app}") + print(f"H:Waiting for app version check on port(s): {self._ports_to_check_app}") else: # Check Complete - decide next state if self._reboop_required: @@ -768,16 +755,37 @@ def _update_state_check(self, delta): # pylint: disable=unused-argument self._sub_state = _SUB_EXIT + def _get_header_for_port(self, port: int) -> HexpansionHeader | None: + header = None + if hexpansion_app is not None: + if hasattr(hexpansion_app, "_hexpansion_manager"): + manager = hexpansion_app._hexpansion_manager # pylint: disable=protected-access + if manager is not None: + header = manager.hexpansion_headers[port] + return header + + + def get_active_hexdrive_unique_id(self) -> int | None: + """Return unique_id of the first active HexDrive port, if available.""" + app = self._app + for port in app.hexdrive_ports: + header = self._get_header_for_port(port) + unique_id = header.unique_id if header else None + if unique_id is not None: + return unique_id + return None + + def _update_state_port_select(self, delta: int): # pylint: disable=unused-argument app = self._app if app.button_states.get(BUTTON_TYPES["RIGHT"]): app.button_states.clear() - self._port_selected = (self._port_selected % _NUM_HEXPANSION_SLOTS) + 1 + self._port_selected = (self._port_selected % _SLOTS) + 1 self._read_port_header(self._port_selected) app.refresh = True elif app.button_states.get(BUTTON_TYPES["LEFT"]): app.button_states.clear() - self._port_selected = ((self._port_selected - 2) % _NUM_HEXPANSION_SLOTS) + 1 + self._port_selected = ((self._port_selected - 2) % _SLOTS) + 1 self._read_port_header(self._port_selected) app.refresh = True elif app.button_states.get(BUTTON_TYPES["CONFIRM"]): @@ -797,12 +805,12 @@ def _update_state_port_select(self, delta: int): # pylint: disable=unused-argu app.button_states.clear() if self._port_detail_page_count > 0: self._port_detail_page = (self._port_detail_page - 1) % self._port_detail_page_count - app.refresh = True + app.refresh = True elif app.button_states.get(BUTTON_TYPES["DOWN"]): app.button_states.clear() if self._port_detail_page_count > 0: self._port_detail_page = (self._port_detail_page + 1) % self._port_detail_page_count - app.refresh = True + app.refresh = True elif app.button_states.get(BUTTON_TYPES["CANCEL"]): app.button_states.clear() self._sub_state = _SUB_EXIT @@ -815,7 +823,7 @@ def _update_state_port_select(self, delta: int): # pylint: disable=unused-argu def _type_name_for_port(self, port: int, fallback_type_idx: int | None = None) -> str: """Return detected type name for a port, falling back to a selected type index.""" ignore_blank_eeprom = 1 if fallback_type_idx is not None else 0 - if port is not None and 1 <= port <= _NUM_HEXPANSION_SLOTS: + if port is not None and 1 <= port <= _SLOTS: type_idx = self._hexpansion_type_by_slot[port - 1] if type_idx is not None and 0 <= type_idx < len(self._app.HEXPANSION_TYPES)-ignore_blank_eeprom: return self._app.HEXPANSION_TYPES[type_idx].name @@ -833,9 +841,8 @@ def draw(self, ctx) -> bool: hexpansion_sub_type = app.HEXPANSION_TYPES[self._hexpansion_init_type].sub_type app.draw_message(ctx, ["Hexpansion", f"in slot {self._detected_port}:", "Init EEPROM as", hexpansion_type, f"{hexpansion_sub_type if hexpansion_sub_type else ''}?"], \ [(1, 1, 0), (1, 1, 0), (1, 1, 0), (1, 0, 1), (1, 0, 1)], label_font_size) - button_labels(ctx, confirm_label="Yes", up_label=app.special_chars['up'], down_label="\u25BC", \ - left_label=app.HEXPANSION_TYPES[app.HEXDRIVE_HEXPANSION_INDEX].name, \ - right_label=app.HEXPANSION_TYPES[app.HEXSENSE_HEXPANSION_INDEX].name, cancel_label="No") + button_labels(ctx, confirm_label="Yes", up_label=app.special_chars['up'], down_label="\u25BC", cancel_label="No") + return True elif self._sub_state == _SUB_PORT_SELECT: self._draw_port_select(ctx) @@ -845,20 +852,24 @@ def draw(self, ctx) -> bool: return False hexpansion_type_name = self._type_name_for_port(self._erase_port, self._hexpansion_init_type) # If the EEPROM type is unknown, show the proposed type and later allow selecting from common options. - app.draw_message(ctx, [hexpansion_type_name, f"in slot {self._erase_port}:", "Erase EEPROM?"], [(1, 0, 1), (1, 1, 0), (1, 0, 0)], label_font_size) + app.draw_message(ctx, [hexpansion_type_name, f"in slot {self._erase_port}:", "Erase EEPROM?"], \ + [(1, 0, 1), (1, 1, 0), (1, 0, 0)], label_font_size) button_labels(ctx, confirm_label="Yes", cancel_label="No") return True elif self._sub_state == _SUB_ERASE: if self._erase_port is None: return False hexpansion_type_name = self._type_name_for_port(self._erase_port, self._hexpansion_init_type) - app.draw_message(ctx, [hexpansion_type_name, f"in slot {self._erase_port}:", "Erasing..."], [(1, 0, 1), (1, 1, 0), (1, 0, 0)], label_font_size) + app.draw_message(ctx, [hexpansion_type_name, f"in slot {self._erase_port}:", "Erasing..."], \ + [(1, 0, 1), (1, 1, 0), (1, 0, 0)], label_font_size) return True elif self._sub_state == _SUB_UPGRADE_CONFIRM: if self._upgrade_port is None: return False hexpansion_type_name = self._type_name_for_port(self._upgrade_port, self._hexpansion_init_type) - app.draw_message(ctx, [hexpansion_type_name, f"in slot {self._upgrade_port}:", "Upgrade", f"{hexpansion_type_name} app?"], [(1, 0, 1), (1, 1, 0), (1, 1, 0), (1, 1, 0)], label_font_size) + upgrade_or_install = "Upgrade" if self._hexpansion_state_by_slot[self._upgrade_port-1] == _HEXPANSION_STATE_RECOGNISED_OLD_APP else "Install" + app.draw_message(ctx, [hexpansion_type_name, f"in slot {self._upgrade_port}:", upgrade_or_install, f"{hexpansion_type_name} app?"], \ + [(1, 0, 1), (1, 1, 0), (1, 1, 0), (1, 1, 0)], label_font_size) button_labels(ctx, confirm_label="Yes", cancel_label="No") return True elif self._sub_state == _SUB_PROGRAMMING: @@ -866,7 +877,8 @@ def draw(self, ctx) -> bool: if self._upgrade_port is None: return False hexpansion_type_name = self._type_name_for_port(self._upgrade_port, self._hexpansion_init_type) - app.draw_message(ctx, [f"{hexpansion_type_name}:", f"in slot {self._upgrade_port}:", "Programming", "Please wait..."], [(1, 0, 1), (1, 1, 0), (1, 1, 0), (1, 1, 0)], label_font_size) + app.draw_message(ctx, [f"{hexpansion_type_name}:", f"in slot {self._upgrade_port}:", "Programming", "Please wait..."], \ + [(1, 0, 1), (1, 1, 0), (1, 1, 0), (1, 1, 0)], label_font_size) return True return False @@ -923,16 +935,14 @@ def _draw_port_select(self, ctx): colours.append((0, 1, 1)) # Try to get running app version running_app = self._find_hexpansion_app(self._port_selected) - if running_app is not None: - try: - get_version = getattr(running_app, "get_version", None) - if get_version is None: - raise AttributeError("get_version") - ver = get_version() + if running_app is None: + lines.append("App not found") + colours.append((1, 0, 0)) + else: + ver = getattr(running_app, "VERSION", getattr(running_app, "version", None)) + if ver is not None: lines.append(f"v{ver}") colours.append((0, 1, 1)) - except Exception: # pylint: disable=broad-except - pass else: lines.append(hexpansion_state) colours.append((0, 1, 1)) @@ -964,12 +974,12 @@ def _draw_port_select(self, ctx): def _scan_ports(self) -> bool: """Scan all ports one at a time for known hexpansions, and update app state accordingly. Returns True when all have been scanned (even if no hexpansions are detected), False if the scan is still in progress.""" - # use _port_selected as the iterator variable for which port we are currently scanning, starting at 1 and going up to _NUM_HEXPANSION_SLOTS - if self._port_selected is None or self._port_selected > _NUM_HEXPANSION_SLOTS or self._port_selected < 1: + # use _port_selected as the iterator variable for which port we are currently scanning, starting at 1 and going up to _SLOTS + if self._port_selected is None or self._port_selected > _SLOTS or self._port_selected < 1: self._port_selected = 1 self._check_port_for_known_hexpansions(self._port_selected) self._port_selected += 1 - return self._port_selected > _NUM_HEXPANSION_SLOTS + return self._port_selected > _SLOTS def _read_header(self, port: int, i2c: I2C | None=None) -> HexpansionHeader | None: @@ -1005,7 +1015,7 @@ def _check_port_for_known_hexpansions(self, port) -> bool: """Check the given port for known hexpansion types by reading the EEPROM header, and update app state accordingly. Returns True if a known hexpansion type is detected (even if it was already known), False otherwise.""" app = self._app - if port not in range(1, _NUM_HEXPANSION_SLOTS + 1): + if port not in range(1, _SLOTS + 1): return False try: if self.logging: @@ -1022,7 +1032,6 @@ def _check_port_for_known_hexpansions(self, port) -> bool: # return False if self._logging: print(f"H:Found EEPROM on port {port}") - self._hexpansion_type_by_slot[port - 1] = app.BLANK_HEXPANSION_INDEX self._hexpansion_state_by_slot[port - 1] = _HEXPANSION_STATE_BLANK self._ports_to_initialise.add(port) return True @@ -1052,42 +1061,37 @@ def _check_port_for_known_hexpansions(self, port) -> bool: if self._logging: # report VID/PID in hexadecimal print(f"H:Port {port} - VID/PID {hex(hexpansion_header.vid)}/{hex(hexpansion_header.pid)} not recognised") - self._hexpansion_type_by_slot[port - 1] = app.UNRECOGNISED_HEXPANSION_INDEX self._hexpansion_state_by_slot[port - 1] = _HEXPANSION_STATE_UNRECOGNISED return False - def _check_hexpansion_app_on_port(self, port: int, type_index: int) -> object | None: + def _check_hexpansion_app_on_port(self, port: int, type_index: int, ) -> object | None: """Check if the app for the hexpansion on the given port is present and correct""" app = self._app - hexpansion_app = self._find_hexpansion_app(port) - if hexpansion_app is not None: - # get version number from app and compare to expected version for this hexpansion type - try: - get_version = getattr(hexpansion_app, "get_version", None) - if get_version is None: - raise AttributeError("get_version") - version = get_version() - except Exception as e: # pylint: disable=broad-except - try: - version = getattr(hexpansion_app, "version") - except Exception as ee: # pylint: disable=broad-except - print(f"H:Error getting app version for hexpansion on port {port}: {e}, {ee}") - version = None - if version != app.HEXPANSION_TYPES[type_index].app_mpy_version: - if self._logging: - app_version = getattr(hexpansion_app, "version", version) - print(f"H:{app.HEXPANSION_TYPES[type_index].name} app on port {port} has version {app_version}, expected {app.HEXPANSION_TYPES[type_index].app_mpy_version}") - self._hexpansion_state_by_slot[port - 1] = _HEXPANSION_STATE_RECOGNISED_OLD_APP - # add to upgrade list if not already there - if port not in self._ports_to_check_app: - self._ports_to_check_app.add(port) + check_hexpansion_app = self._find_hexpansion_app(port) + if check_hexpansion_app is not None: + # Read version from the running app object's VERSION attribute. + # EEPROM apps expose this on the class so per-port app instances + # can report their loaded code version reliably. + version = getattr(check_hexpansion_app, "VERSION", getattr(check_hexpansion_app, "version", None)) + expected = app.HEXPANSION_TYPES[type_index].app_mpy_version + if expected is None: + # No expected version recorded for this type – treat any running app as current. + self._hexpansion_state_by_slot[port - 1] = _HEXPANSION_STATE_RECOGNISED_APP_OK + elif not _versions_match(version, expected): + if self._hexpansion_state_by_slot[port - 1] != _HEXPANSION_STATE_RECOGNISED_OLD_APP: + if self._logging: + print(f"H:{app.HEXPANSION_TYPES[type_index].name} app on port {port} has version {version}, expected {expected}") + self._hexpansion_state_by_slot[port - 1] = _HEXPANSION_STATE_RECOGNISED_OLD_APP + # add to upgrade list if not already there + if port not in self._ports_to_check_app: + self._ports_to_check_app.add(port) else: self._hexpansion_state_by_slot[port - 1] = _HEXPANSION_STATE_RECOGNISED_APP_OK if self._logging: print(f"H:{app.HEXPANSION_TYPES[type_index].name} app found on port {port}") - return hexpansion_app + return check_hexpansion_app # ------------------------------------------------------------------ # EEPROM operations @@ -1107,7 +1111,7 @@ def _update_app_in_eeprom(self, port) -> int: if self._logging: print(f"H:Hexpansion type {app.HEXPANSION_TYPES[self._hexpansion_init_type].name} does not have an app to copy to EEPROM") return _APP_EEPROM_RESULT_FAILURE - source_file = f"EEPROM/{app.HEXPANSION_TYPES[self._hexpansion_init_type].app_mpy_name}" + source_file = f"EEPROM/{app.HEXPANSION_TYPES[self._hexpansion_init_type].app_mpy_name}.mpy" if self._logging: print(f"H:Writing app.mpy on port {port} with {source_file}") try: @@ -1307,11 +1311,22 @@ def _find_hexpansion_app(self, port: int) -> object | None: if hexpansion_type is None or hexpansion_type >= len(app.HEXPANSION_TYPES): return None expected_app_name = app.HEXPANSION_TYPES[hexpansion_type].app_name + candidate_app = None for an_app in scheduler.apps: if type(an_app).__name__ == expected_app_name: - if hasattr(an_app, "config") and hasattr(an_app.config, "port") and an_app.config.port == port: - return an_app - return None + if hasattr(an_app, "config"): + # if app has a config attribute, check if it has a port and if it matches the port we are checking + # - this is to avoid accidentally matching an app from a different hexpansion slot if there are multiple of the same type. + if hasattr(an_app.config, "port") and an_app.config.port == port: + #if self.logging: + # print(f"H:App {expected_app_name} has matching port {port} in config - app found") + return an_app + else: + # if app doesn't have a config we can't check the port - so assume it is a match + #if self.logging: + # print(f"H:Found app with matching name {expected_app_name} on port {port}") + candidate_app = an_app + return candidate_app # ------------------------------------------------------------------ @@ -1340,38 +1355,36 @@ def _check_ports_to_upgrade(self, delta) -> bool: port = self._ports_to_check_app.pop() self._waiting_app_port = port self._hexpansion_app_startup_timer = 0 - hexpansion_app = self._find_hexpansion_app(port) - if hexpansion_app is not None: - try: - get_version = getattr(hexpansion_app, "get_version", None) - if get_version is None: - raise AttributeError("get_version") - hexpansion_app_version = get_version() - except Exception as e: # pylint: disable=broad-except - hexpansion_app_version = 0 - print(f"H:Error getting Hexpansion app version - assume old: {e}") - elif 5000 < self._hexpansion_app_startup_timer: - if self._logging: - print("H:Timeout waiting for Hexpansion app to be started - assume it needs upgrading") - hexpansion_app_version = 0 - else: - if 0 == self._hexpansion_app_startup_timer: - if self._logging: - print(f"H:No app found on port {port} - WAITING for app to appear in Scheduler") - app.notification = Notification("Checking...", port=port) - self._hexpansion_app_startup_timer += delta - return True - if hexpansion_app_version == app.HEXPANSION_TYPES[self._hexpansion_type_by_slot[port - 1]].app_mpy_version: - if self._logging: - print(f"H:Hexpansion on port {port} has latest App") - self._hexpansion_state_by_slot[port - 1] = _HEXPANSION_STATE_RECOGNISED_APP_OK + type_index = self._hexpansion_type_by_slot[port - 1] + if type_index is None: + if self._logging: + print(f"H:Unexpectedly no hexpansion type for port {port} when checking app - skipping") + self._waiting_app_port = None + return False + check_hexpansion_app = self._check_hexpansion_app_on_port(port, type_index) + if check_hexpansion_app is not None: + if self._hexpansion_state_by_slot[port - 1] == _HEXPANSION_STATE_RECOGNISED_APP_OK: self._sub_state = _SUB_CHECK - else: + elif self._hexpansion_state_by_slot[port - 1] == _HEXPANSION_STATE_RECOGNISED_OLD_APP: if self._logging: - print(f"H:Hexpansion [{port}] version {hexpansion_app_version} upgrade to {app.HEXPANSION_TYPES[self._hexpansion_type_by_slot[port - 1]].app_mpy_version}") + print(f"H:Hexpansion [{port}] upgrade to {app.HEXPANSION_TYPES[type_index].app_mpy_version}?") self._upgrade_port = port - app.notification = Notification("Upgrade?", port=self._upgrade_port) self._sub_state = _SUB_UPGRADE_CONFIRM + elif 5000 < self._hexpansion_app_startup_timer: + if self._logging: + print("H:Timeout waiting for Hexpansion app to be started - assume it needs installing") + print(f"H:Hexpansion [{port}] install {app.HEXPANSION_TYPES[type_index].app_mpy_name} app?") + self._hexpansion_state_by_slot[port - 1] = _HEXPANSION_STATE_RECOGNISED_NO_APP + self._upgrade_port = port + self._sub_state = _SUB_UPGRADE_CONFIRM + else: + if 0 == self._hexpansion_app_startup_timer: + if self._logging: + print(f"H:No app found on port {port} - WAITING for app to appear in Scheduler") + self._hexpansion_app_startup_timer += delta + # Keep calling this function to keep checking for the app and updating the timer until we find the app or hit the timeout, at which point we will prompt to install. + return True + # Clear waiting app state so that we will check the next port on the next call. self._waiting_app_port = None self._hexpansion_app_startup_timer = 0 return True @@ -1380,6 +1393,30 @@ def _check_ports_to_upgrade(self, delta) -> bool: # ---- Hexpansion type descriptor ------------------------------------------- +def _versions_match(running, expected) -> bool: + """Return True when *running* (read from the hexpansion app's ``VERSION`` + attribute) equals *expected* (from ``HexpansionType.app_mpy_version``). + + * Integer versions are compared directly. + * String versions are tokenised the same way as ``parse_version()`` in + ``app.py`` so that ``"1.10"`` compares greater than ``"1.2"``. + * If *running* is ``None`` (attribute missing) the versions do not match. + """ + if running is None: + return False + if isinstance(expected, str) and isinstance(running, str): + def _tok(v): + v = v.strip("v") + if "+" in v: + v = v.split("+", 1)[0] + if "-" in v: + v = v.split("-", 1)[0] + parts = v.split(".") + return [int(p) if p.isdigit() else p for p in parts] + return _tok(running) == _tok(expected) + return running == expected + + class HexpansionType: """Descriptor for known hexpansion types, used for detection and EEPROM programming. diff --git a/motor_controller.py b/motor_controller.py index 382c2b7..f047e4c 100644 --- a/motor_controller.py +++ b/motor_controller.py @@ -23,7 +23,7 @@ except ImportError: _imu = None -from .app import MOTOR_PWM_FREQ +from .app import MOTOR_PWM_FREQ, MOTOR_POWER_SCALE_FACTOR # Constants inlined from Sensor_Testing constants.py to avoid splitting # application constants into a separate module. @@ -138,7 +138,7 @@ def __init__( self._accel_calibrated = False self._ramp_overshoot_m = 0.0 # estimated extra distance during ramp-down self._avg_loop_ms = _TICK_MS # measured average loop period - self._busy = False + self._busy = False if self._logging: print("MotorController initialised") @@ -153,7 +153,7 @@ def __init__( def logging(self) -> bool: """Whether to print diagnostic messages about motor controller activity.""" return self._logging - + @logging.setter def logging(self, value: bool): self._logging = value @@ -161,20 +161,20 @@ def logging(self, value: bool): @property def max_power(self) -> int: """Maximum motor power (PWM value) from settings.""" - return int(self._settings['max_power'].v) + return int(self._settings['max_power'].v) * MOTOR_POWER_SCALE_FACTOR @property def acceleration(self) -> int: """Acceleration for ramps, in motor PWM per second.""" - return max(1, int(self._settings['acceleration'].v)) + return max(1, int(self._settings['acceleration'].v) * MOTOR_POWER_SCALE_FACTOR) @property def drive_step_ms(self) -> int: """Estimated time in ms to drive one step (for time-based driving).""" return int(self._settings['drive_step_ms'].v) if 'drive_step_ms' in self._settings else 0 - + @property def turn_step_ms(self) -> int: @@ -186,7 +186,7 @@ def turn_step_ms(self) -> int: def is_busy(self): """True while a command is executing.""" return self._busy - + # ------------------------------------------------------------------ # Public high-level commands (all awaitable) @@ -559,7 +559,7 @@ def _send_output(self): print("[MC] apply_motor_directions_callback ignored invalid output") self._hexdrive.set_motors(output) - + @staticmethod def _slew(current, target, step): if current < target: diff --git a/motor_moves.py b/motor_moves.py index 4c46c49..942736d 100644 --- a/motor_moves.py +++ b/motor_moves.py @@ -37,18 +37,20 @@ _LONG_PRESS_MS = 750 # Default user timings for drive and turn steps (can be configured in settings) -_DEFAULT_ACCELERATION = 2500 -DEFAULT_MAX_POWER = 50000 # exposed for use in other modules +_ACCELERATION_SCALE_FACTOR = 512 +_POWER_SCALE_FACTOR = 512 +_DEFAULT_ACCELERATION = 24576 // _ACCELERATION_SCALE_FACTOR # user-friendly acceleration value +DEFAULT_MAX_POWER = 49152 // _POWER_SCALE_FACTOR # exposed for use in other modules _DEFAULT_USER_DRIVE_MS = 50 _DEFAULT_USER_TURN_MS = 20 -_MIN_ACCELERATION = 100 -_MIN_MAX_POWER = 1000 +_MIN_ACCELERATION = 1 # 1024 // _ACCELERATION_SCALE_FACTOR +_MIN_MAX_POWER = 10240 // _POWER_SCALE_FACTOR _MIN_USER_DRIVE_MS = 10 _MIN_USER_TURN_MS = 10 -_MAX_MAX_POWER = 65535 -_MAX_ACCELERATION = 20000 +_MAX_MAX_POWER = 65535 // _POWER_SCALE_FACTOR +_MAX_ACCELERATION = 65535 // _ACCELERATION_SCALE_FACTOR _MAX_USER_DRIVE_MS = 10000 _MAX_USER_TURN_MS = 10000 @@ -131,17 +133,20 @@ def make_power_plan(self, mysettings): """Convert the instruction's duration and direction into a power plan, which is a list of (power_tuple, duration) pairs.""" curr_power = 0 ramp_up = [] - max_ramp_up_ticks = ((self.directional_duration(mysettings) * self._duration) // (2 * _TICK_MS)) - 1 + _d = self._duration * self.directional_duration(mysettings) + _a = _ACCELERATION_SCALE_FACTOR * (mysettings['acceleration'].v if 'acceleration' in mysettings else _DEFAULT_ACCELERATION) + _m = _POWER_SCALE_FACTOR * (mysettings['max_power'].v if 'max_power' in mysettings else DEFAULT_MAX_POWER) + max_ramp_up_ticks = (_d // (2 * _TICK_MS)) - 1 for _ in range(max_ramp_up_ticks): - curr_power += mysettings['acceleration'].v - if curr_power >= mysettings['max_power'].v: - curr_power = mysettings['max_power'].v + curr_power += _a + if curr_power >= _m: + curr_power = _m break else: ramp_up.append((self.directional_power_tuple(curr_power), _TICK_MS)) power_durations = ramp_up.copy() # period of constant power after ramp-up, before ramp-down - user_power_duration = (self.directional_duration(mysettings) * self._duration) - (2 * len(ramp_up) * _TICK_MS) + user_power_duration = _d - (2 * len(ramp_up) * _TICK_MS) if user_power_duration > 0: power_durations.append((self.directional_power_tuple(curr_power), user_power_duration)) ramp_down = ramp_up.copy() @@ -246,6 +251,8 @@ def begin_moves(self): else: # Fallback: old power-plan iterator self.power_plan_iter = chain(*(instr.power_plan for instr in self.instructions)) + if self.logging: + print(f"M:Beginning motor moves with power plan iterator based on {len(self.instructions)} instructions") if len(app.hexdrive_apps) > 0: if app.hexdrive_apps[0].initialise() and app.hexdrive_apps[0].set_power(True) and app.hexdrive_apps[0].set_freq(MOTOR_PWM_FREQ): app.hexdrive_apps[0].set_logging(False) @@ -316,7 +323,7 @@ def background_update(self, delta: int) -> tuple[int, int] | None: #else: # Legacy power-plan path if self._sub_state == _SUB_RUN: - print("Running motor moves with power plan iterator") + #print("Running motor moves with power plan iterator") output = self._get_current_power_level(delta) else: output = None @@ -359,22 +366,26 @@ def _update_state_receive_instr(self, delta: int) -> None: if app.button_states.get(BUTTON_TYPES["CONFIRM"]): app.long_press_delta += delta if app.long_press_delta >= _LONG_PRESS_MS: - if self.power_plan_iter is None: + #if self.power_plan_iter is None: + # if self.logging: + # print("No instructions to run, returning to HELP") + # app.scroll_mode_enable(False) + # app.animation_counter = 0 + # self._sub_state = _SUB_HELP + # return + # if there are No instructions then warn the user and return to help, otherwise start the countdown to run the instructions + if len(self.instructions) == 0 and self.current_instruction is None: + if self.logging: + print("No instructions entered, returning to HELP") + app.notification = Notification("No instructions entered") app.scroll_mode_enable(False) app.animation_counter = 0 self._sub_state = _SUB_HELP - else: - # if there are No instructions then warn the user and return to help, otherwise start the countdown to run the instructions - if len(self.instructions) == 0 and self.current_instruction is None: - app.notification = Notification("No instructions entered") - app.scroll_mode_enable(False) - app.animation_counter = 0 - self._sub_state = _SUB_HELP - return - self.finalize_instruction() - app.countdown_next_state = STATE_MOTOR_MOVES - app.run_countdown_elapsed_ms = 0 - app.current_state = STATE_COUNTDOWN + return + self.finalize_instruction() + app.countdown_next_state = STATE_MOTOR_MOVES + app.run_countdown_elapsed_ms = 0 + app.current_state = STATE_COUNTDOWN app.scroll_mode_enable(False) app.long_press_delta = 0 else: @@ -542,7 +553,9 @@ def draw(self, ctx) -> bool: self._draw_receive_instr(ctx) elif self._sub_state == _SUB_RUN: current_power, _ = self.current_power_duration - power_str = str(tuple([int(x / (app.settings['max_power'].v // 100)) for x in current_power])) + # scale factor to get power values between 0 and 100 for display + s = _POWER_SCALE_FACTOR * app.settings['max_power'].v if 'max_power' in app.settings else DEFAULT_MAX_POWER + power_str = str(tuple([int((100*x) / s) for x in current_power])) app.draw_message(ctx, ["Running...", power_str], [(1, 1, 0), (1, 1, 0)], label_font_size) elif self._sub_state == _SUB_DONE: app.draw_message(ctx, ["Program", "complete!"], [(0, 1, 0), (0, 1, 0)], label_font_size) diff --git a/pyrightconfig.json b/pyrightconfig.json new file mode 100644 index 0000000..a5fe6a0 --- /dev/null +++ b/pyrightconfig.json @@ -0,0 +1,9 @@ +{ + "stubPath": "../typings", + "extraPaths": [ + "../../../modules", + "." + ], + "reportMissingImports": "none", + "reportMissingModuleSource": "none" +} diff --git a/sensor_manager.py b/sensor_manager.py index 25f0488..5800037 100644 --- a/sensor_manager.py +++ b/sensor_manager.py @@ -16,10 +16,10 @@ from .sensors import ALL_SENSOR_CLASSES from .sensors.sensor_base import SensorBase -#HexSense LED pin -_LED_PIN = 1 # LED to illumiinate area under colour sensor to mmeasure reflected light from surface below. -_INTERRUPT_PIN = 2 # Not currently used, but we can set it up as an input for future interrupt-based drivers +_LED_PIN = 2 # LED to illumiinate area under colour sensor to measure reflected light from surface below. +_COLOUR_INT_PIN = 1 # Not currently used, but we can set it up as an input for future interrupt-based drivers +_DIST_INT_PIN = 3 # Not currently used, but we can set it up as an input for future interrupt-based drivers class SensorManager: def __init__(self, logging: bool = False): @@ -81,17 +81,21 @@ def open(self, port: int) -> bool: if self.logging: print(f"SM:Port {port} scan: {[hex(a) for a in found_addrs]}") + used_addrs = set() for cls in ALL_SENSOR_CLASSES: addresses = getattr(cls, "I2C_ADDRS", (getattr(cls, "I2C_ADDR", 0),)) for address in addresses: if address not in found_addrs: continue + if address in used_addrs: + continue try: - sensor = cls(i2c_addr=address) + sensor = cls(i2c_addr=address, logging=self.logging) except TypeError: sensor = cls() if sensor.begin(self._i2c): self._sensors.append(sensor) + used_addrs.add(address) if self.logging: print(f"SM: + {cls.NAME} @ 0x{sensor.i2c_addr:02X} {cls.TYPE}") elif self.logging: @@ -100,7 +104,7 @@ def open(self, port: int) -> bool: self._index = 0 self._last_data = {} - # Set read interval from the first found sensor, or default to 250ms + # Set read interval from the first found sensor, or default to 250ms TODO: support multiple sensors with different intervals? if self._sensors: self._read_interval_ms = getattr(self._sensors[0], 'READ_INTERVAL_MS', 250) self._type = getattr(self._sensors[0], 'TYPE', 'Generic') @@ -111,24 +115,26 @@ def open(self, port: int) -> bool: # Enable LED only when at least one Colour sensor is present # (avoids pin conflicts with non-colour hexpansions such as the motor-test board) if len(self._sensors) > 0 and any(getattr(s, 'TYPE', '') == 'Colour' for s in self._sensors): - if self.logging: - print(f"SM:LED On port {port}") config = HexpansionConfig(port) + if self.logging: + print(f"SM:LED On port {port} pin {config.ls_pin[_LED_PIN]} for colour sensor") config.ls_pin[_LED_PIN].init(mode=Pin.OUT) config.ls_pin[_LED_PIN].value(1) - config.ls_pin[_INTERRUPT_PIN].init(mode=Pin.IN) + config.ls_pin[_COLOUR_INT_PIN].init(mode=Pin.IN) + config.ls_pin[_DIST_INT_PIN].init(mode=Pin.IN) return len(self._sensors) > 0 - def report_interrupt(self) -> bool: + def report_interrupt(self): """Check if the interrupt pin is active (low).""" if self._port is None: return False config = HexpansionConfig(self._port) - v = config.ls_pin[_INTERRUPT_PIN].value() - print(f"INT pin value: {v}") - return v == 0 + v = config.ls_pin[_COLOUR_INT_PIN].value() + print(f"[{self._port}] COLOUR INT pin value: {v}") + v = config.ls_pin[_DIST_INT_PIN].value() + print(f"[{self._port}] DIST INT pin value: {v}") def close(self): @@ -182,6 +188,7 @@ def read_current(self) -> dict: if not self._sensors: return {"Error": "no sensors"} self._last_data = self._sensors[self._index].read() + #self.report_interrupt() return self._last_data @@ -210,18 +217,18 @@ def last_data(self) -> dict: return self._last_data @property - def port(self): + def port(self) -> int | None: return self._port @property def is_open(self) -> bool: return self._i2c is not None and len(self._sensors) > 0 - def sensor_list(self) -> list: + def sensor_list(self) -> list[tuple[int, str]]: """Return [(index, name), ...] for all found sensors.""" return [(i, s.NAME) for i, s in enumerate(self._sensors)] - def get_sensor_by_name(self, name: str): + def get_sensor_by_name(self, name: str) -> SensorBase | None: """Return the first sensor instance whose NAME matches, or None.""" for s in self._sensors: if s.NAME == name: diff --git a/sensor_test.py b/sensor_test.py index 733cdc8..07664d2 100644 --- a/sensor_test.py +++ b/sensor_test.py @@ -10,22 +10,21 @@ # draw(ctx) – render sensor-test-related UI # init_settings(settings) – register sensor-test specific settings (none currently) +import time + from events.input import BUTTON_TYPES from app_components.tokens import label_font_size, button_labels from app_components.notification import Notification from system.hexpansion.config import HexpansionConfig +import settings as platform_settings + +from .sensor_manager import SensorManager + +from .app import SETTINGS_NAME_PREFIX, DEFAULT_BACKGROUND_UPDATE_PERIOD + try: - from egpio import ePin -except ImportError: - class ePin: # pylint: disable=invalid-name - """Simulator stub for egpio.ePin – used only for ePin.PWM mode constant.""" - PWM = None -from .app import DEFAULT_BACKGROUND_UPDATE_PERIOD, MOTOR_PWM_FREQ -try: - from machine import Pin, mem32, disable_irq, enable_irq + from machine import mem32, disable_irq, enable_irq except ImportError: - from machine import Pin - class _Mem32Shim: def __getitem__(self, _addr: int) -> int: return 0 @@ -56,40 +55,36 @@ def enable_irq(_state: int) -> None: # on MicroPython; replicate that so module-level const() calls work. const = lambda x: x #pylint: disable=unnecessary-lambda-assignment +_TIME_SLEEP_MS = getattr(time, "sleep_ms", None) + + +def _sleep_ms(delay_ms: int) -> None: + if _TIME_SLEEP_MS is not None: + _TIME_SLEEP_MS(delay_ms) + return + time.sleep(delay_ms / 1000) -# Constants for rotation rate measurement and motor test mode. -_ROTATION_RATE_MEASUREMENT_PERIOD_MS = 2500 # how often to update the displayed rotation rate measurement in ms (tradeoff between display responsiveness and stability of the reading) -_DEFAULT_ROTATION_RATE_EMITTER_DUTY = 20 # default duty cycle for the IR emitter when doing rate testing, 0-255 (0=off, 255=full on) -_DEFAULT_SPOKES_PER_ROTATION = 3 # number of times the photodiode will be triggered per full rotation of the wheel -_MOTOR_TEST_BACKGROUND_UPDATE_PERIOD = 1000 # background update period in ms to use during motor test mode (tradeoff between display responsiveness and CPU load) -_ROTATION_RATE_EMITTER_PINS = [2, 4] # LS_C & LS_D pins used to drive the IR emitter for rotation rate testing -_ROTATION_RATE_SENSOR_PINS = [0, 1] # HS_F & HS_G pins used to read the phottransistors for rotation rate testing -_ROTATION_RATE_SENSOR_ENABLE_PINS = [3] # LS_D pins used to enable the phototransistors for rotation rate testing (set to output and high to enable, input to disable) -_IR_EMITTER_PWM_STEP_SIZE = 2 # Step size for adjusting IR emitter brightness in manual mode, 0-255 (0=off, 255=full on) -# Temporary - while there is no EEPROM on the Test Hexpansion -_ROTATION_RATE_PORT = 1 # Hexpansion slot used for rotation rate measurement # Local sub-states (internal to Sensor Test) _SUB_SELECT_PORT = 0 _SUB_READING = 1 -_SUB_MOTOR_TEST = 2 - -# Rotation Rate Auto scan configuration -_AUTO_SCAN_STEPS = 50 # Number of power levels to test during auto scan -_AUTO_SCAN_SETTLE_MS = 200 # ms to wait after setting power before discarding counter -_AUTO_SCAN_MEASURE_MS = 2000 # ms measurement window per step # Pages of information to show for each sensor (can be switched with up/down buttons) _PAGE_RAW = 0 _PAGE_STATS = 1 _PAGE_DATA = 2 +_PAGE_CAL = 3 _PAGE_NAMES = { 0: "Raw", 1: "Stats", 2: "Data", + 3: "Cal", } +_WHITE_CAL_SCALE = 1024 +_WHITE_CAL_GAIN_PREFIX = "stc" + # Mapping colour RGB/XY values to human readable colour names. # Values are based on the CIE 1931 Chromaticity Diagram COLOR_REGIONS = [ @@ -104,13 +99,18 @@ def enable_irq(_state: int) -> None: {"name": "Gray", "x": (0.30, 0.45), "y": (0.25, 0.45)}, ] +_ENABLE_PIN = const(0) # First LS pin used to enable the SMPSU +_COLOUR_INT_PIN = const(1) # Second LS pin used to detect interrupts from the colour sensor to trigger readings without polling +_LED_PIN = const(2) # Third LS pin used to control an LED to illuminate the area under the colour sensor for better readings of reflected light from the surface below. +_DIST_INT_PIN = const(3) # Fourth LS pin used to detect interrupts from the distance sensor to trigger readings without polling +_DIST_XSHUT_PIN = const(4) # Fifth LS pin used to control the XSHUT pin of the distance sensor to allow it to be power cycled for reset or power saving + # ---- Settings initialisation ----------------------------------------------- def init_settings(s, MySetting: type): # pylint: disable=unused-argument, invalid-name - """Register sensor-test-specific settings in the shared settings dict. - Currently no dedicated settings, but the hook exists for future use.""" - # no sensor-test-specific settings at this time - + """Register sensor-test-specific settings in the shared settings dict.""" + # No settings currently, but this is where they would be registered if needed. + pass # ---- Sensor Test manager --------------------------------------------------- @@ -123,10 +123,10 @@ class SensorTestMgr: Reference to the main application instance. """ - def __init__(self, app, hextest_port: int | None = _ROTATION_RATE_PORT, logging: bool = False): + def __init__(self, app, logging: bool = False): self._app = app self._sub_state = _SUB_SELECT_PORT - self._sensor_mgr = None # SensorManager instance (lazy-imported) + self._sensor_mgr: SensorManager | None = None self._port_selected: int = 1 self._sensor_data: dict = {} self._display_data: dict = {} @@ -138,38 +138,9 @@ def __init__(self, app, hextest_port: int | None = _ROTATION_RATE_PORT, logging: self._count_timer: int = 0 # ms self._sample_rate: int = 0 # Hz self._new_sample: bool = False - self._colour: tuple = (1.0, 1.0, 0.0) # default to yellow for non-colour sensors - - self._rotation_rate_emitter_duty: int = _DEFAULT_ROTATION_RATE_EMITTER_DUTY # duty cycle for the IR emitter when doing rate testing, 0-255 (0=off, 255=full on) - self._rotation_rate_counters = [] # hardware counters used to count photodiode pulses for rate testing - self._rotation_rate_rpms: list[int | None] = [] # computed RPM values derived from counter deltas - self._rotation_rate_measurement_period_elapsed: int = 0 # ticks since last rate check, used to compute pulse rate in Hz based on the change in the counter value - self._rotation_rate_motor_power: int = 0 # Power applied to motors in TEST mode - self._rotation_rate_spokes: int = _DEFAULT_SPOKES_PER_ROTATION - self._rotation_rate_rounding: int = (_ROTATION_RATE_MEASUREMENT_PERIOD_MS * self._rotation_rate_spokes) // 2 - - # Auto scan state - self._auto_mode: bool = False # True = auto scanning, False = manual - self._auto_direction: int = 1 # 1 = forwards, -1 = reverse - self._auto_step: int = 0 # current step index (0.._AUTO_SCAN_STEPS-1) - self._auto_timer: int = 0 # elapsed ms within current phase - self._auto_settling: bool = True # True = in settle phase, False = in measure phase - self._auto_results: list[tuple[int, list[int], int | None]] = [] # list of (power, rpm list, current mA) - self._auto_max_rpm: int = 0 # max rpm seen during scan - self._auto_max_current_ma: int = 0 # max current seen during scan - self._auto_last_current_ma: int = 0 # latest current sampled in auto mode - self._auto_done: bool = False # True = scan complete - self._ina226 = None - self._ina226_sensor_mgr = None # SensorManager used exclusively for motor-test INA226 discovery - self._ina226_reading: dict[str, int] = {} - self._ina226_sum_current_ma: int = 0 - self._ina226_sum_bus_mv: int = 0 - self._ina226_sum_power_mw: int = 0 - self._ina226_sample_count: int = 0 - - # Use HS pins on a spare Hexpansion to measure rotation rate - self._test_support_hexpansion_config: HexpansionConfig | None = None - self.hextest_setup(hextest_port) + self._colour: tuple[float, float, float] = (1.0, 1.0, 0.0) # default to yellow for non-colour sensors + self._white_gains: tuple[int, int, int, int] | None = None # white reference gains for RGBC channels, scaled by _WHITE_CAL_SCALE + self._test_results: dict = {} # dict to hold test results if self._logging: print("SensorTestMgr initialised") @@ -199,27 +170,6 @@ def sample_count(self, value: int): self._sample_count = value - def hextest_setup(self, port: int | None): - """Use HS pins on a spare Hexpansion to make rotation rate measurements.""" - if self._test_support_hexpansion_config is not None and port != self._test_support_hexpansion_config.port: - try: - for i in range(4): - self._test_support_hexpansion_config.pin[i].init(mode=Pin.IN) - if self._sub_state == _SUB_MOTOR_TEST: - if self._logging: - print(f"Test Hexpansion {'removed' if port is None else 'changed'}") - self._app.notification = Notification("Motor Test - aborted", port=self._test_support_hexpansion_config.port) - self._stop_motor_test_mode() - except AttributeError: - pass # Simulator Pin stubs lack .init() - self._test_support_hexpansion_config = None - if port is not None and self._test_support_hexpansion_config is None: - if self._logging: - print(f"Setting up Hexpansion on port {port} for rotation rate measurement") - self._test_support_hexpansion_config = HexpansionConfig(port) - self._rotation_rate_enable(False) # start with rotation rate emitter and sensors off until we enter motor test mode - - # ------------------------------------------------------------------ # Entry point from menu # ------------------------------------------------------------------ @@ -233,22 +183,17 @@ def start(self) -> bool: self._display_data = {} app.refresh = True sensor_mgr = self._ensure_sensor_mgr() - self.colour = (1.0, 1.0, 0.0) # reset to yellow when starting sensor test - # If a HexDrive is present, try its port first + self._colour = (1.0, 1.0, 0.0) # reset to yellow when starting sensor test if app.hexdrive_ports is not None: + # If a HexDrive is present try its port for sensors for port in app.hexdrive_ports: if sensor_mgr.open(port): self._port_selected = port app.update_period = sensor_mgr.read_interval self._sub_state = _SUB_READING break - # If no HexDrive, but a HexSense is present, try its port next - elif app.hexsense_port is not None and sensor_mgr.open(app.hexsense_port): - self._port_selected = app.hexsense_port - app.update_period = sensor_mgr.read_interval - self._sub_state = _SUB_READING - # Otherwise, start in port selection mode else: + # Otherwise, start in port selection mode self._port_selected = 1 self._sub_state = _SUB_SELECT_PORT return True @@ -258,10 +203,10 @@ def start(self) -> bool: # Sensor Manager access # ------------------------------------------------------------------ - def _ensure_sensor_mgr(self) -> "SensorManager": + def _ensure_sensor_mgr(self) -> SensorManager: """Lazy-import and create SensorManager if needed.""" if self._sensor_mgr is None: - from .sensor_manager import SensorManager + #from .sensor_manager import SensorManager self._sensor_mgr = SensorManager(logging=self._logging) else: self._sensor_mgr.close() @@ -302,19 +247,6 @@ def colour(self, value: tuple): self._colour = value - @property - def rotation_rate_emitter_duty(self) -> int: - """Duty cycle (0-255) for the IR emitter when doing rotation rate testing.""" - return self._rotation_rate_emitter_duty - - @rotation_rate_emitter_duty.setter - def rotation_rate_emitter_duty(self, value: int): - self._rotation_rate_emitter_duty = value - if self._test_support_hexpansion_config is not None: - for pin_num in _ROTATION_RATE_EMITTER_PINS: - self._test_support_hexpansion_config.ls_pin[pin_num].duty(self._rotation_rate_emitter_duty) - - @staticmethod def lookup_color_XYZ(x: int, y: int, z: int, brightness_threshold: int = 10) -> str: #pylint: disable=invalid-name """ @@ -410,6 +342,19 @@ def lookup_colour_RGB(r: int, g: int, b: int, clear: int = 0) -> str: #pylint return "Magenta" + @staticmethod + def _apply_white_reference(r: int, g: int, b: int, w: int = 0, white_gains: tuple[int, int, int, int] | None = None) -> tuple[int, int, int, int]: + """Apply white reference gains to raw RGBC values and return adjusted RGBC tuple.""" + if white_gains is None: + return (r, g, b, w) + return ( + max(0, ((r * white_gains[0]) + (_WHITE_CAL_SCALE // 2)) // _WHITE_CAL_SCALE), + max(0, ((g * white_gains[1]) + (_WHITE_CAL_SCALE // 2)) // _WHITE_CAL_SCALE), + max(0, ((b * white_gains[2]) + (_WHITE_CAL_SCALE // 2)) // _WHITE_CAL_SCALE), + max(0, ((w * white_gains[3]) + (_WHITE_CAL_SCALE // 2)) // _WHITE_CAL_SCALE) if w > 0 else 0, + ) + + # ------------------------------------------------------------------ # Background update (called from the fast loop) # ------------------------------------------------------------------ @@ -420,45 +365,53 @@ def background_update(self, delta) -> tuple[int, int] | None: # pylint: disable sensor_mgr = self._sensor_mgr if sensor_mgr is None: return None - # need per sensor read timing here to balance responsiveness with CPU load, since some sensors can be slow to read and we don't want to bog down the system by reading too frequently. We also want to update the displayed sample rate at a regular interval (e.g. every second) based on the number of samples read in that time. + + self._count_timer += delta + if self._count_timer >= 1000: + # compute sample rate every second based on the number of samples read and the elapsed time + self._sample_rate = ((1000 * self.sample_count) + 500) // self._count_timer # sample rate in Hz + self._count_timer = 0 + self.sample_count = 0 + self._new_sample = True + + # need per sensor read timing here to balance responsiveness with CPU load, + # since some sensors can be slow to read and we don't want to bog down the system by reading too frequently. + # We also want to update the displayed sample rate at a regular interval (e.g. every second) based on the number of samples read in that time. #self._read_timer += delta #if self._read_timer >= self._sensor_mgr.read_interval: - #print(f"S:Reading sensor (S:read_timer={self._read_timer}ms, count_timer={self._count_timer}ms, sample_count={self.sample_count})") + #print(f"ST:Reading sensor (S:read_timer={self._read_timer}ms, count_timer={self._count_timer}ms, sample_count={self.sample_count})") #self._count_timer += self._read_timer #self._read_timer = 0 # Read sensor data in the background and update sample count and rate calculation + # TODO - make this more generic - interrupt property of sensor, and avoid having code split between sensor test and sensor manager... + # if colour sensor - see if the interrupt pin is active (low) before trying to read, to avoid long waits when the sensor is not ready with new data + config = HexpansionConfig(self._port_selected) + if sensor_mgr.type == "Colour": + if config.ls_pin[_COLOUR_INT_PIN].value(): + # interrupt pin active low - NOT active, so sensor not ready with new data + return None + self._test_results["colour int low"] = True + elif sensor_mgr.type == "Distance": + if config.ls_pin[_DIST_INT_PIN].value(): + #return None + pass + else: + self._test_results["distance int low"] = True + try: self._sensor_data = sensor_mgr.read_current() self.sample_count = self.sample_count + 1 except Exception as e: # pylint: disable=broad-exception-caught self._sensor_data = {"Error": str(e)} - self._count_timer += delta - if self._count_timer >= 1000: - # compute sample rate every second based on the number of samples read and the elapsed time - self._sample_rate = ((1000 * self.sample_count) + 500) // self._count_timer # sample rate in Hz - self._count_timer = 0 - self.sample_count = 0 - self._new_sample = True - elif self._sub_state == _SUB_MOTOR_TEST: - self._sample_ina226_in_background() - return (self._rotation_rate_motor_power, self._rotation_rate_motor_power) - return None - + if sensor_mgr.type == "Colour": + if config.ls_pin[_COLOUR_INT_PIN].value(): + self._test_results["colour int high"] = True + elif sensor_mgr.type == "Distance": + if config.ls_pin[_DIST_INT_PIN].value(): + self._test_results["distance int high"] = True - def _auto_rotation_rate_step(self): - self._auto_step += 1 - self._app.refresh = True - if self._auto_step >= _AUTO_SCAN_STEPS: - # Scan complete — stop motors - self._auto_done = True - self._rotation_rate_motor_power = 0 - self._auto_direction *= -1 # reverse direction for next scan - else: - # Advance to next power level - self._rotation_rate_motor_power = self._auto_direction * (65535 * self._auto_step) // (_AUTO_SCAN_STEPS - 1) - self._auto_timer = 0 - self._auto_settling = True + return None # ------------------------------------------------------------------ @@ -471,222 +424,32 @@ def update(self, delta: int): self._update_select_port(delta) elif self._sub_state == _SUB_READING: self._update_reading(delta) - elif self._sub_state == _SUB_MOTOR_TEST: - self._update_motor_test_mode(delta) - def _rotation_rate_enable(self, enable: bool = True) -> bool: - if self._test_support_hexpansion_config is None: - return False - try: - if enable: - if self._logging: - print("Enabling rotation rate emitter and sensors") - for pin_num in _ROTATION_RATE_EMITTER_PINS: - self._test_support_hexpansion_config.ls_pin[pin_num].init(mode=ePin.PWM) # Set LS pins to output mode to turn on the IR emitters - self._test_support_hexpansion_config.ls_pin[pin_num].duty(self.rotation_rate_emitter_duty) # Set LS pins to the current duty cycle to drive the IR emitters) - for pin_num in _ROTATION_RATE_SENSOR_ENABLE_PINS: - self._test_support_hexpansion_config.ls_pin[pin_num].init(mode=Pin.OUT) # Set LS pins to output mode to enable the phototransistors for rotation rate measurement - self._test_support_hexpansion_config.ls_pin[pin_num].value(1) # Set LS enable pins high to turn on the phototransistors for rotation rate measurement - else: - if self._logging: - print("Disabling rotation rate emitter and sensors") - for pin_num in _ROTATION_RATE_EMITTER_PINS: - self._test_support_hexpansion_config.ls_pin[pin_num].init(mode=Pin.IN) # Set LS pins to input mode to turn off the IR emitters - for pin_num in _ROTATION_RATE_SENSOR_ENABLE_PINS: - self._test_support_hexpansion_config.ls_pin[pin_num].init(mode=Pin.IN) # Set LS pins to input mode to turn off the phototransistors for rotation rate measurement - - for pin_num in _ROTATION_RATE_SENSOR_PINS: - self._test_support_hexpansion_config.pin[pin_num].init(mode=Pin.IN) # Set HS pins to input mode to read the phototransistors for rotation rate measurement - except AttributeError: - pass # Simulator Pin stubs lack .init() - return True - - def _init_ina226_for_motor_test(self) -> bool: - self._ina226 = None - self._ina226_sensor_mgr = None - self._ina226_reading = {} - self._reset_ina226_accumulators() - if self._test_support_hexpansion_config is None: - return False - try: - from .sensor_manager import SensorManager - mgr = SensorManager(logging=self._logging) - port = self._test_support_hexpansion_config.port - if not mgr.open(port): - mgr.close() - if self._logging: - print(f"S:INA226 – no sensors found on port {port}") - return False - # Find the first INA226 sensor in the discovered list - sensor = mgr.get_sensor_by_name("INA226") - if sensor is not None: - self._ina226 = sensor - self._ina226_sensor_mgr = mgr - if self._logging: - print(f"S:INA226 found @ 0x{sensor.i2c_addr:02X}") - return True - # No INA226 found; close the manager - mgr.close() - except Exception as e: # pylint: disable=broad-exception-caught - if self._logging: - print(f"S:INA226 init failed: {e}") - return False - - def _reset_ina226_accumulators(self) -> None: - self._ina226_sum_current_ma = 0 - self._ina226_sum_bus_mv = 0 - self._ina226_sum_power_mw = 0 - self._ina226_sample_count = 0 - - def _sample_ina226_in_background(self) -> None: - sensor = self._ina226 - if sensor is None: - return - data = sensor.read_sample_if_ready() - if data is None: - return - try: - self._ina226_sum_current_ma += int(data.get("current_mA", 0)) - self._ina226_sum_bus_mv += int(data.get("bus_mV", 0)) - self._ina226_sum_power_mw += int(data.get("power_mW", 0)) - self._ina226_sample_count += 1 - except Exception as e: # pylint: disable=broad-exception-caught - if self._logging: - print(f"S:INA226 sample error: {e}") - return - - def _consume_ina226_average(self) -> int | None: - if self._ina226_sample_count <= 0: - self._ina226_reading = {} - return None - count = self._ina226_sample_count - current_ma = self._ina226_sum_current_ma // count - self._ina226_reading = { - "current_mA": current_ma, - "bus_mV": self._ina226_sum_bus_mv // count, - "power_mW": self._ina226_sum_power_mw // count, - } - self._reset_ina226_accumulators() - return current_ma - - - def _update_motor_test_mode(self, delta: int): # pylint: disable=unused-argument - app = self._app - if self._test_support_hexpansion_config is None: - self._stop_motor_test_mode() - return - - # CANCEL always exits motor test mode - if app.button_states.get(BUTTON_TYPES["CANCEL"]): - app.button_states.clear() - self._stop_motor_test_mode() - return - - # CONFIRM toggles between manual and auto mode - elif app.button_states.get(BUTTON_TYPES["CONFIRM"]): - app.button_states.clear() - self._rotation_rate_motor_power = 0 - self._auto_last_current_ma = 0 - self._rotation_rate_measurement_period_elapsed = 0 - self._reset_ina226_accumulators() - for counter in self._rotation_rate_counters: - if counter is not None: - counter.value(0) # reset counter - if self._auto_mode: - # Switch back to manual - self._auto_mode = False - self._auto_done = False - else: - # Start auto scan - self._auto_mode = True - self._auto_done = False - self._auto_step = 0 - self._auto_timer = 0 - self._auto_settling = True - self._auto_results = [] - self._auto_max_rpm = 0 - self._auto_max_current_ma = 0 - app.refresh = True + def _setup_for_sensor_type(self): + sensor_mgr = self._sensor_mgr + if sensor_mgr is None: return - if self._auto_mode: - if not self._auto_done: - self._auto_timer += delta - if self._auto_settling: - if self._auto_timer >= _AUTO_SCAN_SETTLE_MS: - # Settle phase done — discard counter and start measuring - count = 0 - for counter in self._rotation_rate_counters: - if counter is not None: - count += counter.value(0) # read-and-reset to discard - if count == 0: - # There has been no motion from any motors - so we can skip the measure phase and move straight to the next power level - self._auto_rotation_rate_step() - else: - self._auto_timer = 0 - self._auto_settling = False - self._reset_ina226_accumulators() - else: - if self._auto_timer >= _AUTO_SCAN_MEASURE_MS: - # Measure phase done — read counter and record result - rounding = (_AUTO_SCAN_MEASURE_MS * self._rotation_rate_spokes) // 2 - rate = [0] * len(self._rotation_rate_counters) - for index, counter in enumerate(self._rotation_rate_counters): - if counter is not None: - count = counter.value(0) - rpm = ((60000 * count) + rounding) // (_AUTO_SCAN_MEASURE_MS * self._rotation_rate_spokes) - if rpm > self._auto_max_rpm: - self._auto_max_rpm = rpm - rate[index] = rpm - current_ma = self._consume_ina226_average() - if current_ma is not None: - current_abs = abs(current_ma) - self._auto_last_current_ma = current_ma - if current_abs > self._auto_max_current_ma: - self._auto_max_current_ma = current_abs - power = self._rotation_rate_motor_power - self._auto_results.append((power, rate, current_ma)) - self._auto_rotation_rate_step() - # In auto mode, no manual button control for power/IR - return - else: - # manual measurement mode - self._rotation_rate_measurement_period_elapsed += delta - if self._rotation_rate_measurement_period_elapsed >= _ROTATION_RATE_MEASUREMENT_PERIOD_MS: - count = 0 - for index, counter in enumerate(self._rotation_rate_counters): - if counter is not None: - count = counter.value(0) # read-and-reset to get the count for the elapsed period - self._rotation_rate_rpms[index] = ((60000 * count) + self._rotation_rate_rounding) // (self._rotation_rate_measurement_period_elapsed * self._rotation_rate_spokes) - self._rotation_rate_measurement_period_elapsed = 0 - self._consume_ina226_average() - if self.logging: - print(f"S:Rotation Rates: {self._rotation_rate_rpms}") - - # Manual mode button handling - if app.button_states.get(BUTTON_TYPES["UP"]): - app.button_states.clear() - self.rotation_rate_emitter_duty = min(255, self.rotation_rate_emitter_duty + _IR_EMITTER_PWM_STEP_SIZE) - if self.logging: - print(f"S:IR+Emitter Duty: {self.rotation_rate_emitter_duty}") - elif app.button_states.get(BUTTON_TYPES["DOWN"]): - app.button_states.clear() - self.rotation_rate_emitter_duty = max(0, self.rotation_rate_emitter_duty - _IR_EMITTER_PWM_STEP_SIZE) - if self.logging: - print(f"S:IR-Emitter Duty: {self.rotation_rate_emitter_duty}") - elif app.button_states.get(BUTTON_TYPES["RIGHT"]): - app.button_states.clear() - self._rotation_rate_motor_power = min(65535, self._rotation_rate_motor_power + 1000) - if self.logging: - print(f"S:Motor+Power: {self._rotation_rate_motor_power}") - elif app.button_states.get(BUTTON_TYPES["LEFT"]): - app.button_states.clear() - self._rotation_rate_motor_power = max(-65535, self._rotation_rate_motor_power - 1000) - if self.logging: - print(f"S:Motor-Power: {self._rotation_rate_motor_power}") + if self.logging: + print(f"ST:Opened sensor port {self._port_selected} with read_interval {sensor_mgr.read_interval}ms") + self._app.update_period = sensor_mgr.read_interval + # Reset all sensor and display data when starting to read a new sensor + self._sensor_data = {} + self._display_data = {} + self._read_timer = 0 + self._count_timer = 0 + self._sample_rate = 0 + self._sample_count = 0 + self._new_sample = False + self._colour = (1.0, 1.0, 0.0) # reset to yellow when switching sensors + # Sensor specific setup + if sensor_mgr.type == "Colour": + self._white_gains = self._load_white_gains("ref") + if self._white_gains is not None and self.logging: + print(f"ST:Loaded white gains from settings: {self._white_gains}") def _update_select_port(self, delta: int): # pylint: disable=unused-argument @@ -701,43 +464,107 @@ def _update_select_port(self, delta: int): # pylint: disable=unused-argument app.refresh = True elif app.button_states.get(BUTTON_TYPES["CONFIRM"]): app.button_states.clear() - motor_test_port = self._test_support_hexpansion_config.port if self._test_support_hexpansion_config is not None else 0 - if self._port_selected == motor_test_port and self._start_motor_test_mode(): - app.notification = Notification("Motor Test", port=self._port_selected) - if self.logging: - print(f"S:Entering Motor Test mode on port {self._port_selected}") - self._sub_state = _SUB_MOTOR_TEST - app.refresh = True + sensor_mgr = self._ensure_sensor_mgr() + app.refresh = True + if sensor_mgr.open(self._port_selected): + self._setup_for_sensor_type() + self._sub_state = _SUB_READING else: - sensor_mgr = self._ensure_sensor_mgr() - self._sensor_data = {} - self._display_data = {} - self._read_timer = 0 - self._count_timer = 0 - self._sample_rate = 0 - self._sample_count = 0 - self._new_sample = False - app.refresh = True - if sensor_mgr.open(self._port_selected): - app.update_period = sensor_mgr.read_interval - if self.logging: - print(f"Opened sensor port {self._port_selected} with read_interval {sensor_mgr.read_interval}ms") - self._sub_state = _SUB_READING - else: - app.notification = Notification(" No Sensors", port=self._port_selected) + app.notification = Notification(" No Sensors", port=self._port_selected) elif app.button_states.get(BUTTON_TYPES["CANCEL"]): app.button_states.clear() if self.logging: print("Exiting Sensor Test") if self._sensor_mgr is not None: self._sensor_mgr.close() - self._rotation_rate_enable(False) app.return_to_menu() + @staticmethod + def _ordered_display_items(display_data: dict) -> list[tuple[str, str]]: + """ with dicts not maintinaing order we need to force into a list in the order we want to display""" + items = [] + seen = set() + for key in ("r", "g", "b"): + if key in display_data: + items.append((key, str(display_data[key]))) + seen.add(key) + for key, value in display_data.items(): + if key not in seen: + items.append((key, str(value))) + return items + + + @staticmethod + def _white_gain_setting_keys(key: str) -> tuple[str, str, str, str]: + base = f"{_WHITE_CAL_GAIN_PREFIX}_{key}_" + return (f"{base}r", f"{base}g", f"{base}b", f"{base}w") + + + @staticmethod + def _reference_to_gains(r: int, g: int, b: int, w: int = 0) -> tuple[int, int, int, int]: + ref_r = max(int(r), 1) + ref_g = max(int(g), 1) + ref_b = max(int(b), 1) + ref_w = max(int(w), 1) if w > 0 else _WHITE_CAL_SCALE + gain_scale = _WHITE_CAL_SCALE * _WHITE_CAL_SCALE + return ( + (gain_scale + (ref_r // 2)) // ref_r, + (gain_scale + (ref_g // 2)) // ref_g, + (gain_scale + (ref_b // 2)) // ref_b, + (gain_scale + (ref_w // 2)) // ref_w, + ) + + + def _load_white_gains(self, key: str) -> tuple[int, int, int, int] | None: + setting_keys = self._white_gain_setting_keys(key) + values = [] + for setting_key in setting_keys: + value = platform_settings.get(f"{SETTINGS_NAME_PREFIX}.{setting_key}", None) + if value is None: + return None + values.append(int(value)) + gains = (values[0], values[1], values[2], values[3]) + return gains + + + def _update_page_count(self) -> None: + sensor_mgr = self._sensor_mgr + self._page_count = 4 if sensor_mgr is not None and sensor_mgr.type == "Colour" else 3 + if self._page_selected >= self._page_count: + self._page_selected = _PAGE_RAW + + + def _capture_white_reference(self) -> bool: + sensor_mgr = self._sensor_mgr + if sensor_mgr is None or sensor_mgr.type != "Colour": + return False + if not all(key in self._sensor_data for key in ("r", "g", "b")): + return False + + gains = self._reference_to_gains( + int(self._sensor_data["r"]), + int(self._sensor_data["g"]), + int(self._sensor_data["b"]), + int(self._sensor_data.get("w", 0)), + ) + # Update white gains in this manager + self._white_gains = gains + + # Save white gains to platform settings for persistence across sessions and availability in other modules + setting_keys = self._white_gain_setting_keys("ref") + for setting_key, gain in zip(setting_keys, gains): + platform_settings.set(f"{SETTINGS_NAME_PREFIX}.{setting_key}", gain) + if self._logging: + print(f"ST:Stored white gains: {gains}") + self._app.notification = Notification("White Cal Saved", port=self._port_selected) + return True + + def _update_display_values(self): # pylint: disable=unused-argument # clear old display data self._display_data = {} + self._update_page_count() # Sensor-specific display logic based on sensor type and available data if self._sensor_mgr and self._sensor_mgr.type == "Colour": @@ -767,7 +594,10 @@ def _update_display_values(self): # pylint: disable=unused-argument colour_name = f"x={x_f:.2f}, y={y_f:.2f}" self._display_data["colour"] = colour_name elif self._page_selected == _PAGE_RAW: - self._display_data = {k: str(v) for k, v in self._sensor_data.items()} + self._display_data = self._sensor_data + elif self._page_selected == _PAGE_CAL: + self._display_data["ref"] = "white" + self._display_data["press"] = "CONFIRM" #convert CIE1931 XYZ to RGB using a simple matrix transform r = int( 3.2406 * x - 1.5372 * y - 0.4986 * z) @@ -776,27 +606,28 @@ def _update_display_values(self): # pylint: disable=unused-argument except Exception as e: # pylint: disable=broad-exception-caught - print(f"S:Colour conversion error: {e}") + print(f"ST:Colour conversion error: {e}") r = g = b = 0 - elif all(k in self._sensor_data for k in ("red", "green", "blue")): + elif all(k in self._sensor_data for k in ("r", "g", "b")): try: - r = int(self._sensor_data["red"]) - g = int(self._sensor_data["green"]) - b = int(self._sensor_data["blue"]) + r = int(self._sensor_data["r"]) + g = int(self._sensor_data["g"]) + b = int(self._sensor_data["b"]) + w = int(self._sensor_data.get("w", 0)) + calibrated_r, calibrated_g, calibrated_b, calibrated_w = self._apply_white_reference(r, g, b, w, self._white_gains) if self._page_selected == _PAGE_DATA: - if "clear" in self._sensor_data: - clear = int(self._sensor_data["clear"]) - colour_name = self.lookup_colour_RGB(r, g, b, clear) - else: - colour_name = self.lookup_colour_RGB(r, g, b) + colour_name = self.lookup_colour_RGB(calibrated_r, calibrated_g, calibrated_b, calibrated_w) self._display_data["colour"] = colour_name elif self._page_selected == _PAGE_RAW: - self._display_data = {k: str(v) for k, v in self._sensor_data.items()} + self._display_data = self._sensor_data + elif self._page_selected == _PAGE_CAL: + self._display_data["ref"] = "white" + self._display_data["press"] = "CONFIRM" except Exception as e: # pylint: disable=broad-exception-caught - print(f"S:Colour conversion error: {e}") + print(f"ST:Colour conversion error: {e}") r = g = b = 0 else: r = g = b = 0 @@ -808,26 +639,29 @@ def _update_display_values(self): # pylint: disable=unused-argument red_f = r / max_channel green_f = g / max_channel blue_f = b / max_channel - self.colour = (red_f, green_f, blue_f) + self._colour = (red_f, green_f, blue_f) else: - self.colour = (1.0,1.0,0.0) # default to yellow if all channels are zero to avoid divide-by-zero and to provide a visible colour for non-colour sensors + self._colour = (1.0,1.0,0.0) # default to yellow if all channels are zero to avoid divide-by-zero and to provide a visible colour for non-colour sensors elif self._sensor_mgr and self._sensor_mgr.type == "Distance": - if self._page_selected == _PAGE_DATA and "dist_mm" in self._sensor_data: + if self._page_selected == _PAGE_DATA and "dist" in self._sensor_data: try: - dist_mm = int(self._sensor_data["dist_mm"]) + dist_mm = int(self._sensor_data["dist"]) if dist_mm < 20: - distance_str = f"{dist_mm}mm (Very Close)" + distance_str = "V Close" elif dist_mm < 100: - distance_str = f"{dist_mm}mm (Close)" + distance_str = "Close" elif dist_mm < 500: - distance_str = f"{dist_mm}mm (Medium)" + distance_str = "Medium" else: - distance_str = f"{dist_mm}mm (Far)" - self._display_data["Distance"] = distance_str + distance_str = "Far" + self._display_data["Range"] = distance_str + self._display_data["Dist"] = f"{dist_mm}mm" except Exception as e: # pylint: disable=broad-exception-caught - print(f"S:Distance processing error: {e}") + print(f"ST:Distance processing error: {e}") + elif self._page_selected == _PAGE_RAW: + self._display_data = self._sensor_data elif self._page_selected == _PAGE_RAW: - self._display_data = {k: str(v) for k, v in self._sensor_data.items()} + self._display_data = self._sensor_data if self._page_selected == _PAGE_STATS: if self._sample_rate > 0: @@ -843,17 +677,13 @@ def _update_reading(self, delta: int): # pylint: disable=unused-argument if app.button_states.get(BUTTON_TYPES["RIGHT"]) and self._sensor_mgr and self._sensor_mgr.num_sensors > 1: app.button_states.clear() - self.colour = (1.0, 1.0, 0.0) # reset to yellow when switching sensors self._sensor_mgr.next_sensor() - self._sensor_data = {} - self._display_data = {} + self._setup_for_sensor_type() # reset any sensor-specific settings for the new sensor app.refresh = True elif app.button_states.get(BUTTON_TYPES["LEFT"]) and self._sensor_mgr and self._sensor_mgr.num_sensors > 1: app.button_states.clear() - self.colour = (1.0, 1.0, 0.0) # reset to yellow when switching sensors self._sensor_mgr.prev_sensor() - self._sensor_data = {} - self._display_data = {} + self._setup_for_sensor_type() # reset any sensor-specific settings for the new sensor app.refresh = True elif app.button_states.get(BUTTON_TYPES["UP"]): app.button_states.clear() @@ -867,6 +697,11 @@ def _update_reading(self, delta: int): # pylint: disable=unused-argument self._page_selected = (self._page_selected + 1) % self._page_count self._update_display_values() app.refresh = True + elif app.button_states.get(BUTTON_TYPES["CONFIRM"]): + app.button_states.clear() + if self._page_selected == _PAGE_CAL and self._capture_white_reference(): + self._update_display_values() + app.refresh = True elif app.button_states.get(BUTTON_TYPES["CANCEL"]): app.button_states.clear() sensor_mgr = self._sensor_mgr @@ -877,82 +712,6 @@ def _update_reading(self, delta: int): # pylint: disable=unused-argument app.refresh = True - def _start_motor_test_mode(self) -> bool: - # enable HexDrive power - app = self._app - if len(app.hexdrive_apps) > 0 and self._test_support_hexpansion_config is not None: - app.hexdrive_apps[0].set_logging(True) - if app.hexdrive_apps[0].initialise() and app.hexdrive_apps[0].set_power(True) and app.hexdrive_apps[0].set_freq(MOTOR_PWM_FREQ): - app.hexdrive_apps[0].set_keep_alive(2000) # Updates can be quite slow as we are using the draw function - app.hexdrive_apps[0].set_motors((-1,-1)) # Try forcing PWM to be reinitialised by swapping direction. - # Enable the IR emitter for measuring wheel rotation rate - self._rotation_rate_enable(True) - - # Enable the phototransistor input for measuring wheel rotation rate - for pin_num in _ROTATION_RATE_SENSOR_PINS: - # configure the ESP32S3 hardware to count pulses on the HS_F pin - # Counter not yet available in this Micropython port so we have created our own... - gpio_num = _HS_PIN_TO_GPIO[self._test_support_hexpansion_config.port][pin_num] - counter = Counter(None, gpio_num, filter_ns=1000000, logging=self.logging) # auto-select PCNT unit - if counter is not None and counter.unit is not None: - self._rotation_rate_counters.append(counter) - else: - if self.logging: - print(f"S:Failed to allocate PCNT counter for pin {pin_num} (GPIO {gpio_num})") - app.notification = Notification("PCNT Init Failed") - # deinit any counters we did manage to create before returning - for c in self._rotation_rate_counters: - if c is not None: - c.deinit() - self._rotation_rate_counters = [] - return False - if self.logging: - print(f"S:Rate counter {self._rotation_rate_counters}") - self._rotation_rate_measurement_period_elapsed = 0 - self._rotation_rate_rpms = [0] * len(self._rotation_rate_counters) - self._init_ina226_for_motor_test() - app.update_period = _MOTOR_TEST_BACKGROUND_UPDATE_PERIOD # update every 1000ms to give a responsive display without overwhelming the CPU with updates - return True - if self.logging: - print("H:Failed to initialise HexDrive for motor test mode") - app.notification = Notification("HexDrive Init Failed") - return False - - - def _stop_motor_test_mode(self): - if self._logging: - print("Stopping Motor Test mode and cleaning up") - app = self._app - self._auto_mode = False - self._auto_done = False - self._rotation_rate_motor_power = 0 - self._ina226_reading = {} - self._reset_ina226_accumulators() - if self._ina226 is not None: - if self._ina226_sensor_mgr is not None: - try: - self._ina226_sensor_mgr.close() - except Exception as exc: # pylint: disable=broad-exception-caught - if self._logging: - print("INA226 sensor manager close failed:", exc) - self._ina226_sensor_mgr = None - self._ina226 = None - - if len(app.hexdrive_apps) > 0: - app.hexdrive_apps[0].set_pwm((0, 0, 0, 0)) - app.hexdrive_apps[0].set_power(False) - - for c in self._rotation_rate_counters: - if c is not None: - c.deinit() - self._rotation_rate_counters = [] - - app.update_period = DEFAULT_BACKGROUND_UPDATE_PERIOD - self._rotation_rate_enable(False) - self._sub_state = _SUB_SELECT_PORT - app.refresh = True - - # ------------------------------------------------------------------ # Draw # ------------------------------------------------------------------ @@ -965,101 +724,9 @@ def draw(self, ctx): elif self._sub_state == _SUB_READING: self._draw_reading(ctx) return True - elif self._sub_state == _SUB_MOTOR_TEST: - self._draw_motor_test_mode(ctx) - return True return False - def _draw_motor_test_mode(self, ctx): - if self._test_support_hexpansion_config is None: - return - if self._auto_mode: - self._draw_auto_scan(ctx) - return - # Manual mode: show the current emitter duty cycle as a percentage in the label, and show the current photodiode reading and rate counter value in the display data - lines = [f"IR:{int(self.rotation_rate_emitter_duty * 100 // 255)}%"] - colours = [(1, 1, 0)] - # Show power - lines += [f"Pwr:{self._rotation_rate_motor_power}"] - colours += [(0, 1, 1)] - for index, rpm in enumerate(self._rotation_rate_rpms): - if rpm is not None: - lines += [f"{index}: {rpm}rpm"] - colours += [(1, 0, 1)] - if self._ina226_reading: - lines += [f"I:{self._ina226_reading.get('current_mA', 0)}mA"] - colours += [(1, 0.3, 0.3)] - lines += [f"V:{self._ina226_reading.get('bus_mV', 0)}mV"] - colours += [(0.3, 0.8, 1.0)] - self._app.draw_message(ctx, lines, colours, label_font_size) - button_labels(ctx, up_label="IR+", down_label="IR-", cancel_label="Back", - left_label="Pwr-", right_label="Pwr+", confirm_label="Auto") - - - def _draw_auto_scan(self, ctx): - """Draw a chart of power vs RPM from the auto scan results.""" - # Chart area within the 240x240 circular display (origin at centre) - chart_left = -90 - chart_right = 90 - chart_top = -65 - chart_bottom = 65 - chart_w = chart_right - chart_left - chart_h = chart_bottom - chart_top - - # Background - ctx.rgb(0.05, 0.05, 0.05).rectangle(chart_left - 5, chart_top - 5, chart_w + 10, chart_h + 10).fill() - - # Axes - ctx.rgb(0.4, 0.4, 0.4) - ctx.move_to(chart_left, chart_bottom).line_to(chart_right, chart_bottom).stroke() # X axis - ctx.move_to(chart_left, chart_bottom).line_to(chart_left, chart_top).stroke() # Y axis - - n = len(self._auto_results) - max_rpm = self._auto_max_rpm if self._auto_max_rpm > 0 else 1 - max_current_ma = self._auto_max_current_ma if self._auto_max_current_ma > 0 else 1 - - if n > 1: - # Plot data points as small bars. - # Auto-scan results may contain either a scalar RPM or a list/tuple - # of per-counter RPMs. Reduce multi-counter readings to a single - # scalar for this chart by using the maximum measured RPM. - bar_w = max(1, chart_w // _AUTO_SCAN_STEPS) - for i in range(n): - power, rpms, current_ma = self._auto_results[i] - x = chart_left + (abs(power) * chart_w) // 65535 - for index, rpm in enumerate(rpms): - h = (rpm * chart_h) // max_rpm - if h > 0: - # colour by index to differentiate multiple counters if present - if index == 0: - ctx.rgb(0.0, 1.0, 0.5) - else: - ctx.rgb(1.0, 0.5, 0.0) - ctx.rectangle(x, chart_bottom - h, bar_w, h).fill() - if current_ma is not None: - current_h = (abs(current_ma) * chart_h) // max_current_ma - marker_y = chart_bottom - current_h - ctx.rgb(1.0, 0.2, 0.2) - ctx.rectangle(x + bar_w, marker_y - 1, 2, 2).fill() - - # Title and max RPM label - ctx.rgb(1, 1, 0) - ctx.font_size = label_font_size - if self._auto_done: - ctx.move_to(-55, chart_top - 5).text("Complete") - else: - progress = (self._auto_step * 100) // _AUTO_SCAN_STEPS - ctx.move_to(-55, chart_top - 5).text(f"Scan {progress}%") - - ctx.rgb(0, 1, 1) - ctx.move_to(-60, chart_bottom + label_font_size + 2).text(f"Max:{max_rpm}rpm") - ctx.rgb(1.0, 0.2, 0.2) - ctx.move_to(15, chart_bottom + label_font_size + 2).text(f"Ipk:{max_current_ma}mA") - - button_labels(ctx, cancel_label="Back", confirm_label="Manual") - - def _draw_select_port(self, ctx): self._app.draw_message(ctx, ["Sensor Test", f"Port: {self._port_selected}"], @@ -1070,7 +737,7 @@ def _draw_select_port(self, ctx): def _draw_reading(self, ctx): - up_label = down_label = "" + up_label = down_label = confirm_label = "" sensor_mgr = self._sensor_mgr num_sensors = sensor_mgr.num_sensors if sensor_mgr else 1 sensor_name = sensor_mgr.current_sensor_name if sensor_mgr else "Sensor" @@ -1086,9 +753,9 @@ def _draw_reading(self, ctx): lines += [f"{sensor_name}-{_PAGE_NAMES[self._page_selected]}"] colours += [(1, 0, 1)] if self._display_data: - for label, value in self._display_data.items(): + for label, value in self._ordered_display_items(self._display_data): lines += [f"{label}:{value}"] - colours += [self.colour] + colours += [self._colour] else: lines += ["Reading..."] colours += [(0.5, 0.5, 0.5)] @@ -1101,12 +768,14 @@ def _draw_reading(self, ctx): down_label=_PAGE_NAMES[down_page] if self._page_count > 1 else "" # only show the UP label if there are more than 2 pages, otherwise it would just show the same as the DOWN up_label=_PAGE_NAMES[up_page] if self._page_count > 2 else "" + if self._page_selected == _PAGE_CAL and sensor_mgr is not None and sensor_mgr.type == "Colour": + confirm_label = "Store" if num_sensors > 1: button_labels(ctx, left_label=" int: + return _PCNT_BASE + unit * _PCNT_UNIT_STRIDE + + +def _pcnt_conf1_addr(unit: int) -> int: + return _pcnt_conf0_addr(unit) + _PCNT_CONF1_OFFSET + + +def _pcnt_conf2_addr(unit: int) -> int: + return _pcnt_conf0_addr(unit) + _PCNT_CONF2_OFFSET + + +def _pcnt_cnt_addr(unit: int) -> int: + return _PCNT_BASE + _PCNT_CNT_OFFSET + unit * 4 + + +def _pcnt_rst_bit(unit: int) -> int: + return 1 << (unit * 2) + + +def _pcnt_signal_index(unit: int, channel: int, control: bool = False) -> int: + return _PCNT_SIG_BASE + unit * 4 + channel + (2 if control else 0) + + +def _pcnt_gpio_label(gpio: int) -> str: + hs_pin = _GPIO_TO_HS.get(gpio) + if hs_pin is None: + return f"GPIO {gpio}" + return f"GPIO {gpio} (port {hs_pin[0]} HS pin {hs_pin[1]})" + + +def _pcnt_filter_bits(filter_ns: int | None) -> int: + if filter_ns is None or filter_ns <= 0: + return 0 + filter_val = (_APB_CLK_HZ * filter_ns) // 1_000_000_000 + if filter_val > 1023: + filter_val = 1023 + return (filter_val & _CONF0_FILTER_THRES_M) | _CONF0_FILTER_EN + + +def _pcnt_enable_peripheral() -> None: + mem32[_CLK_EN0_REG] |= _PCNT_CLK_BIT + mem32[_RST_EN0_REG] &= ~_PCNT_CLK_BIT + + +def _pcnt_disable_peripheral() -> None: + mem32[_CLK_EN0_REG] &= ~_PCNT_CLK_BIT + mem32[_RST_EN0_REG] |= _PCNT_CLK_BIT + + +def _pcnt_route_input(signal_index: int, gpio: int | None) -> None: + route = _SIG_IN_SEL_BIT | (_PCNT_GPIO_CONST_HIGH if gpio is None else gpio) + mem32[_GPIO_FUNC_IN_SEL_CFG_BASE + (signal_index * 4)] = route + + +def _pcnt_read_count_signed(unit: int) -> int: + raw = mem32[_pcnt_cnt_addr(unit)] & _PCNT_COUNTER_MASK + if raw & _PCNT_COUNTER_SIGN_BIT: + return raw - _PCNT_COUNTER_MODULO + return raw + + +def _pcnt_reset_counter(unit: int) -> None: + rst_bit = _pcnt_rst_bit(unit) + irq_state = disable_irq() + mem32[_PCNT_CTRL_REG] |= rst_bit + mem32[_PCNT_CTRL_REG] &= ~rst_bit + enable_irq(irq_state) + + +def _pcnt_unit_in_use(unit: int, logging: bool = False) -> bool: + """Return True when *unit* appears to be configured and active.""" + clk_on = (mem32[_CLK_EN0_REG] & _PCNT_CLK_BIT) != 0 + if not clk_on: + if logging: + print(f"PCNT: unit {unit} - peripheral clock off, unit free") + return False + + ctrl = mem32[_PCNT_CTRL_REG] + if not (ctrl & _PCNT_CTRL_CLK_EN): + if logging: + print(f"PCNT: unit {unit} - register clock gate off, unit free") + return False + + rst_bit = _pcnt_rst_bit(unit) + if ctrl & rst_bit: + if logging: + print(f"PCNT: unit {unit} - held in reset, unit free") + return False + + conf0 = mem32[_pcnt_conf0_addr(unit)] + if conf0 in (0, 0x3C10): + if logging: + print(f"PCNT: unit {unit} - CONF0=0x{conf0:08X} (unconfigured), unit free") + return False + + if logging: + cnt = mem32[_pcnt_cnt_addr(unit)] & _PCNT_COUNTER_MASK + pulse_sig = _pcnt_signal_index(unit, 0) + gpio_route = mem32[_GPIO_FUNC_IN_SEL_CFG_BASE + pulse_sig * 4] + routed_gpio = gpio_route & 0x3F + print(f"PCNT: unit {unit} - IN USE: CONF0=0x{conf0:08X}, count={cnt}, routed to GPIO {routed_gpio}") + return True + + +def _pcnt_allocate_unit(unit: int | None, logging: bool = False) -> int | None: + if unit is not None: + if unit < 0 or unit >= _PCNT_NUM_UNITS: + if logging: + print(f"PCNT: unit {unit} out of range (0-{_PCNT_NUM_UNITS - 1})") + return None + if _pcnt_unit_in_use(unit, logging): + if logging: + print(f"PCNT: requested unit {unit} is already in use") + return None + if logging: + print(f"PCNT: using requested unit {unit}") + return unit + + for candidate in range(_PCNT_NUM_UNITS): + if not _pcnt_unit_in_use(candidate, logging): + if logging: + print(f"PCNT: auto-selected unit {candidate}") + return candidate + + if logging: + print("PCNT: all units in use, no free unit available") + return None + + +def _pcnt_disable_peripheral_if_unused(logging: bool = False) -> None: + if any(_pcnt_unit_in_use(unit) for unit in range(_PCNT_NUM_UNITS)): + return + _pcnt_disable_peripheral() + if logging: + print("PCNT: all units released, peripheral clock disabled") + + +class _PCNTUnitBase: + """Shared low-level PCNT unit allocation and teardown helpers.""" + + def __init__(self, unit: int | None, logging: bool = False): self.logging = logging + self.unit = _pcnt_allocate_unit(unit, logging) self._configured = False - if unit is not None: - if unit < 0 or unit >= _PCNT_NUM_UNITS: - if self.logging: - print(f"PCNT: unit {unit} out of range (0-{_PCNT_NUM_UNITS - 1})") - self.unit = None - return - if self._unit_in_use(unit): - self.unit = None - return - self.unit = unit - else: - # Auto-select first available unit - self.unit = None - for u in range(_PCNT_NUM_UNITS): - if not self._unit_in_use(u): - self.unit = u - break - if self.unit is None: - if self.logging: - print("PCNT: all units in use, no free unit available") - return + def _log(self, message: str) -> None: + if self.logging: + print(message) - if not self.init(src, filter_ns): - if self.logging: - print(f"PCNT: failed to configure unit {self.unit}") - self.unit = None + def _begin_configuration(self) -> tuple[int, int]: + unit = self.unit + if unit is None: + raise ValueError("PCNT unit not available") + _pcnt_enable_peripheral() - def _unit_in_use(self, unit: int) -> bool: - """Check whether a PCNT unit appears to already be in use. + ctrl = mem32[_PCNT_CTRL_REG] + ctrl |= _PCNT_CTRL_CLK_EN | _pcnt_rst_bit(unit) + mem32[_PCNT_CTRL_REG] = ctrl - A unit is considered in use if: - - The peripheral clock is enabled AND - - The register clock gate is enabled AND - - The unit is NOT held in reset AND - - CONF0 is non-zero (has been configured) - """ - # Check peripheral clock - clk_on = (mem32[_CLK_EN0_REG] & _PCNT_CLK_BIT) != 0 - if not clk_on: - if self.logging: - print(f"PCNT: unit {unit} - peripheral clock off, unit free") - return False + mem32[_pcnt_conf0_addr(unit)] = 0 + mem32[_pcnt_conf1_addr(unit)] = 0 + mem32[_pcnt_conf2_addr(unit)] = 0 + return _pcnt_conf0_addr(unit), _pcnt_cnt_addr(unit) + + def _finish_configuration(self, conf0_addr: int, cnt_addr: int) -> None: + unit = self.unit + if unit is None: + raise ValueError("PCNT unit not available") ctrl = mem32[_PCNT_CTRL_REG] + ctrl &= ~_pcnt_rst_bit(unit) + mem32[_PCNT_CTRL_REG] = ctrl + _pcnt_reset_counter(unit) + self._configured = True - # Check register clock gate - if not ctrl & _PCNT_CTRL_CLK_EN: - if self.logging: - print(f"PCNT: unit {unit} - register clock gate off, unit free") - return False + self._log( + f"PCNT U{unit}: configured OK, CONF0=0x{mem32[conf0_addr]:08X}, " + f"CTRL=0x{mem32[_PCNT_CTRL_REG]:08X}, CNT={mem32[cnt_addr] & _PCNT_COUNTER_MASK}" + ) - # Check if held in reset (reset bit = unit * 2) - rst_bit = 1 << (unit * 2) - if ctrl & rst_bit: - if self.logging: - print(f"PCNT: unit {unit} - held in reset, unit free") - return False + def deinit(self): + """Release the PCNT unit and make it available again.""" + if not self._configured or self.unit is None: + return - # Check CONF0 register - conf0_addr = _PCNT_BASE + unit * 0x0C - conf0 = mem32[conf0_addr] - if conf0 == 0x3C10: # a slightly odd reset state - if self.logging: - print(f"PCNT: unit {unit} - CONF0=0x3C10 (unconfigured), unit free") - return False + unit = self.unit + mem32[_PCNT_CTRL_REG] |= _pcnt_rst_bit(unit) + mem32[_pcnt_conf0_addr(unit)] = 0 + mem32[_pcnt_conf1_addr(unit)] = 0 + mem32[_pcnt_conf2_addr(unit)] = 0 + self._configured = False + + self._log(f"PCNT U{unit}: released") + _pcnt_disable_peripheral_if_unused(self.logging) - # Unit appears to be actively configured and running - if self.logging: - cnt_addr = _PCNT_BASE + 0x30 + unit * 4 - cnt = mem32[cnt_addr] & 0xFFFF - pulse_sig = _PCNT_SIG_BASE + unit * 4 - gpio_route = mem32[_GPIO_FUNC_IN_SEL_CFG_BASE + pulse_sig * 4] - routed_gpio = gpio_route & 0x3F - print(f"PCNT: unit {unit} - IN USE: CONF0=0x{conf0:08X}, " - f"count={cnt}, routed to GPIO {routed_gpio}") - return True +class Counter(_PCNTUnitBase): + """Wrapper around ESP32-S3 PCNT hardware for counting rising edges.""" + + def __init__(self, unit: int | None, src: int, filter_ns: int = 0, logging: bool = False): + self.pin = src + super().__init__(unit, logging) + if self.unit is None: + return + if not self.init(src, filter_ns): + self._log(f"PCNT: failed to configure unit {self.unit}") + self.unit = None def __str__(self): if self.unit is None: return "Counter(not configured)" - count = self.value() - return f"Counter(unit={self.unit}, GPIO={self.pin}, count={count})" + return f"Counter(unit={self.unit}, GPIO={self.pin}, count={self.value()})" + __repr__ = __str__ def init(self, src: int, filter_ns: int | None = None) -> bool: - """Configure a PCNT unit to count rising edges on the GPIO pin specified by src.""" - self.pin = src - + """Configure the unit to count rising edges on *src*.""" unit = self.unit if unit is None: return False - conf0_addr = _PCNT_BASE + unit * 0x0C - cnt_addr = _PCNT_BASE + 0x30 + unit * 4 - rst_bit = 1 << (unit * 2) - pulse_sig = _PCNT_SIG_BASE + unit * 4 # PCNT_SIG_CH0_INn - ctrl_sig = _PCNT_SIG_BASE + unit * 4 + 2 # PCNT_CTRL_CH0_INn - if self.logging: - hs = _GPIO_TO_HS.get(self.pin) - hs_str = f" port {hs[0]} HS pin {hs[1]})" if hs else "" - print(f"PCNT U{unit}: on GPIO {self.pin}{hs_str}, filter_ns={filter_ns}ns") - print(f" CONF0 addr=0x{conf0_addr:08X}, CNT addr=0x{cnt_addr:08X}") - print(f" pulse_sig={pulse_sig}, ctrl_sig={ctrl_sig}") + self.pin = src + conf0_addr, cnt_addr = self._begin_configuration() + pulse_sig = _pcnt_signal_index(unit, 0) + ctrl_sig = _pcnt_signal_index(unit, 0, control=True) + aux_pulse_sig = _pcnt_signal_index(unit, 1) + aux_ctrl_sig = _pcnt_signal_index(unit, 1, control=True) - try: - # --- 1. ENABLE PERIPHERAL CLOCK --- - mem32[_CLK_EN0_REG] |= _PCNT_CLK_BIT - mem32[_RST_EN0_REG] &= ~_PCNT_CLK_BIT - - # --- 2. ENABLE REGISTER CLOCK GATE, HOLD THIS UNIT IN RESET --- - # Read-modify-write to preserve other units' state - ctrl = mem32[_PCNT_CTRL_REG] - ctrl |= _PCNT_CTRL_CLK_EN | rst_bit - mem32[_PCNT_CTRL_REG] = ctrl - - # --- 3. ROUTE GPIO VIA MATRIX --- - mem32[_GPIO_FUNC_IN_SEL_CFG_BASE + (pulse_sig * 4)] = _SIG_IN_SEL_BIT | self.pin - # Route constant high (0x38) to control signal - mem32[_GPIO_FUNC_IN_SEL_CFG_BASE + (ctrl_sig * 4)] = _SIG_IN_SEL_BIT | 0x38 - - # --- 4. CONFIGURE COUNTING --- - # Calculate filter threshold from min pulse width - if filter_ns is not None and filter_ns > 0: - filter_val = (_APB_CLK_HZ * filter_ns) // 1_000_000_000 - if filter_val > 1023: - filter_val = 1023 - config = (filter_val & _CONF0_FILTER_THRES_M) | _CONF0_FILTER_EN - else: - config = 0 - config |= (1 << _CONF0_CH0_POS_MODE_S) # Inc on rising edge - mem32[conf0_addr] = config + self._log(f"PCNT U{unit}: counter on {_pcnt_gpio_label(src)}, filter_ns={filter_ns}ns") - # --- 5. RELEASE FROM RESET --- - ctrl = mem32[_PCNT_CTRL_REG] - ctrl &= ~rst_bit - mem32[_PCNT_CTRL_REG] = ctrl + try: + _pcnt_route_input(pulse_sig, src) + _pcnt_route_input(ctrl_sig, None) + _pcnt_route_input(aux_pulse_sig, None) + _pcnt_route_input(aux_ctrl_sig, None) - self._configured = True + config = _pcnt_filter_bits(filter_ns) + config |= _PCNT_COUNT_INCREMENT << _CONF0_CH0_POS_MODE_S + mem32[conf0_addr] = config - except Exception as e: # pylint: disable=broad-exception-caught - print(f"PCNT U{unit}: error configuring: {e}") + self._finish_configuration(conf0_addr, cnt_addr) + except Exception as exc: # pylint: disable=broad-exception-caught + self._log(f"PCNT U{unit}: error configuring counter: {exc}") + self._configured = False return False if self.logging: - print(f"PCNT U{unit}: configured OK, " - f"CONF0=0x{mem32[conf0_addr]:08X}, " - f"CTRL=0x{mem32[_PCNT_CTRL_REG]:08X}, " - f"CNT={mem32[cnt_addr] & 0xFFFF}") + print(f"PCNT U{unit}: configured - CONF0=0x{mem32[conf0_addr]:08X}, CTRL=0x{mem32[_PCNT_CTRL_REG]:08X}, CNT={mem32[cnt_addr] & 0xFFFF}") return True - def value(self, value: int | None = None) -> int: - """Read the current count and optionally reset the counter to zero. - DOES NOT SUPPORT SETTING THE COUNTER TO AN ARBITRARY VALUE, ONLY RESETTING TO ZERO.""" - if not self._configured: + """Return the current count, optionally read-and-reset on ``value(0)``.""" + if not self._configured or self.unit is None: return 0 + count = mem32[_pcnt_cnt_addr(self.unit)] & _PCNT_COUNTER_MASK + if value == 0: + _pcnt_reset_counter(self.unit) + return count + + +class Encoder(_PCNTUnitBase): + """4x quadrature encoder wrapper built on a single ESP32-S3 PCNT unit.""" + + def __init__( + self, + unit: int | None, + phase_a: int, + phase_b: int, + filter_ns: int = 0, + max: int | None = None, + min: int = 0, + logging: bool = False, + ): + self.phase_a = phase_a + self.phase_b = phase_b + self._position = 0 + self._cycles = 0 + self._last_raw = 0 + self._range_min = 0 + self._range_max = 0 + self._range_enabled = False + super().__init__(unit, logging) + if self.unit is None: + return + if not self.init(phase_a, phase_b, filter_ns=filter_ns, max=max, min=min): + self._log(f"PCNT: failed to configure encoder on unit {self.unit}") + self.unit = None + + def __str__(self): + if self.unit is None: + return "Encoder(not configured)" + return ( + f"Encoder(unit={self.unit}, phase_a={self.phase_a}, phase_b={self.phase_b}, position={self.value()}, cycles={self._cycles})" + ) + + __repr__ = __str__ + + def init( + self, + phase_a: int, + phase_b: int, + filter_ns: int = 0, + max: int | None = None, + min: int = 0, + ) -> bool: + """Configure the unit for 4x quadrature decoding on *phase_a* and *phase_b*.""" unit = self.unit if unit is None: - return 0 + return False + if phase_a == phase_b: + self._log("PCNT: encoder phase_a and phase_b must use different GPIOs") + return False - rst_bit = 1 << (unit * 2) - cnt_addr = _PCNT_BASE + 0x30 + unit * 4 - if value is not None and value == 0: - irq_state = disable_irq() - count = mem32[cnt_addr] & 0xFFFF - mem32[_PCNT_CTRL_REG] |= rst_bit - mem32[_PCNT_CTRL_REG] &= ~rst_bit - enable_irq(irq_state) - else: - count = mem32[cnt_addr] & 0xFFFF - return count + range_enabled = max is not None and not (max == 0 and min == 0) + range_max = 0 if max is None else max + range_min = 0 if max is None else min + if range_enabled and range_max < range_min: + self._log(f"PCNT U{unit}: invalid encoder range min={range_min}, max={range_max}") + return False + self.phase_a = phase_a + self.phase_b = phase_b + conf0_addr, cnt_addr = self._begin_configuration() + range_desc = "hardware range" + if range_enabled: + range_desc = f"min={range_min}, max={range_max}" + self._log( + f"PCNT U{unit}: encoder on {_pcnt_gpio_label(phase_a)} and {_pcnt_gpio_label(phase_b)}, phases=4, filter_ns={filter_ns}ns, {range_desc}" + ) - def deinit(self): - """Release the PCNT unit: hold it in reset and clear its CONF0.""" + try: + _pcnt_route_input(_pcnt_signal_index(unit, 0), phase_a) + _pcnt_route_input(_pcnt_signal_index(unit, 0, control=True), phase_b) + _pcnt_route_input(_pcnt_signal_index(unit, 1), phase_b) + _pcnt_route_input(_pcnt_signal_index(unit, 1, control=True), phase_a) + + config = _pcnt_filter_bits(filter_ns) + config |= _PCNT_COUNT_INCREMENT << _CONF0_CH0_NEG_MODE_S + config |= _PCNT_COUNT_DECREMENT << _CONF0_CH0_POS_MODE_S + config |= _PCNT_CTRL_REVERSE << _CONF0_CH0_LCTRL_MODE_S + config |= _PCNT_COUNT_DECREMENT << _CONF0_CH1_NEG_MODE_S + config |= _PCNT_COUNT_INCREMENT << _CONF0_CH1_POS_MODE_S + config |= _PCNT_CTRL_REVERSE << _CONF0_CH1_LCTRL_MODE_S + mem32[conf0_addr] = config + + self._finish_configuration(conf0_addr, cnt_addr) + except Exception as exc: # pylint: disable=broad-exception-caught + self._log(f"PCNT U{unit}: error configuring encoder: {exc}") + self._configured = False + return False + + self._range_min = range_min + self._range_max = range_max + self._range_enabled = range_enabled + self._position = range_min if self._range_enabled else 0 + self._cycles = 0 + self._last_raw = _pcnt_read_count_signed(unit) + return True + + def _update_position(self) -> None: if not self._configured or self.unit is None: return - unit = self.unit - conf0_addr = _PCNT_BASE + unit * 0x0C - rst_bit = 1 << (unit * 2) - mem32[_PCNT_CTRL_REG] |= rst_bit # hold in reset - mem32[conf0_addr] = 0 # clear config so unit appears free - self._configured = False + + raw = _pcnt_read_count_signed(self.unit) + delta = raw - self._last_raw + if delta > _PCNT_COUNTER_MAX: + delta -= _PCNT_COUNTER_MODULO + elif delta < -_PCNT_COUNTER_SIGN_BIT: + delta += _PCNT_COUNTER_MODULO + self._last_raw = raw + + if delta == 0: + return + + previous_cycles = self._cycles + if self._range_enabled: + span = (self._range_max - self._range_min) + 1 + absolute = self._cycles * span + (self._position - self._range_min) + absolute += delta + self._cycles, offset = divmod(absolute, span) + self._position = self._range_min + offset + else: + self._position += delta if self.logging: - print(f"PCNT U{unit}: released") + wrap_note = " (wrapped)" if self._cycles != previous_cycles else "" + print( + f"PCNT U{self.unit}: encoder delta={delta}, position={self._position}, cycles={self._cycles}{wrap_note}" + ) - # disable the peripheral clock if no units are in use to save power - if not any(self._unit_in_use(u) for u in range(_PCNT_NUM_UNITS)): - mem32[_CLK_EN0_REG] &= ~_PCNT_CLK_BIT - mem32[_RST_EN0_REG] |= _PCNT_CLK_BIT - if self.logging: - print("PCNT: all units released, peripheral clock disabled") + def value(self, value: int | None = None) -> int: + """Return the current position and optionally reset it with ``value(0)``.""" + if not self._configured or self.unit is None: + return 0 + + self._update_position() + position = self._position + if value == 0: + if self._range_enabled and not (self._range_min <= 0 <= self._range_max): + raise ValueError("0 outside configured encoder range") + _pcnt_reset_counter(self.unit) + self._last_raw = _pcnt_read_count_signed(self.unit) + self._position = 0 + self._cycles = 0 + self._log(f"PCNT U{self.unit}: encoder position reset to 0, cycles=0") + return position + + def cycles(self) -> int: + """Return the current logical wrap/underflow cycle count.""" + if not self._configured or self.unit is None: + return 0 + + self._update_position() + return self._cycles diff --git a/sensors/__init__.py b/sensors/__init__.py index 5a2e263..57acceb 100644 --- a/sensors/__init__.py +++ b/sensors/__init__.py @@ -30,8 +30,8 @@ def _try_add_sensor(import_name: str, class_name: str) -> None: # Ordered list used by the manager when scanning a port. _try_add_sensor("vl53l0x", "VL53L0X") _try_add_sensor("vl6180x", "VL6180X") -_try_add_sensor("tcs3472", "TCS3472") -_try_add_sensor("tcs3430", "TCS3430") -_try_add_sensor("opt4048", "OPT4048") +#_try_add_sensor("tcs3472", "TCS3472") +#_try_add_sensor("tcs3430", "TCS3430") +#_try_add_sensor("opt4048", "OPT4048") _try_add_sensor("opt4060", "OPT4060") -_try_add_sensor("ina226", "INA226") +#_try_add_sensor("ina226", "INA226") diff --git a/sensors/ina226.py b/sensors/ina226.py index d98520e..1ed2849 100644 --- a/sensors/ina226.py +++ b/sensors/ina226.py @@ -9,27 +9,8 @@ """ import time - from .sensor_base import SensorBase -try: - _ticks_ms = time.ticks_ms - _ticks_add = time.ticks_add - _ticks_diff = time.ticks_diff - _sleep_ms = time.sleep_ms -except AttributeError: - def _ticks_ms() -> int: - return int(time.time() * 1000) - - def _ticks_add(base: int, delta: int) -> int: - return base + delta - - def _ticks_diff(a: int, b: int) -> int: - return a - b - - def _sleep_ms(delay_ms: int) -> None: - time.sleep(delay_ms / 1000) - # Register map _REG_CONFIGURATION = 0x00 # Configuration register @@ -46,12 +27,12 @@ def _sleep_ms(delay_ms: int) -> None: # Configuration register bits (0x00) _CFG_RESET_BIT = 0x8000 # Software reset bit -_CFG_AVG_SHIFT = 12 # Averaging field shift (bits 14:12) -_CFG_VBUSCT_SHIFT = 9 # Bus voltage conversion time field shift (bits 11:9) -_CFG_VSHCT_SHIFT = 6 # Shunt voltage conversion time field shift (bits 8:6) +_CFG_AVG_SHIFT = 9 # Averaging field shift (bits 11:9) +_CFG_VBUSCT_SHIFT = 6 # Bus voltage conversion time field shift (bits 8:6) +_CFG_VSHCT_SHIFT = 3 # Shunt voltage conversion time field shift (bits 5:3) _CFG_MODE_SHIFT = 0 # Operating mode field shift (bits 2:0) -# AVG field values (bits 14:12) +# AVG field values (bits 11:9) _CFG_AVG_1 = 0b000 # 1 sample average _CFG_AVG_4 = 0b001 # 4 sample average _CFG_AVG_16 = 0b010 # 16 sample average @@ -61,7 +42,7 @@ def _sleep_ms(delay_ms: int) -> None: _CFG_AVG_512 = 0b110 # 512 sample average _CFG_AVG_1024 = 0b111 # 1024 sample average -# Conversion time field values for VBUSCT/VSHCT (bits 11:9 and 8:6) +# Conversion time field values for VBUSCT/VSHCT (bits 8:6 and 5:3) _CFG_CT_140US = 0b000 # 140 us conversion time _CFG_CT_204US = 0b001 # 204 us conversion time _CFG_CT_332US = 0b010 # 332 us conversion time @@ -105,14 +86,14 @@ def _sleep_ms(delay_ms: int) -> None: _CALIBRATION_VALUE = 0x0200 # 512 => 0.1 mA current register LSB with 100 mΩ shunt _CURRENT_LSB_UA = 100 # 0.1 mA current LSB in microamps _POWER_LSB_UW = 2500 # 2.5 mW power LSB in microwatts -_READ_TIMEOUT_MS = 10 +_READ_TIMEOUT_MS = 50 # Default operating configuration: # - shunt conversion: 8.244 ms # - bus conversion: 1.1 ms -# - averaging: 16 samples +# - averaging: 16 sample _DEFAULT_CONFIGURATION = ( - (_CFG_AVG_16 << _CFG_AVG_SHIFT) + (_CFG_AVG_16 << _CFG_AVG_SHIFT) | (_CFG_CT_1100US << _CFG_VBUSCT_SHIFT) | (_CFG_CT_8244US << _CFG_VSHCT_SHIFT) | (_CFG_MODE_SHUNT_BUS_CONT << _CFG_MODE_SHIFT) @@ -131,19 +112,16 @@ class INA226(SensorBase): def _measure_from_registers(self) -> dict[str, int]: bus_raw = self._read_u16_be(_REG_BUS_VOLTAGE) current_raw = self._read_s16_be(_REG_CURRENT) - power_raw = self._read_u16_be(_REG_POWER) # Bus LSB = 1.25 mV bus_mv = (bus_raw * 125) // 100 # Current LSB from calibration = 100 uA (0.1 mA) current_ma = (current_raw * _CURRENT_LSB_UA) // 1000 - # Power LSB from calibration = 2500 uW (2.5 mW) - power_mw = (power_raw * _POWER_LSB_UW) // 1000 + #print(f"S:{self.NAME} {bus_mv}mV, {current_ma}mA") return { - "bus_mV": bus_mv, - "current_mA": current_ma, - "power_mW": power_mw, + "mV": bus_mv, + "mA": current_ma, } def read_sample_if_ready(self) -> dict[str, int] | None: @@ -157,10 +135,14 @@ def read_sample_if_ready(self) -> dict[str, int] | None: return None status = self._read_u16_be(_REG_MASK_ENABLE) if (status & _MASK_CVRF) == 0: - print(f"S:{self.NAME} sample not ready (status=0x{status:04X})") + #print(f"S:{self.NAME} sample not ready (status=0x{status:04X})") + return None + if (status & _MASK_OVF) != 0: + print(f"S:{self.NAME} math overflow (status=0x{status:04X})") return None return self._measure_from_registers() + def _init(self) -> bool: manufacturer = self._read_u16_be(_REG_MANUFACTURER_ID) if manufacturer != _MANUFACTURER_ID_TI: @@ -172,19 +154,18 @@ def _init(self) -> bool: return True - def _measure(self) -> dict: - deadline = _ticks_add(_ticks_ms(), _READ_TIMEOUT_MS) + def _measure(self, timeout: int=_READ_TIMEOUT_MS) -> dict: + deadline = time.ticks_add(time.ticks_ms(), timeout) while True: sample = self.read_sample_if_ready() if sample is not None: return { - "bus_mV": str(sample["bus_mV"]), - "current_mA": str(sample["current_mA"]), - "power_mW": str(sample["power_mW"]), + "mV": str(sample["mV"]), + "mA": str(sample["mA"]), } - if _ticks_diff(deadline, _ticks_ms()) <= 0: + if time.ticks_diff(deadline, time.ticks_ms()) <= 0: return {"Error": "timeout"} - _sleep_ms(1) + time.sleep_ms(1) def _shutdown(self) -> None: self._write_u16_be(_REG_CONFIGURATION, _CFG_MODE_POWER_DOWN) diff --git a/sensors/opt4048.py b/sensors/opt4048.py index 558cc74..336c7d2 100644 --- a/sensors/opt4048.py +++ b/sensors/opt4048.py @@ -162,7 +162,7 @@ def set_latched_interrupt(self, enabled: bool, threshold_ch: int = 3, threshold_ if enabled: # Setup Threshold self._write_u16_be(_REG_THRESH_LO, threshold_low) # low threshold - self._write_u16_be(_REG_THRESH_HI, threshold_high) # high threshold + self._write_u16_be(_REG_THRESH_HI, threshold_high) # high threshold cfg = self._read_u16_be(_REG_CONFIG) if enabled: @@ -204,7 +204,7 @@ def _init(self) -> bool: # Enable conversion-ready interrupt so status polling works #self.set_interrupt_enabled(True) - # The conversion ready interrupt is only 1us in duration which is too short for the LS pin to + # The conversion ready interrupt is only 1us in duration which is too short for the LS pin to # reliably capture, so we enable latching mode and poll the status register for the ready flag instead. self.set_latched_interrupt(True, threshold_low = 0x8400, threshold_high = 0x8400) @@ -222,6 +222,9 @@ def _init(self) -> bool: return True def _measure(self) -> dict: + if self._i2c is None: + return {"Error": "not initialized"} + # Poll status for conversion-ready; timeout after 30 ms deadline = time.ticks_add(time.ticks_ms(), self.READ_INTERVAL_MS) while True: diff --git a/sensors/opt4060.py b/sensors/opt4060.py index 01620fa..bc7b754 100644 --- a/sensors/opt4060.py +++ b/sensors/opt4060.py @@ -150,12 +150,7 @@ _RES_CTRL_FLAG_H_MASK = 0x0002 # Bit 1 _RES_CTRL_FLAG_L_MASK = 0x0001 # Bit 0 -# Legacy single-bit names (used internally by driver logic) -_FLAG_READY = _RES_CTRL_CONV_READY_MASK -_FLAG_OVERLOAD = _RES_CTRL_OVERLOAD_MASK -_FLAG_HIGH = _RES_CTRL_FLAG_H_MASK -_FLAG_LOW = _RES_CTRL_FLAG_L_MASK - +_READ_TIMEOUT_MS = 100 # Max time to wait for conversion-ready status in _measure() class OPT4060(SensorBase): """Driver for the TI OPT4060 RGBW colour sensor. @@ -278,13 +273,16 @@ def _init(self) -> bool: return True - def _measure(self) -> dict: - # Poll status for conversion-ready; timeout after READ_INTERVAL_MS - deadline = time.ticks_add(time.ticks_ms(), self.READ_INTERVAL_MS) + def _measure(self, timeout: int = _READ_TIMEOUT_MS) -> dict: + if self._i2c is None: + return {"Error": "not initialized"} + + # Poll status for conversion-ready + deadline = time.ticks_add(time.ticks_ms(), timeout) while True: st = self._read_u16_be(_REG_RES_CTRL) - if st & _FLAG_READY: - self._overload = bool(st & _FLAG_OVERLOAD) + if st & _RES_CTRL_CONV_READY_MASK: + self._overload = bool(st & _RES_CTRL_OVERLOAD_MASK) break if time.ticks_diff(deadline, time.ticks_ms()) <= 0: return {"Error": "timeout"} @@ -298,10 +296,10 @@ def _measure(self) -> dict: blue = self._decode_channel(raw, 8) w = self._decode_channel(raw, 12) return { - "red": str(red), - "green": str(green), - "blue": str(blue), - "w": str(w), + "r": str(red), + "g": str(green), + "b": str(blue), + "w": str(w), } @staticmethod diff --git a/sensors/sensor_base.py b/sensors/sensor_base.py index 557b1de..f8f04b1 100644 --- a/sensors/sensor_base.py +++ b/sensors/sensor_base.py @@ -14,16 +14,18 @@ class SensorBase: + """Abstract base class for BadgeBot I2C sensor drivers.""" # Sub-classes must override these I2C_ADDR = 0x00 NAME = "Unknown" READ_INTERVAL_MS = 250 TYPE = "Generic" - def __init__(self, i2c_addr: int | None = None): + def __init__(self, i2c_addr: int | None = None, logging: bool = False): self._i2c = None self._ready = False self._i2c_addr = self.I2C_ADDR if i2c_addr is None else i2c_addr + self._logging = logging # ------------------------------------------------------------------ # Public API (called by SensorManager / app.py) @@ -44,7 +46,7 @@ def begin(self, i2c) -> bool: self._ready = False return self._ready - def read(self) -> dict: + def read(self, timeout: int | None = None) -> dict: """Return the latest measurement as {label: value_string}. Returns an empty dict or {'Error': 'msg'} on failure. @@ -52,11 +54,15 @@ def read(self) -> dict: if not self._ready: return {"Error": "not ready"} try: - return self._measure() + return self._measure(timeout=timeout) if timeout is not None else self._measure() except Exception as e: # pylint: disable=broad-exception-caught print(f"S:{self.NAME} read error: {e}") return {"Error": str(e)} + def read_sample_if_ready(self) -> dict | None: + """Optional non-blocking sample hook for sensors that support it.""" + return None + def reset(self): """Put the sensor into a low-power / safe state.""" try: @@ -76,10 +82,12 @@ def shutdown(self): @property def is_ready(self) -> bool: + """True if the sensor is initialised and ready for measurements.""" return self._ready @property def i2c_addr(self) -> int: + """Return the I2C address of the sensor.""" return self._i2c_addr # ------------------------------------------------------------------ @@ -90,7 +98,7 @@ def _init(self) -> bool: """Hardware initialisation. Return True on success.""" raise NotImplementedError - def _measure(self) -> dict: + def _measure(self, timeout: int = 0) -> dict: """Perform measurement. Return dict of {label: value_str}.""" raise NotImplementedError @@ -108,9 +116,13 @@ def _shutdown(self): # ------------------------------------------------------------------ def _write_reg(self, reg: int, data: bytes): + if self._i2c is None: + raise RuntimeError("I2C not initialized") self._i2c.writeto_mem(self._i2c_addr, reg, data) def _read_reg(self, reg: int, n: int = 1) -> bytes: + if self._i2c is None: + raise RuntimeError("I2C not initialized") return self._i2c.readfrom_mem(self._i2c_addr, reg, n) def _read_u8(self, reg: int) -> int: diff --git a/sensors/vl53l0x.py b/sensors/vl53l0x.py index 1da8f32..cf9578e 100644 --- a/sensors/vl53l0x.py +++ b/sensors/vl53l0x.py @@ -11,6 +11,7 @@ """ import time +from ..diagnostics import diagnostics_output from .sensor_base import SensorBase @@ -18,71 +19,223 @@ _WHO_AM_I_EXPECT = 0xEE # Key registers (abridged - sufficient for single-shot ranging) -_SYSRANGE_START = 0x00 -_RESULT_INTERRUPT_STATUS = 0x13 -_RESULT_RANGE_STATUS = 0x14 -_SYSTEM_SEQUENCE_CONFIG = 0x01 -_MSRC_CONFIG_CONTROL = 0x60 -_FINAL_RANGE_CONF_MIN_CNT = 0x45 -_GLOBAL_CONFIG_VCSEL_WIDTH = 0x70 -_SYSTEM_INTERRUPT_CLEAR = 0x0B -_GPIO_HV_MUX_ACTIVE_HIGH = 0x84 -_SYSTEM_INTERRUPT_CONFIG = 0x0A +_SYSRANGE_START = 0x00 +_SYSTEM_SEQUENCE_CONFIG = 0x01 +_SYSTEM_INTERRUPT_CONFIG = 0x0A +_SYSTEM_INTERRUPT_CLEAR = 0x0B +_RESULT_INTERRUPT_STATUS = 0x13 +_RESULT_RANGE_STATUS = 0x14 +_MSRC_CONFIG_CONTROL = 0x60 +_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT = 0x44 +_GPIO_HV_MUX_ACTIVE_HIGH = 0x84 +_GLOBAL_CONFIG_SPAD_ENABLES_REF_0 = 0xB0 +_GLOBAL_CONFIG_REF_EN_START_SELECT = 0xB6 +_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD = 0x4E +_DYNAMIC_SPAD_REF_EN_START_OFFSET = 0x4F +_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV = 0x89 + +_STOP_VARIABLE_REG = 0x91 +_SPAD_INFO_REG = 0x92 +_SPAD_POLL_REG = 0x83 +_INTERRUPT_READY_MASK = 0x07 _RANGE_TIMEOUT_MS = 100 # ms to wait for a measurement +_DEFAULT_TUNING_SETTINGS = ( + (0xFF, 0x01), (0x00, 0x00), + (0xFF, 0x00), (0x09, 0x00), (0x10, 0x00), (0x11, 0x00), + (0x24, 0x01), (0x25, 0xFF), (0x75, 0x00), + (0xFF, 0x01), (0x4E, 0x2C), (0x48, 0x00), (0x30, 0x20), + (0xFF, 0x00), (0x30, 0x09), (0x54, 0x00), (0x31, 0x04), + (0x32, 0x03), (0x40, 0x83), (0x46, 0x25), (0x60, 0x00), + (0x27, 0x00), (0x50, 0x06), (0x51, 0x00), (0x52, 0x96), + (0x56, 0x08), (0x57, 0x30), (0x61, 0x00), (0x62, 0x00), + (0x64, 0x00), (0x65, 0x00), (0x66, 0xA0), + (0xFF, 0x01), (0x22, 0x32), (0x47, 0x14), (0x49, 0xFF), + (0x4A, 0x00), + (0xFF, 0x00), (0x7A, 0x0A), (0x7B, 0x00), (0x78, 0x21), + (0xFF, 0x01), (0x23, 0x34), (0x42, 0x00), (0x44, 0xFF), + (0x45, 0x26), (0x46, 0x05), (0x40, 0x40), (0x0E, 0x06), + (0x20, 0x1A), (0x43, 0x40), + (0xFF, 0x00), (0x34, 0x03), (0x35, 0x44), + (0xFF, 0x01), (0x31, 0x04), (0x4B, 0x09), (0x4C, 0x05), + (0x4D, 0x04), + (0xFF, 0x00), (0x44, 0x00), (0x45, 0x20), (0x47, 0x08), + (0x48, 0x28), (0x67, 0x00), (0x70, 0x04), (0x71, 0x01), + (0x72, 0xFE), (0x76, 0x00), (0x77, 0x00), + (0xFF, 0x01), (0x0D, 0x01), + (0xFF, 0x00), (0x80, 0x01), (0x01, 0xF8), + (0xFF, 0x01), (0x8E, 0x01), (0x00, 0x01), + (0xFF, 0x00), (0x80, 0x00), +) class VL53L0X(SensorBase): + """VL53L0X Time-of-Flight distance sensor driver.""" I2C_ADDR = 0x29 NAME = "VL53L0X" READ_INTERVAL_MS = 100 TYPE = "Distance" - + + def __init__(self, i2c_addr: int | None = None): + super().__init__(i2c_addr=i2c_addr) + self._stop_variable = 0 + def _init(self) -> bool: - # Check WHO_AM_I who = self._read_u8(_WHO_AM_I_REG) if who != _WHO_AM_I_EXPECT: - print(f"S:VL53L0X unexpected ID 0x{who:02X} (expected 0x{_WHO_AM_I_EXPECT:02X})") + if self._logging: + print(f"S:VL53L0X unexpected ID 0x{who:02X} (expected 0x{_WHO_AM_I_EXPECT:02X})") return False - # Minimal init sequence to enable single-shot ranging. - # For a production driver you would replicate ST's full reference - # init (reading SPAD counts, calibration etc.). This abbreviated - # version is sufficient for functional testing of the sensor. + # The VL53L0X needs a substantial startup sequence before single-shot + # ranging becomes trustworthy; the earlier minimal init returned a + # fixed-looking distance even though the result register read was valid. + self._write_u8( + _VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV, + self._read_u8(_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV) | 0x01, + ) + self._write_u8(0x88, 0x00) + + self._open_stop_variable_window() + self._stop_variable = self._read_u8(_STOP_VARIABLE_REG) + self._close_stop_variable_window() + + self._write_u8( + _MSRC_CONFIG_CONTROL, + self._read_u8(_MSRC_CONFIG_CONTROL) | 0x12, + ) + self._set_signal_rate_limit(0.25) + self._write_u8(_SYSTEM_SEQUENCE_CONFIG, 0xFF) + + spad_info = self._get_spad_info() + if spad_info is None: + return False - # Set GPIO interrupt to active-high and configure for range complete - self._write_u8(_SYSTEM_INTERRUPT_CONFIG, 0x04) # new sample ready - gpio = self._read_u8(_GPIO_HV_MUX_ACTIVE_HIGH) - self._write_u8(_GPIO_HV_MUX_ACTIVE_HIGH, gpio & ~0x10) # active LOW + spad_count, spad_type_is_aperture = spad_info + ref_spad_map = bytearray(self._read_reg(_GLOBAL_CONFIG_SPAD_ENABLES_REF_0, 6)) + self._write_u8(0xFF, 0x01) + self._write_u8(_DYNAMIC_SPAD_REF_EN_START_OFFSET, 0x00) + self._write_u8(_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD, 0x2C) + self._write_u8(0xFF, 0x00) + self._write_u8(_GLOBAL_CONFIG_REF_EN_START_SELECT, 0xB4) + + first_spad_to_enable = 12 if spad_type_is_aperture else 0 + spads_enabled = 0 + for index in range(48): + if index < first_spad_to_enable or spads_enabled == spad_count: + ref_spad_map[index // 8] &= ~(1 << (index % 8)) + continue + if (ref_spad_map[index // 8] >> (index % 8)) & 0x01: + spads_enabled += 1 + self._write_reg(_GLOBAL_CONFIG_SPAD_ENABLES_REF_0, bytes(ref_spad_map)) + + for reg, value in _DEFAULT_TUNING_SETTINGS: + self._write_u8(reg, value) + + self._write_u8(_SYSTEM_INTERRUPT_CONFIG, 0x04) + self._write_u8( + _GPIO_HV_MUX_ACTIVE_HIGH, + self._read_u8(_GPIO_HV_MUX_ACTIVE_HIGH) & ~0x10, + ) self._write_u8(_SYSTEM_INTERRUPT_CLEAR, 0x01) - # Disable MSRC and TCC - self._write_u8(_MSRC_CONFIG_CONTROL, 0x12) - - # Set sequence steps + self._write_u8(_SYSTEM_SEQUENCE_CONFIG, 0xE8) + self._write_u8(_SYSTEM_SEQUENCE_CONFIG, 0x01) + if not self._perform_single_ref_calibration(0x40): + return False + self._write_u8(_SYSTEM_SEQUENCE_CONFIG, 0x02) + if not self._perform_single_ref_calibration(0x00): + return False self._write_u8(_SYSTEM_SEQUENCE_CONFIG, 0xE8) return True - def _measure(self) -> dict: - # Trigger single-shot measurement + def _measure(self, timeout: int = _RANGE_TIMEOUT_MS) -> dict: + diagnostics_output(1,0) + self._prepare_single_shot() self._write_u8(_SYSRANGE_START, 0x01) - # Wait for result (poll interrupt status) - deadline = time.ticks_add(time.ticks_ms(), _RANGE_TIMEOUT_MS) - while True: - status = self._read_u8(_RESULT_INTERRUPT_STATUS) - if (status & 0x07) != 0: - break + deadline = time.ticks_add(time.ticks_ms(), timeout) + while self._read_u8(_SYSRANGE_START) & 0x01: if time.ticks_diff(deadline, time.ticks_ms()) <= 0: return {"dist_mm": "timeout"} time.sleep_ms(1) - # Read range result (bytes 10-11 of RESULT_RANGE_STATUS block) - data = self._i2c.readfrom_mem(self.I2C_ADDR, _RESULT_RANGE_STATUS + 10, 2) - dist_mm = (data[0] << 8) | data[1] + if not self._wait_for_interrupt_ready(): + return {"dist_mm": "timeout"} + + # The range value lives 10 bytes into the RESULT_RANGE_STATUS block in + # ST's register map; this offset matches the reference driver. + dist_mm = self._read_u16_be(_RESULT_RANGE_STATUS + 10) + + if self._logging: + print(f"S:VL53L0X measured {dist_mm} mm") + + self._write_u8(_SYSTEM_INTERRUPT_CLEAR, 0x01) + diagnostics_output(1,1) + + return {"dist": f"{dist_mm}"} + + def _open_stop_variable_window(self): + self._write_u8(0x80, 0x01) + self._write_u8(0xFF, 0x01) + self._write_u8(0x00, 0x00) + + def _close_stop_variable_window(self): + self._write_u8(0x00, 0x01) + self._write_u8(0xFF, 0x00) + self._write_u8(0x80, 0x00) - # Clear interrupt + def _prepare_single_shot(self): + self._open_stop_variable_window() + self._write_u8(_STOP_VARIABLE_REG, self._stop_variable) + self._close_stop_variable_window() + + def _wait_for_interrupt_ready(self) -> bool: + deadline = time.ticks_add(time.ticks_ms(), _RANGE_TIMEOUT_MS) + while (self._read_u8(_RESULT_INTERRUPT_STATUS) & _INTERRUPT_READY_MASK) == 0: + if time.ticks_diff(deadline, time.ticks_ms()) <= 0: + return False + time.sleep_ms(1) + return True + + def _perform_single_ref_calibration(self, vhv_init_byte: int) -> bool: + self._write_u8(_SYSRANGE_START, 0x01 | vhv_init_byte) + if not self._wait_for_interrupt_ready(): + return False self._write_u8(_SYSTEM_INTERRUPT_CLEAR, 0x01) + self._write_u8(_SYSRANGE_START, 0x00) + return True + + def _set_signal_rate_limit(self, limit_mcps: float): + self._write_u16_be( + _FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT, + int(limit_mcps * (1 << 7)), + ) + + def _get_spad_info(self): + self._open_stop_variable_window() + self._write_u8(0xFF, 0x06) + self._write_u8(_SPAD_POLL_REG, self._read_u8(_SPAD_POLL_REG) | 0x04) + self._write_u8(0xFF, 0x07) + self._write_u8(0x81, 0x01) + self._write_u8(0x80, 0x01) + self._write_u8(0x94, 0x6B) + self._write_u8(_SPAD_POLL_REG, 0x00) + + deadline = time.ticks_add(time.ticks_ms(), _RANGE_TIMEOUT_MS) + while self._read_u8(_SPAD_POLL_REG) == 0x00: + if time.ticks_diff(deadline, time.ticks_ms()) <= 0: + return None + time.sleep_ms(1) + + self._write_u8(_SPAD_POLL_REG, 0x01) + spad_info = self._read_u8(_SPAD_INFO_REG) + + self._write_u8(0x81, 0x00) + self._write_u8(0xFF, 0x06) + self._write_u8(_SPAD_POLL_REG, self._read_u8(_SPAD_POLL_REG) & ~0x04) + self._write_u8(0xFF, 0x01) + self._close_stop_variable_window() - return {"dist_mm": f"{dist_mm}mm"} + return spad_info & 0x7F, ((spad_info >> 7) & 0x01) == 1 diff --git a/settings_mgr.py b/settings_mgr.py index fb9f441..47d18ef 100644 --- a/settings_mgr.py +++ b/settings_mgr.py @@ -13,6 +13,7 @@ from events.input import BUTTON_TYPES from app_components.tokens import label_font_size, button_labels from app_components.notification import Notification +from .app import SETTINGS_NAME_PREFIX MENU_ENTRY_NAME = "Settings" @@ -34,7 +35,7 @@ def _index(self): return k return None - def label(self, index: int = None): + def label(self, index: int | None = None): if index is not None: if self._labels is not None and index < len(self._labels): return self._labels[int(index)] @@ -101,13 +102,14 @@ def dec(self, v, l=0): def persist(self): """Persist the setting value to platform storage. If the value is equal to the default, the setting will be removed from storage to save space.""" + index = self._index() + if index is None: + return + key = f"{SETTINGS_NAME_PREFIX}.{index}" try: - if self.v != self.d: - platform_settings.set(f"badgebot.{self._index()}", self.v) - else: - platform_settings.set(f"badgebot.{self._index()}", None) + platform_settings.set(key, self.v if self.v != self.d else None) except Exception as e: # pylint: disable=broad-except - print(f"H:Failed to persist setting {self._index()}: {e}") + print(f"H:Failed to persist setting {key}: {e}") class SettingsMgr: @@ -122,7 +124,7 @@ class SettingsMgr: def __init__(self, app, logging: bool = False): self._app = app self._logging: bool = logging - self.edit_setting: int = None + self.edit_setting: str | None = None self.edit_setting_value = None if self._logging: print("SettingsMgr initialised") @@ -133,15 +135,16 @@ def __init__(self, app, logging: bool = False): def logging(self) -> bool: """Whether to print debug logs to the console.""" return self._logging - + @logging.setter def logging(self, value: bool): self._logging = value def start(self, item: str) -> bool: - """Enter Settings editing mode from the main menu.""" + """Enter Setting editing mode from the main menu.""" app = self._app + app._settings_menu_position = app.menu.position if app.menu else 0 app.set_menu(None) app.button_states.clear() app.refresh = True diff --git a/tests/conftest.py b/tests/conftest.py index 1cb440b..075c69f 100644 --- a/tests/conftest.py +++ b/tests/conftest.py @@ -91,19 +91,16 @@ class _FakeHexDriveApp: exposes the tiny surface that ``_update_state_check`` probes: * ``config.port`` – the port number - * ``get_version()`` – returns the current HEXDRIVE_APP_VERSION * ``get_status()`` – returns True (PWM ready) * ``set_motors()`` – no-op """ + VERSION = 6 # Match the real HexDriveApp.VERSION def __init__(self, port: int, version: int): _ensure_sim_initialized() from system.hexpansion.config import HexpansionConfig self.config = HexpansionConfig(port) - self._version = version - - def get_version(self) -> int: - return self._version + self.VERSION = version def get_status(self) -> bool: return True @@ -149,8 +146,6 @@ def install_fake_hexpansion(vid: int, pid: int, port: int, in the corresponding ``HexpansionType``. Defaults to ``HexDriveApp`` (name = ``"HexDriveApp"``). app_version : int, optional - Value returned by ``get_version()``. If *None* it is imported from - ``hexdrive.VERSION`` at call time. Yields ------ @@ -162,8 +157,7 @@ def install_fake_hexpansion(vid: int, pid: int, port: int, if app_class is None: app_class = HexDriveApp if app_version is None: - from sim.apps.BadgeBot.EEPROM.hexdrive import VERSION - app_version = VERSION + app_version = HexDriveApp.VERSION fake_app = app_class(port, app_version) diff --git a/tests/test_ina226.py b/tests/test_ina226.py deleted file mode 100644 index 6a09e50..0000000 --- a/tests/test_ina226.py +++ /dev/null @@ -1,72 +0,0 @@ -from pathlib import Path -import sys - -_APP_DIR = Path(__file__).resolve().parents[1] -if str(_APP_DIR) not in sys.path: - sys.path.insert(0, str(_APP_DIR)) - -from sensors.ina226 import ( - INA226, - _MASK_CVRF, - _REG_BUS_VOLTAGE, - _REG_CALIBRATION, - _REG_CONFIGURATION, - _REG_CURRENT, - _REG_MANUFACTURER_ID, - _REG_MASK_ENABLE, - _REG_POWER, - _DEFAULT_CONFIGURATION, -) - - -def _u16_be(value: int) -> bytes: - return bytes([(value >> 8) & 0xFF, value & 0xFF]) - - -class _FakeI2C: - def __init__(self): - self.reads = {} - self.writes = [] - - def readfrom_mem(self, addr, reg, n): - value = self.reads[(addr, reg)] - return value[:n] - - def writeto_mem(self, addr, reg, data): - self.writes.append((addr, reg, bytes(data))) - - -def test_ina226_supports_alternative_i2c_addresses(): - assert INA226.I2C_ADDR == 0x40 - assert INA226.I2C_ADDRS[0] == 0x40 - assert INA226.I2C_ADDRS[-1] == 0x4F - assert len(INA226.I2C_ADDRS) == 16 - - -def test_ina226_init_and_measure_integer_units(): - fake_i2c = _FakeI2C() - sensor = INA226(i2c_addr=0x45) - fake_i2c.reads[(0x45, _REG_MANUFACTURER_ID)] = _u16_be(0x5449) - assert sensor.begin(fake_i2c) is True - - assert (0x45, _REG_CONFIGURATION, _u16_be(_DEFAULT_CONFIGURATION)) in fake_i2c.writes - assert (0x45, _REG_CALIBRATION, _u16_be(0x0200)) in fake_i2c.writes - - fake_i2c.reads[(0x45, _REG_MASK_ENABLE)] = _u16_be(_MASK_CVRF) - fake_i2c.reads[(0x45, _REG_BUS_VOLTAGE)] = _u16_be(4000) - fake_i2c.reads[(0x45, _REG_CURRENT)] = _u16_be(1234) - fake_i2c.reads[(0x45, _REG_POWER)] = _u16_be(320) - - result = sensor.read() - assert result["bus_mV"] == "5000" - assert result["current_mA"] == "123" - assert result["power_mW"] == "800" - - -def test_ina226_read_sample_if_ready_none_when_not_ready(): - fake_i2c = _FakeI2C() - sensor = INA226(i2c_addr=0x40) - fake_i2c.reads[(0x40, _REG_MANUFACTURER_ID)] = _u16_be(0x5449) - fake_i2c.reads[(0x40, _REG_MASK_ENABLE)] = _u16_be(0x0000) - assert sensor.begin(fake_i2c) is True - assert sensor.read_sample_if_ready() is None diff --git a/tests/test_opt4048.py b/tests/test_opt4060.py similarity index 72% rename from tests/test_opt4048.py rename to tests/test_opt4060.py index e432c90..88892f2 100644 --- a/tests/test_opt4048.py +++ b/tests/test_opt4060.py @@ -1,4 +1,4 @@ -"""Tests for the OPT4048 tristimulus XYZ colour sensor driver. +"""Tests for the OPT4060 tristimulus XYZ colour sensor driver. These tests mock the I2C bus to validate register-level behaviour without real hardware. They run inside the badge simulator environment set up by @@ -72,26 +72,26 @@ def writeto_mem(self, addr, reg, data): # --------------------------------------------------------------------------- @pytest.fixture -def opt4048_module(): - """Import and return the opt4048 module (after sim init).""" +def OPT4060_module(): + """Import and return the OPT4060 module (after sim init).""" _ensure_sim() - import sim.apps.BadgeBot.sensors.opt4048 as mod + import sim.apps.BadgeBot.sensors.opt4060 as mod return mod @pytest.fixture -def sensor(opt4048_module): - """Return an OPT4048 sensor instance wired to a FakeI2C bus that +def sensor(OPT4060_module): + """Return an OPT4060 sensor instance wired to a FakeI2C bus that has the correct device-ID pre-loaded so begin() succeeds. """ i2c = FakeI2C() - mod = opt4048_module + mod = OPT4060_module # Pre-load device ID register with expected value - i2c.set_reg16(mod.OPT4048.I2C_ADDR, 0x11, 0x0821) + i2c.set_reg16(mod.OPT4060.I2C_ADDR, 0x11, 0x0821) # Pre-load status as conversion-ready so _measure() doesn't time out - i2c.set_reg16(mod.OPT4048.I2C_ADDR, 0x0C, 0x0004) # _FLAG_READY + i2c.set_reg16(mod.OPT4060.I2C_ADDR, 0x0C, 0x0004) # _FLAG_READY - s = mod.OPT4048() + s = mod.OPT4060() assert s.begin(i2c) is True return s @@ -105,24 +105,24 @@ def fake_i2c(): # Import & interface tests # --------------------------------------------------------------------------- -def test_import_opt4048(opt4048_module): +def test_import_OPT4060(OPT4060_module): """Module can be imported and has the expected class.""" - assert hasattr(opt4048_module, 'OPT4048') + assert hasattr(OPT4060_module, 'OPT4060') -def test_class_attributes(opt4048_module): +def test_class_attributes(OPT4060_module): """Verify class-level constants match the datasheet.""" - cls = opt4048_module.OPT4048 + cls = OPT4060_module.OPT4060 assert cls.I2C_ADDR == 0x44 - assert cls.NAME == "OPT4048" + assert cls.NAME == "OPT4060" # check that class constant is between 10 and 100ms, to catch any accidental typos assert 10 <= cls.READ_INTERVAL_MS <= 100 assert cls.TYPE == "Colour" -def test_sensor_base_interface(opt4048_module): - """OPT4048 implements the full SensorBase public API.""" - s = opt4048_module.OPT4048() +def test_sensor_base_interface(OPT4060_module): + """OPT4060 implements the full SensorBase public API.""" + s = OPT4060_module.OPT4060() for attr in ('begin', 'read', 'reset', 'is_ready'): assert hasattr(s, attr) assert s.is_ready is False @@ -132,17 +132,17 @@ def test_sensor_base_interface(opt4048_module): # Initialisation tests # --------------------------------------------------------------------------- -def test_begin_sets_continuous_mode(opt4048_module, fake_i2c): +def test_begin_sets_continuous_mode(OPT4060_module, fake_i2c): """After begin(), the config register should reflect continuous mode, auto-range, and 1.8 ms conversion time.""" - mod = opt4048_module - fake_i2c.set_reg16(mod.OPT4048.I2C_ADDR, 0x11, 0x0821) - fake_i2c.set_reg16(mod.OPT4048.I2C_ADDR, 0x0C, 0x0004) + mod = OPT4060_module + fake_i2c.set_reg16(mod.OPT4060.I2C_ADDR, 0x11, 0x0821) + fake_i2c.set_reg16(mod.OPT4060.I2C_ADDR, 0x0C, 0x0004) - s = mod.OPT4048() + s = mod.OPT4060() assert s.begin(fake_i2c) - cfg_bytes = fake_i2c.readfrom_mem(mod.OPT4048.I2C_ADDR, 0x0A, 2) + cfg_bytes = fake_i2c.readfrom_mem(mod.OPT4060.I2C_ADDR, 0x0A, 2) cfg = (cfg_bytes[0] << 8) | cfg_bytes[1] # Range = AUTO (12) in bits 13:10 @@ -153,13 +153,13 @@ def test_begin_sets_continuous_mode(opt4048_module, fake_i2c): assert (cfg >> 4) & 0x03 == mod.MODE_CONTINUOUS -def test_begin_wrong_id_fails(opt4048_module, fake_i2c): +def test_begin_wrong_id_fails(OPT4060_module, fake_i2c): """begin() should reject an unexpected device ID.""" - mod = opt4048_module - fake_i2c.set_reg16(mod.OPT4048.I2C_ADDR, 0x11, 0xFFFF) # wrong ID - fake_i2c.set_reg16(mod.OPT4048.I2C_ADDR, 0x0C, 0x0004) + mod = OPT4060_module + fake_i2c.set_reg16(mod.OPT4060.I2C_ADDR, 0x11, 0xFFFF) # wrong ID + fake_i2c.set_reg16(mod.OPT4060.I2C_ADDR, 0x0C, 0x0004) - s = mod.OPT4048() + s = mod.OPT4060() assert s.begin(fake_i2c) is False @@ -167,13 +167,13 @@ def test_begin_wrong_id_fails(opt4048_module, fake_i2c): # Channel decode tests # --------------------------------------------------------------------------- -def test_decode_channel_zero(opt4048_module): +def test_decode_channel_zero(OPT4060_module): """Zero mantissa and exponent should produce 0.""" - result = opt4048_module.OPT4048._decode_channel(bytes([0, 0, 0, 0]), 0) + result = OPT4060_module.OPT4060._decode_channel(bytes([0, 0, 0, 0]), 0) assert result == 0 -def test_decode_channel_known_value(opt4048_module): +def test_decode_channel_known_value(OPT4060_module): """Verify decoding against a manually-calculated example. MSB register: exponent=3 (0x3), mantissa_hi=0x123 → byte0=0x31, byte1=0x23 @@ -182,25 +182,25 @@ def test_decode_channel_known_value(opt4048_module): ADC code = 0x12345 << 3 = 0x91A28 = 596520 """ buf = bytes([0x31, 0x23, 0x45, 0x00]) - result = opt4048_module.OPT4048._decode_channel(buf, 0) + result = OPT4060_module.OPT4060._decode_channel(buf, 0) assert result == 0x12345 << 3 -def test_decode_channel_max_exponent(opt4048_module): +def test_decode_channel_max_exponent(OPT4060_module): """Maximum exponent (15) should shift mantissa left by 15.""" # exponent=15 (0xF), mantissa_hi=0x000, mantissa_lo=0x01 → mantissa=1 buf = bytes([0xF0, 0x00, 0x01, 0x00]) - result = opt4048_module.OPT4048._decode_channel(buf, 0) + result = OPT4060_module.OPT4060._decode_channel(buf, 0) assert result == 1 << 15 -def test_decode_channel_with_offset(opt4048_module): +def test_decode_channel_with_offset(OPT4060_module): """Decoding from a non-zero offset within the buffer should work.""" # 4 bytes padding + channel data padding = bytes([0xFF, 0xFF, 0xFF, 0xFF]) ch_data = bytes([0x10, 0x00, 0x80, 0x00]) # exp=1, mant_hi=0, mant_lo=0x80 buf = padding + ch_data - result = opt4048_module.OPT4048._decode_channel(buf, 4) + result = OPT4060_module.OPT4060._decode_channel(buf, 4) # mantissa = (0 << 16) | (0 << 8) | 0x80 = 128; code = 128 << 1 = 256 assert result == 128 << 1 @@ -209,11 +209,11 @@ def test_decode_channel_with_offset(opt4048_module): # Measurement tests # --------------------------------------------------------------------------- -def test_measure_returns_xyz(sensor, opt4048_module): +def test_measure_returns_xyz(sensor, OPT4060_module): """_measure() should return a dict with x, y, z string values.""" # Set up channel data: all channels have mantissa=100, exponent=0 i2c = sensor._i2c - addr = opt4048_module.OPT4048.I2C_ADDR + addr = OPT4060_module.OPT4060.I2C_ADDR for ch_reg in (0x00, 0x02, 0x04, 0x06): # MSB: exp=0, mantissa_hi=0x000 @@ -222,23 +222,25 @@ def test_measure_returns_xyz(sensor, opt4048_module): i2c.set_reg16(addr, ch_reg + 1, 0x6400) result = sensor.read() - assert 'x' in result - assert 'y' in result - assert 'z' in result + assert 'r' in result + assert 'g' in result + assert 'b' in result + assert 'w' in result # All channels should decode to mantissa 100 << 0 = 100 - assert result['x'] == '100' - assert result['y'] == '100' - assert result['z'] == '100' + assert result['r'] == '100' + assert result['g'] == '100' + assert result['b'] == '100' + assert result['w'] == '100' -def test_measure_timeout(opt4048_module, fake_i2c): +def test_measure_timeout(OPT4060_module, fake_i2c): """_measure() returns an error dict when status never shows ready.""" - mod = opt4048_module - fake_i2c.set_reg16(mod.OPT4048.I2C_ADDR, 0x11, 0x0821) + mod = OPT4060_module + fake_i2c.set_reg16(mod.OPT4060.I2C_ADDR, 0x11, 0x0821) # Status: NOT ready (no _FLAG_READY bit set) - fake_i2c.set_reg16(mod.OPT4048.I2C_ADDR, 0x0C, 0x0000) + fake_i2c.set_reg16(mod.OPT4060.I2C_ADDR, 0x0C, 0x0000) - s = mod.OPT4048() + s = mod.OPT4060() assert s.begin(fake_i2c) result = s.read() @@ -250,23 +252,23 @@ def test_measure_timeout(opt4048_module, fake_i2c): # Configuration API tests # --------------------------------------------------------------------------- -def test_set_get_range(sensor, opt4048_module): +def test_set_get_range(sensor, OPT4060_module): """set_range / get_range round-trip.""" - for rng in (opt4048_module.RANGE_2K, opt4048_module.RANGE_72K, opt4048_module.RANGE_AUTO): + for rng in (OPT4060_module.RANGE_2K, OPT4060_module.RANGE_72K, OPT4060_module.RANGE_AUTO): sensor.set_range(rng) assert sensor.get_range() == rng -def test_set_get_conversion_time(sensor, opt4048_module): +def test_set_get_conversion_time(sensor, OPT4060_module): """set_conversion_time / get_conversion_time round-trip.""" - for ct in (opt4048_module.CONV_600US, opt4048_module.CONV_1_8MS, opt4048_module.CONV_800MS): + for ct in (OPT4060_module.CONV_600US, OPT4060_module.CONV_1_8MS, OPT4060_module.CONV_800MS): sensor.set_conversion_time(ct) assert sensor.get_conversion_time() == ct -def test_set_get_mode(sensor, opt4048_module): +def test_set_get_mode(sensor, OPT4060_module): """set_mode / get_mode round-trip.""" - for mode in (opt4048_module.MODE_POWERDOWN, opt4048_module.MODE_CONTINUOUS): + for mode in (OPT4060_module.MODE_POWERDOWN, OPT4060_module.MODE_CONTINUOUS): sensor.set_mode(mode) assert sensor.get_mode() == mode @@ -283,10 +285,10 @@ def test_set_get_interrupt(sensor): # Shutdown test # --------------------------------------------------------------------------- -def test_shutdown_powers_down(sensor, opt4048_module): +def test_shutdown_powers_down(sensor, OPT4060_module): """reset() should set the sensor to power-down mode.""" sensor.reset() - assert sensor.get_mode() == opt4048_module.MODE_POWERDOWN + assert sensor.get_mode() == OPT4060_module.MODE_POWERDOWN assert sensor.is_ready is False @@ -294,23 +296,23 @@ def test_shutdown_powers_down(sensor, opt4048_module): # Module-level constant tests # --------------------------------------------------------------------------- -def test_range_constants(opt4048_module): +def test_range_constants(OPT4060_module): """Range constants should cover the datasheet values.""" - mod = opt4048_module + mod = OPT4060_module assert mod.RANGE_2K == 0 assert mod.RANGE_144K == 6 assert mod.RANGE_AUTO == 12 -def test_conv_time_constants(opt4048_module): +def test_conv_time_constants(OPT4060_module): """Conversion time constants should span 600 µs to 800 ms.""" - mod = opt4048_module + mod = OPT4060_module assert mod.CONV_600US == 0 assert mod.CONV_800MS == 11 -def test_mode_constants(opt4048_module): +def test_mode_constants(OPT4060_module): """Operating mode constants should match datasheet.""" - mod = opt4048_module + mod = OPT4060_module assert mod.MODE_POWERDOWN == 0 assert mod.MODE_CONTINUOUS == 3 diff --git a/tests/test_sensor_test.py b/tests/test_sensor_test.py new file mode 100644 index 0000000..bc46569 --- /dev/null +++ b/tests/test_sensor_test.py @@ -0,0 +1,41 @@ +"""Focused tests for SensorTestMgr display helpers and page rendering.""" + +# pylint: disable=protected-access + +import sys +from types import SimpleNamespace + +sys.path.append("../../../") + +import sim.run as _sim_run + + +def test_sensor_display_orders_rgb_first(): + from sim.apps.BadgeBot.sensor_test import SensorTestMgr + + ordered = SensorTestMgr._ordered_display_items({"w": 4, "b": 3, "extra": 5, "g": 2, "r": 1}) + assert ordered == [("r", "1"), ("g", "2"), ("b", "3"), ("w", "4"), ("extra", "5")] + + +def test_sensor_white_reference_normalises_rgb_channels(): + from sim.apps.BadgeBot.sensor_test import SensorTestMgr + + gains = SensorTestMgr._reference_to_gains(50, 100, 200, 400) + calibrated = SensorTestMgr._apply_white_reference(50, 100, 150, 200, gains) + assert calibrated == (1024, 1024, 768, 512) + + +def test_distance_sensor_raw_page_shows_latest_sample(): + from sim.apps.BadgeBot.sensor_test import SensorTestMgr, _PAGE_RAW + + mgr = object.__new__(SensorTestMgr) + mgr._display_data = {} + mgr._page_selected = _PAGE_RAW + mgr._page_count = 0 + mgr._sample_rate = 0 + mgr._sensor_data = {"dist_mm": "345"} + mgr._sensor_mgr = SimpleNamespace(type="Distance") + + mgr._update_display_values() + + assert mgr._display_data == {"dist_mm": "345"} diff --git a/tests/test_smoke.py b/tests/test_smoke.py index 7094fb0..268ed99 100644 --- a/tests/test_smoke.py +++ b/tests/test_smoke.py @@ -1,38 +1,66 @@ +import re import sys - -import pytest +from pathlib import Path # Add badge software to pythonpath -sys.path.append("../../../") +sys.path.append("../../../") -import sim.run +import sim.run as _sim_run from system.hexpansion.config import HexpansionConfig +def _extract_version_from_source(path: Path) -> int: + content = path.read_text(encoding="utf-8") + match = re.search(r"^\s*VERSION\s*=\s*(\d+)", content, re.MULTILINE) + assert match is not None, f"Could not find VERSION in {path}" + return int(match.group(1)) + + def test_import_badgebot_app_and_app_export(): import sim.apps.BadgeBot.app as BadgeBot from sim.apps.BadgeBot import BadgeBotApp assert BadgeBot.__app_export__ == BadgeBotApp def test_import_hexdrive_app_and_app_export(): - import sim.apps.BadgeBot.EEPROM.hexdrive as HexDrive - from sim.apps.BadgeBot.EEPROM.hexdrive import HexDriveApp + import sim.apps.BadgeBot.vendor.HexDrive.hexdrive as HexDrive + from sim.apps.BadgeBot.vendor.HexDrive.hexdrive import HexDriveApp assert HexDrive.__app_export__ == HexDriveApp +def test_hexdrive_instance_exposes_version(): + from sim.apps.BadgeBot.vendor.HexDrive.hexdrive import HexDriveApp + assert getattr(HexDriveApp(), "VERSION", None) == HexDriveApp.VERSION + def test_badgebot_app_init(): from sim.apps.BadgeBot import BadgeBotApp BadgeBotApp() def test_hexdrive_app_init(port): - from sim.apps.BadgeBot.EEPROM.hexdrive import HexDriveApp + from sim.apps.BadgeBot.vendor.HexDrive.hexdrive import HexDriveApp config = HexpansionConfig(port) HexDriveApp(config) def test_app_versions_match(): import sim.apps.BadgeBot.app as BadgeBot - import sim.apps.BadgeBot.EEPROM.hexdrive as HexDrive - assert BadgeBot.HEXDRIVE_APP_VERSION == HexDrive.VERSION - # above test should always pass since BadgeBot.HEXDRIVE_APP_VERSION is imported from HexDrive.VERSION, but this test will at least catch if someone accidentally changes one without the other. + from sim.apps.BadgeBot.vendor.HexDrive.hexdrive import HexDriveApp + assert BadgeBot.HEXDRIVE_APP_VERSION == HexDriveApp.VERSION + +def test_hexdrive2_metadata_matches_vendor_source(): + import sim.apps.BadgeBot.app as BadgeBot + from sim.apps.BadgeBot import BadgeBotApp + + source_version = _extract_version_from_source( + Path(__file__).resolve().parents[1] / "vendor" / "HexDrive2" / "hexdrive2.py" + ) + assert BadgeBot.HEXDRIVE2_APP_VERSION == source_version + + app_instance = BadgeBotApp() + hexdrive2_entries = [ + ht for ht in app_instance.HEXPANSION_TYPES if ht.name == "HexDrive2" + ] + assert hexdrive2_entries, "No HexDrive2 entries found in BadgeBot metadata" + for entry in hexdrive2_entries: + assert entry.app_mpy_name == "hexdrive2" + assert entry.app_mpy_version == BadgeBot.HEXDRIVE2_APP_VERSION def test_hexdrive_type_pids_consistent(): """Verify HexDriveType PIDs in hexdrive.py are consistent with HexpansionType PIDs in app.py. @@ -43,7 +71,7 @@ def test_hexdrive_type_pids_consistent(): the motor/servo capability counts must agree. """ from sim.apps.BadgeBot import BadgeBotApp - from sim.apps.BadgeBot.EEPROM.hexdrive import _HEXDRIVE_TYPES + from sim.apps.BadgeBot.vendor.HexDrive.hexdrive import _HEXDRIVE_TYPES app_instance = BadgeBotApp() hexdrive_hexpansion_types = [ @@ -77,14 +105,6 @@ def test_hexdrive_type_pids_consistent(): ) -def test_new_states_exist(): - """Verify the new STATE_SENSOR and STATE_AUTODRIVE constants are defined.""" - import sim.apps.BadgeBot.app as BadgeBot - assert hasattr(BadgeBot, 'STATE_SENSOR') - assert hasattr(BadgeBot, 'STATE_AUTODRIVE') - assert BadgeBot.STATE_SENSOR != BadgeBot.STATE_AUTODRIVE - - def test_new_settings_registered(): """Verify motor1_dir, motor2_dir, and front_face base settings are always registered.""" from sim.apps.BadgeBot import BadgeBotApp @@ -107,8 +127,9 @@ def test_autodrive_settings_need_hexpansion(): def test_front_face_labels_complete(): """Verify _FRONT_FACE_LABELS has one entry for each valid front_face value (0-11).""" import sim.apps.BadgeBot.app as BadgeBot - assert hasattr(BadgeBot, '_FRONT_FACE_LABELS') - assert len(BadgeBot._FRONT_FACE_LABELS) == 12 + front_face_labels = getattr(BadgeBot, '_FRONT_FACE_LABELS', None) + assert front_face_labels is not None + assert len(front_face_labels) == 12 def test_menu_items_include_sensor_and_auto(): @@ -132,9 +153,7 @@ def test_sensor_base_interface(): def test_all_sensor_classes_populated(): """Verify ALL_SENSOR_CLASSES contains the expected sensor drivers.""" from sim.apps.BadgeBot.sensors import ALL_SENSOR_CLASSES - assert len(ALL_SENSOR_CLASSES) >= 5 + assert len(ALL_SENSOR_CLASSES) >= 2 names = {cls.NAME for cls in ALL_SENSOR_CLASSES} assert 'VL53L0X' in names or 'VL6180X' in names # at least one ToF sensor - assert 'TCS3472' in names or 'TCS3430' in names # at least one color sensor - assert 'OPT4048' in names # OPT4048 tristimulus sensor assert 'OPT4060' in names # OPT4060 RGBW colour sensor diff --git a/tests/test_vl53l0x.py b/tests/test_vl53l0x.py new file mode 100644 index 0000000..2c3ae69 --- /dev/null +++ b/tests/test_vl53l0x.py @@ -0,0 +1,105 @@ +"""Register-level tests for the VL53L0X distance sensor driver.""" + +# pylint: disable=protected-access,redefined-outer-name,unused-import + +import sys + +import pytest + +sys.path.append("../../../") + +import sim.run + + +class FakeI2C: + def __init__(self): + self._mem = {} + self._queued_reads = {} + self.write_log = [] + + def set_reg8(self, addr, reg, value): + self._mem[(addr, reg)] = value & 0xFF + + def set_reg16(self, addr, reg, value): + self.set_reg8(addr, reg, value >> 8) + self.set_reg8(addr, reg + 1, value) + + def set_block(self, addr, reg, data): + for offset, value in enumerate(data): + self.set_reg8(addr, reg + offset, value) + + def queue_reads(self, addr, reg, values): + self._queued_reads.setdefault((addr, reg), []).extend(values) + + def readfrom_mem(self, addr, reg, nbytes): + result = bytearray() + for offset in range(nbytes): + key = (addr, reg + offset) + queued = self._queued_reads.get(key) + if queued: + result.append(queued.pop(0)) + else: + result.append(self._mem.get(key, 0x00)) + return bytes(result) + + def writeto_mem(self, addr, reg, data): + payload = bytes(data) + self.write_log.append((addr, reg, payload)) + for offset, value in enumerate(payload): + self._mem[(addr, reg + offset)] = value + + +@pytest.fixture +def vl53l0x_module(): + import sim.apps.BadgeBot.sensors.vl53l0x as mod + return mod + + +def _make_sensor_environment(mod): + i2c = FakeI2C() + addr = mod.VL53L0X.I2C_ADDR + + i2c.set_reg8(addr, mod._WHO_AM_I_REG, mod._WHO_AM_I_EXPECT) + i2c.set_reg8(addr, mod._VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV, 0x00) + i2c.set_reg8(addr, mod._MSRC_CONFIG_CONTROL, 0x00) + i2c.set_reg8(addr, mod._GPIO_HV_MUX_ACTIVE_HIGH, 0x10) + i2c.set_reg8(addr, mod._STOP_VARIABLE_REG, 0xAB) + i2c.set_reg8(addr, mod._SPAD_INFO_REG, 0x8F) + i2c.set_block(addr, mod._GLOBAL_CONFIG_SPAD_ENABLES_REF_0, [0xFF] * 6) + i2c.set_reg16(addr, mod._RESULT_RANGE_STATUS + 10, 345) + + i2c.queue_reads(addr, mod._SPAD_POLL_REG, [0x01, 0x01]) + i2c.queue_reads(addr, mod._RESULT_INTERRUPT_STATUS, [0x01, 0x01, 0x01]) + i2c.queue_reads(addr, mod._SYSRANGE_START, [0x00]) + + sensor = mod.VL53L0X() + return sensor, i2c + + +def test_begin_captures_stop_variable_and_finishes_calibration(vl53l0x_module): + sensor, i2c = _make_sensor_environment(vl53l0x_module) + + assert sensor.begin(i2c) is True + assert sensor._stop_variable == 0xAB + assert i2c.readfrom_mem(sensor.i2c_addr, vl53l0x_module._SYSTEM_SEQUENCE_CONFIG, 1) == bytes([0xE8]) + + +def test_read_restores_stop_variable_and_returns_range(vl53l0x_module): + sensor, i2c = _make_sensor_environment(vl53l0x_module) + + assert sensor.begin(i2c) is True + assert sensor.read() == {"dist": "345"} + assert any( + reg == vl53l0x_module._STOP_VARIABLE_REG and payload == b"\xAB" + for _, reg, payload in i2c.write_log + ) + + +def test_read_times_out_when_interrupt_never_asserts(monkeypatch, vl53l0x_module): + sensor, i2c = _make_sensor_environment(vl53l0x_module) + + assert sensor.begin(i2c) is True + i2c._queued_reads[(sensor.i2c_addr, vl53l0x_module._SYSRANGE_START)] = [0x00] + i2c._queued_reads[(sensor.i2c_addr, vl53l0x_module._RESULT_INTERRUPT_STATUS)] = [0x00] * 8 + monkeypatch.setattr(vl53l0x_module, "_RANGE_TIMEOUT_MS", 1) + assert sensor.read() == {"dist_mm": "timeout"} diff --git a/typings/system/hexpansion/events.pyi b/typings/system/hexpansion/events.pyi index 54635f9..84ce8ca 100644 --- a/typings/system/hexpansion/events.pyi +++ b/typings/system/hexpansion/events.pyi @@ -1,3 +1,6 @@ +# support Any +from typing import Any + class HexpansionInsertionEvent: port: int @@ -7,3 +10,14 @@ class HexpansionRemovalEvent: port: int def __init__(self, port: int) -> None: ... + +class HexpansionMountedEvent: + port: int + header: Any + + def __init__(self, port: int, header: Any) -> None: ... + +class HexpansionUnmountedEvent: + port: int + + def __init__(self, port: int) -> None: ... \ No newline at end of file diff --git a/typings/system/hexpansion/util.pyi b/typings/system/hexpansion/util.pyi index 2263968..9cd4972 100644 --- a/typings/system/hexpansion/util.pyi +++ b/typings/system/hexpansion/util.pyi @@ -2,3 +2,5 @@ from typing import Any def get_hexpansion_block_devices(_i2c: Any, _header: Any, _addr: int, *_args: Any, **_kwargs: Any) -> tuple[Any, Any]: ... def detect_eeprom_addr(_i2c: Any, *_args: Any, **_kwargs: Any) -> tuple[int | None, int | None]: ... +def get_app_by_slot(_slot: int) -> Any: ... +def get_slots_by_vid_pid(_vid: int, _pid: int) -> list[int]: ... \ No newline at end of file diff --git a/vendor/HexDrive b/vendor/HexDrive new file mode 160000 index 0000000..c4707c7 --- /dev/null +++ b/vendor/HexDrive @@ -0,0 +1 @@ +Subproject commit c4707c7b5a268018dca9a0bc5cbf0460f6a5dfeb diff --git a/vendor/HexDrive2 b/vendor/HexDrive2 new file mode 160000 index 0000000..4b40431 --- /dev/null +++ b/vendor/HexDrive2 @@ -0,0 +1 @@ +Subproject commit 4b40431d367e04df21790c1e3df8102192bb9a97