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..5ea844f --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "vendor/HexDrive2"] + path = vendor/HexDrive2 + url = https://github.com/TeamRobotmad/HexDrive2.git 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 index ee09d4b..7ea1dd6 100644 --- a/EEPROM/gps.py +++ b/EEPROM/gps.py @@ -12,9 +12,7 @@ # 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 @@ -22,12 +20,17 @@ RESET_PIN = 2 # HS_H for reset PPS_PIN = 3 # HS_I for PPS +GREEN_LED_PIN = 2 # LS_C for green LED control +RED_LED_PIN = 3 # LS_D for red LED control + ###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. """ + VERSION = 1 # Increment this when making changes to the app that require the hexpansion app to be re-flashed with the new code. + 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 @@ -68,11 +71,6 @@ def deinit(self): 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: diff --git a/EEPROM/hexdrive.py b/EEPROM/hexdrive.py index 7cb0a83..befa401 100644 --- a/EEPROM/hexdrive.py +++ b/EEPROM/hexdrive.py @@ -1,71 +1,107 @@ """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 compiled and copied onto the EEPROM app.mpy # It is then run from the EEPROM by the BadgeOS. +try: + from micropython import const +except ImportError: + # CPython / simulator fallback – const() is an identity function on MicroPython + const = lambda x: x # noqa: E731 import ota from machine import PWM, Pin from system.eventbus import eventbus from system.hexpansion.config import HexpansionConfig +try: + # only introduced in Badge v2.?.? + from system.hexpansion import app as hexpansion_app +except ImportError: + hexpansion_app = None from system.scheduler.events import RequestStopAppEvent - import app +from tildagon import Pin as ePin -# HexDrive.py App Version - used to check if upgrade is required -VERSION = 7 +# Define the minimum BadgeOS version required to run this app (e.g. if we need features that are only available in a certain version of BadgeOS) +_MIN_BADGEOS_VERSION = [1, 9, 0] # v1.9.0 is required to be able to read the EEPROM with 16-bit addressing # 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 +_ENABLE_PIN = const(0) # First LS pin used to enable the SMPSU # 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 +_DEFAULT_PWM_FREQ = const(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 = const(50) # 50Hz = 20mS period +_DEFAULT_KEEP_ALIVE_PERIOD = const(1000) # 1 second +_MAX_NUM_CHANNELS = const(4) # Max number of PWM channels supported by any type of HexDrive (Hexpansion limitation, not BadgeBot limit) +_MAX_NUM_MOTORS = const(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 +_MAX_SERVO_FREQ = const(200) # 200Hz = 5mS period (can work with some Servos but not all) +_SERVO_CENTRE = const(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 = const(1400) # 1400us either side of centre (VERY WIDE) +_SERVO_MAX_TRIM = const(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 +_EEPROM_ADDR = const(0x50) # I2C address of the EEPROM on the HexDrive and HexSense Hexpansion +_EEPROM_NUM_ADDRESS_BYTES = const(2) # Number of bytes used for the memory address when reading from the EEPROM (e.g. 2 for 16-bit addressing) +_VID_ADDR = const(0x10) # Address in the EEPROM where the Vendor ID (VID) byte is stored - used to identify the hardware version of the HexDrive +_PID_ADDR = const(0x12) # Address in the EEPROM where the Product ID (PID) byte is stored - used to identify the type of Hexpansion class HexDriveType: """Represents a sub-type of HexDrive Hexpansion module.""" - __slots__ = ("pid", "name", "motors", "servos") + __slots__ = ("pid", "name", "motors", "servos", "servo_pin_map") - 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 + def __init__(self, pid_byte: int, motors: int = 0, servos: int = 0, name: str = "Uncommitted", servo_pins: tuple[int, int, int, int] = (-1, -1, -1, -1)): + 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) + self.servo_pin_map: tuple[int, int, int, int] = servo_pins # Map the logical servo channels to the physical pin index according to hardware version _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"), + HexDriveType(0xCB, motors=2, servos=4, servo_pins=(3, 2, 1, 0)), # uncommitted version can be used for anything + HexDriveType(0xCC, servos=4, name="4 Servo", servo_pins=(3, 2, 1, 0)), + HexDriveType(0xCD, motors=1, servos=2, name="1 Mot 2 Srvo", servo_pins=(1, 0, -1, -1)), ) +_DEFAULT_HEXDRIVE_TYPE = _HEXDRIVE_TYPES[1] # default to the uncommitted version if we can't read the EEPROM for some reason + class HexDriveApp(app.App): # pylint: disable=no-member """ HexDrive Hexpansion App for BadgeBot.""" + VERSION = 7 # Increment this when making changes to the app that require the hexpansion app to be re-flashed with the new code. + def __init__(self, config: HexpansionConfig | None = None): super().__init__() - self.config: HexpansionConfig | None = config - self._hexdrive_type: HexDriveType | None = None + if config is None: + raise ValueError("HexDriveApp requires a HexpansionConfig on initialisation") + + self.config: HexpansionConfig = config self._logging: bool = True + + # What version of BadgeOS are we running on? + try: + ver = self._parse_version(ota.get_version()) + if ver >= _MIN_BADGEOS_VERSION: + pass + else: + raise RuntimeError("HexDriveApp requires BadgeOS Upgrade") + except Exception as e: # pylint: disable=broad-except + print(f"D:Ver check failed {e}!") + + # read hexpansion header (from EEPROM if necesssary) to find out which sub-type we are + _hexdrive_type = self._check_port_for_hexdrive(self.config.port) + if _hexdrive_type is None: + #print(f"D:{self.config.port}:Unknown HexDrive type - initialisation failed") + raise RuntimeError("Unknown HexDrive type") + + # report app starting and which port it is running on + print(f"D:HexDrive Type:'{_hexdrive_type.name}' App V{self.VERSION} by RobotMad on port {self.config.port}") + + self._hexdrive_type: HexDriveType = _hexdrive_type + self._servo_pin_map: tuple[int, int, int, int] = self._hexdrive_type.servo_pin_map self._keep_alive_period: int = _DEFAULT_KEEP_ALIVE_PERIOD self._power_state: bool = False self._pwm_setup: bool = False @@ -73,70 +109,57 @@ def __init__(self, config: HexpansionConfig | None = None): 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 + self._motor_output: list[int] = [0] * self._hexdrive_type.motors + # LS Pins - #self._power_detect = self.config.ls_pin[_DETECT_PIN] - self._power_control = self.config.ls_pin[_ENABLE_PIN] + self._power_control: ePin = self.config.ls_pin[_ENABLE_PIN] + + # Servo related + self._servo_pin_map: tuple[int, int, int, int] = self._hexdrive_type.servo_pin_map + self._servo_centre: list[int] = [_SERVO_CENTRE] * self._hexdrive_type.servos - 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() + + if not self.initialise(): + raise RuntimeError("HexDriveApp init failed") 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() - + # We delay the PWM initialisation until we actually need to set a servo position or motor speed + # because there are a limited number of PWM resources and we want to leave them available for + # other apps to use if the HexDrive is not actively being used. + + for channel in range(self._hexdrive_type.motors): + #print(f"D:{self.config.port}:Motor {channel} on Physical channels {channel<<1} & {(channel<<1) + 1}") + self._motor_output[channel] = 0 # initialise motor output state to 0 (stopped) + self._freq[channel<<1] = _DEFAULT_PWM_FREQ + self._freq[(channel<<1) + 1] = _DEFAULT_PWM_FREQ + for channel in range(self._hexdrive_type.servos): + physical_channel = self._servo_pin_map[channel] + if physical_channel >= 0 and self._freq[physical_channel] == 0: + # give priority to motor frequency if there is a conflict on the same physical channel, otherwise set to default servo frequency + #print(f"D:{self.config.port}:Servo {channel} on Physical channel {physical_channel}") + self._freq[physical_channel] = _DEFAULT_SERVO_FREQ + self._pwm_setup = True - 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 @@ -146,44 +169,37 @@ async def _handle_stop_app(self, event): if event.app == self: if self._logging: print(f"D:{self.config.port}:Stop") - self.deinitialise() + self._pwm_deinit() + # the badge HexpansionManagerApp tidies up the LS and HS pins when a hexpansion app is removed 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 not self._pwm_setup or not self._outputs_energised: # 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 + 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") + 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 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) + return self._pwm_setup def set_logging(self, state: bool): @@ -192,27 +208,19 @@ def set_logging(self, state: bool): 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 + """ Turn the SMPSU on or off. Returns success or failure. """ + if state == self._power_state: + return True # No change needed 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}") + #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 + return True def set_keep_alive(self, period: int): @@ -222,22 +230,51 @@ def set_keep_alive(self, period: int): self._keep_alive_period = period - def set_freq(self, freq: int, channel: int | None = None) -> bool: + def set_freq(self, freq: int, channel: int | None = None, servo: bool = False) -> 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: + if freq < 0 or freq > 100000: return False + if channel is not None: + _max_channel = self._hexdrive_type.servos if servo else self._hexdrive_type.motors + if channel < 0 or channel >= _max_channel: + return False + # map from logical channel to physical channel(s) for servos and motors + if servo: + self._freq[channel] = freq + physical_channel = self._servo_pin_map[channel] + else: + self._freq[channel << 1] = freq + self._freq[(channel << 1) + 1] = freq + physical_channel = 3- ((channel << 1) + (self._motor_output[channel] > 0)) # 3- to reverse pin order to match Hexpansion hardware + else: + if servo: + for ch in range(self._hexdrive_type.servos): + self._freq[ch] = freq + else: + for ch in range(self._hexdrive_type.motors): + self._freq[ch<<1] = freq + self._freq[(ch<<1)+1] = freq + physical_channel = None # All channels + + # Action new frequency immediately for any channels that are already setup 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 + if (physical_channel is None or (this_channel == physical_channel)) and pwm is not None: + if freq == 0: + # If frequency is set to 0 then we deinit the PWM to free up resources as much as possible + pwm.deinit() + self.PWMOutput[this_channel] = None + self.config.pin[this_channel].value(0) + #if self._logging: + # print(self._pwm_log_string(this_channel) + " disabled") + else: + 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}") + return False return True @@ -255,74 +292,83 @@ def set_servoposition(self, channel: int | None = None, position: int | None = N 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: + if pwm is not None and ch in self._servo_pin_map: 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") + pass + #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: + physical_channel = self._servo_pin_map[channel] + pwm = self.PWMOutput[physical_channel] + if pwm is None: + return False try: - self.PWMOutput[channel].duty_ns(0) - if self._logging: - print(self._pwm_log_string(channel) + "Off") + pwm.duty_ns(0) + #if self._logging: + # print(self._pwm_log_string(physical_channel) + "Off") except Exception as e: # pylint: disable=broad-except - print(self._pwm_log_string(channel) + f"Off failed {e}") + #print(self._pwm_log_string(physical_channel) + f"Off failed {e}") return False # check if all channels are now off and set outputs_energised accordingly - self._check_outputs_energised() + #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 + physical_channel = self._servo_pin_map[channel] pulse_width_in_ns = (self._servo_centre[channel] + position) * 1000 # convert from us to ns - if self.PWMOutput[channel] is None: + if self.PWMOutput[physical_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") + pin = self.config.pin[physical_channel] + self.PWMOutput[physical_channel] = PWM(pin, freq = self._freq[channel]) + self.PWMOutput[physical_channel].duty_ns(pulse_width_in_ns) + #if self._logging: + # print(self._pwm_log_string(physical_channel) + f"{self.PWMOutput[physical_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}") + print(self._pwm_log_string(physical_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 + pwm = self.PWMOutput[physical_channel] + if pwm is None: + return False try: - if _MAX_SERVO_FREQ < self.PWMOutput[channel].freq(): + if _MAX_SERVO_FREQ < pwm.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) + pwm.freq(_DEFAULT_SERVO_FREQ) if self._logging: - print(self._pwm_log_string(channel) + f"{_DEFAULT_SERVO_FREQ}Hz for Servo") + print(self._pwm_log_string(physical_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}") + #print(self._pwm_log_string(physical_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") + if 2000 < abs(pulse_width_in_ns - pwm.duty_ns()): # allow tolerance of 2us to avoid unnecessary updates + #if self._logging: + # print(self._pwm_log_string(physical_channel) + f"{pulse_width_in_ns}ns") + pwm.duty_ns(pulse_width_in_ns) + #if self._logging: + # print(self._pwm_log_string(physical_channel) + f"{pwm} duty") except Exception as e: # pylint: disable=broad-except - print(self._pwm_log_string(channel) + f"set duty failed {e}") + #print(self._pwm_log_string(physical_channel) + f"set duty failed {e}") return False self._outputs_energised = True @@ -335,14 +381,12 @@ def set_servocentre(self, centre: int, channel: int | None = None) -> bool: 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 + self._servo_centre = [centre] * self._hexdrive_type.servos else: self._servo_centre[channel] = centre return True @@ -354,7 +398,7 @@ def set_motors(self, outputs: tuple[int, ...]) -> bool: 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: + if len(outputs) > self._hexdrive_type.motors: return False for motor, output in enumerate(outputs): if abs(output) > 65535: @@ -365,34 +409,24 @@ def set_motors(self, outputs: tuple[int, ...]) -> bool: 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) + output_to_enable = 3- ((motor<<1) if output > 0 else ((motor<<1)+1)) + output_to_disable = 3- ((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: + pwm_to_disable = self.PWMOutput[output_to_disable] + if pwm_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() + pwm_to_disable.deinit() self.PWMOutput[output_to_disable] = None - self.config.pin[output_to_disable].value(0) + self.config.pin[output_to_disable].value(0) # pin mapping necessary to match the physical channel numbering on the HexDrive Hexpansion + #if self._logging: + # print(self._pwm_log_string(output_to_disable) + " disabled") 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}") + #print(f"D:{self.config.port}:Motor{motor}:{output} set failed {e}") + pass 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 + if output != 0: + self._outputs_energised = True self._time_since_last_update = 0 return True @@ -401,109 +435,83 @@ def set_pwm(self, duty_cycles: tuple[int, ...]) -> bool: # 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: + for _channel, _pwm in enumerate(self.PWMOutput): + if _pwm is not None: try: - pwm.deinit() + _pwm.deinit() except Exception: # pylint: disable=broad-except pass - self.PWMOutput[channel] = None - self._freq[channel] = 0 - self._motor_output[(channel>>1)] = 0 + self.PWMOutput[_channel] = None + for _channel in range(_MAX_NUM_CHANNELS): + self._freq[_channel] = 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 + # using simpler tracking - which may leave us thinking the outputs are still energised when they have in fact + # all been turned off one at a time, but all this means is you would then get a spuriour Timeout if the + # keep alive isn't being refreshed by commands. - # Set a single PWM duty cycle (0-65535) for a specific output + # Set a single PWM duty cycle (0-65535) for a specific MOTOR 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: + 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: + 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}") + pin = self.config.pin[_channel] + self.PWMOutput[_channel] = PWM(pin, freq = self._freq[_channel]) + self.PWMOutput[_channel].duty_u16(_duty_cycle) + #if self._logging: + # print(self._pwm_log_string(_channel) + f"{self.PWMOutput[_channel]} init") + pwm = self.PWMOutput[_channel] + if pwm is None: + return False + if _duty_cycle != pwm.duty_u16(): + pwm.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}") + 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 + + pid: int = 0 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 + if hexpansion_app is not None: + manager = hexpansion_app._hexpansion_manager + if manager is not None: + headers = manager.hexpansion_headers + if headers[port] is not None: + pid = headers[port].pid + if self._logging: + print(f"D:{port}:PID={pid:#04x}") + else: + if self._logging: + print(f"D:{port}:No hexpansion header found") + return None + except Exception as e: # pylint: disable=broad-except + print(f"D:{port}:use of hexpansion manager headers failed {e}") + #just read the part of the header which contains the PID + try: + pid = int.from_bytes(self.config.i2c.readfrom_mem(_EEPROM_ADDR, _PID_ADDR, 2, addrsize = 8*_EEPROM_NUM_ADDRESS_BYTES), "little") + except OSError as e: + # no EEPROM on this port + print(f"D:{port}:EEPROM error: {e}") + return _DEFAULT_HEXDRIVE_TYPE + # check which type of HexDrive this is by scanning the HEXDRIVE_TYPES list for _, hexpansion_type in enumerate(_HEXDRIVE_TYPES): - if pid_bytes[0] == hexpansion_type.pid: + # we only use the LSByte of the PID to identify the type of HexDrive, as the MSByte is used for other things + if pid & 0xFF == hexpansion_type.pid: return hexpansion_type - # we are not interested in this type of hexpansion - return None + # we are not interested in this type of hexpansion as it was not recognised + return _DEFAULT_HEXDRIVE_TYPE def _parse_version(self, version): diff --git a/EEPROM/hextest.py b/EEPROM/hextest.py new file mode 100644 index 0000000..34a4c10 --- /dev/null +++ b/EEPROM/hextest.py @@ -0,0 +1,2525 @@ +"""HexTest Hexpansion App for BadgeBot.""" + +# This is the app to be installed from the HexTest 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 asyncio +import time + +try: + from typing import TYPE_CHECKING +except ImportError: + TYPE_CHECKING = False + +import ota +import vfs +from machine import I2C, Pin, mem32, disable_irq, enable_irq + +import settings as platform_settings +from app_components import Menu, button_labels, clear_background, label_font_size +from app_components.notification import Notification +from events.input import BUTTON_TYPES, Buttons + +from system.eventbus import eventbus +from system.hexpansion import app as hexpansion_app +from system.hexpansion.config import HexpansionConfig +from system.hexpansion.events import HexpansionMountedEvent, HexpansionRemovalEvent +from system.scheduler.events import (RequestForegroundPopEvent, + RequestForegroundPushEvent, + RequestStopAppEvent) +from system.hexpansion.header import HexpansionHeader +from system.scheduler import scheduler +from system.hexpansion.util import ( + detect_eeprom_addr, + get_hexpansion_block_devices, +) +try: + from system.hexpansion.util import get_app_by_slot, get_slots_by_vid_pid +except ImportError: + def get_app_by_slot(slot): + """Find the app instance running from the hexpansion on the given port, if any. Returns the app instance if found, None otherwise.""" + for an_app in scheduler.apps: + if hasattr(an_app, "config"): + if hasattr(an_app.config, "port") and an_app.config.port == slot: + return an_app + return None + + def get_slots_by_vid_pid(vid, pid): + """Find all hexpansion ports with the given VID/PID. Returns a list of port numbers.""" + ports_with_hexpansion = [] + for port in range(1, 1+_SLOTS): + try: + i2c = I2C(port) # port is 0-indexed in code but 1-indexed in hardware + # Autodetect eeprom addr + eeprom_addr, addr_len = detect_eeprom_addr(i2c) + if eeprom_addr is None: + continue + # Do we have a header? + header_bytes = i2c.readfrom_mem(eeprom_addr, 0x00, 32, addrsize=8*addr_len) + hexpansion_header = HexpansionHeader.from_bytes(header_bytes) + except OSError: + # OSError just means there is no hexpansion EEPROM on this port + continue + except RuntimeError: + # RuntimeError means there is a blank EEPROM + continue + except Exception: # pylint: disable=broad-except + continue + if hexpansion_header is None: + continue + if hexpansion_header.vid == vid and hexpansion_header.pid == pid: + ports_with_hexpansion.append(port) + return ports_with_hexpansion + + + +import app +from tildagon import Pin as ePin +try: + from micropython import const +except ImportError: + # CPython / simulator fallback – const() is an identity function on MicroPython + const = lambda x: x # noqa: E731 + +if TYPE_CHECKING: + from typing import cast + from hexdrive_protocol import HexDriveLike + + def _as_hexdrive_app(value: object) -> HexDriveLike: + return cast(HexDriveLike, value) +else: + HexDriveLike = object + + def _as_hexdrive_app(value): + return value + + +# Define the minimum BadgeOS version required to run this app (e.g. if we need features that are only available in a certain version of BadgeOS) +_MIN_BADGEOS_VERSION = [1, 9, 0] # v1.9.0 is required to be able to read the EEPROM with 16-bit addressing + +_PRE = "hextest." # Prefix for settings keys in EEPROM + +# HexTest Hexpansion constants +# Hardware defintions: +_SLOTS = const(6) + +# Constants for rotation rate measurement and motor test mode. +_ROTATION_RATE_MEASUREMENT_PERIOD_MS = const(3000) # 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 = const(10) # default duty cycle for the IR emitter when doing rate testing, 0-255 (0=off, 255=full on) +_DEFAULT_SPOKES_PER_ROTATION = const(10) # number of times the photodiode will be triggered per full rotation of the wheel +_ROTATION_RATE_EMITTER_PINS = [const(1), const(2)] # LS_B & LS_C pins used to drive the IR emitter for rotation rate testing +_ROTATION_RATE_SENSOR_PINS = [const(0), const(1)] # HS_F & HS_G pins used to read the phottransistors for rotation rate testing +_ROTATION_RATE_SENSOR_ENABLE_PINS = [const(3), const(4)] # LS_D & LS_E 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 = const(2) # Step size for adjusting IR emitter brightness in manual mode, 0-255 (0=off, 255=full on) +_MAX_POWER = const(65535) # maximum power level to apply to motors, corresponds to 100% on the HexDrive +_MOTOR_PWM_FREQUENCY = const(20000) # Default PWM frequency to set on the HexDrive for testing, in Hz. + +# Rotation Rate Auto scan configuration +_AUTO_SCAN_STEPS = const(50) # Number of power levels to test during auto scan +_AUTO_SCAN_SETTLE_MS = const(800) # ms to wait after setting power before starting actual measurement period +_AUTO_SCAN_MEASURE_MS = const(10000) # ms measurement window per step (maximum) +_AUTO_SCAN_MIN_POWER = const(15535) # Minimum power level to test during auto scan (0-65535) +_FILE = "motor" +_EXT = "csv" +_FILE_DEST_LABELS = ("Badge FS", "Hex FS") + +# App states +STATE_MENU = const(0) +STATE_MESSAGE = const(1) # Message display +STATE_SETTINGS = const(2) # Edit Settings +STATE_SENSOR = const(3) # Sensor Test +STATE_MOTOR_TEST = const(4) # Motor Test + +# App states where user can minimise app (Menu, Message, Logo) +MINIMISE_VALID_STATES = [STATE_MENU, STATE_MESSAGE] + +# Main Menu Items +MAIN_MENU_ITEMS = ["Sensor Test", "Motor Test", "Settings", "About","Exit"] +MENU_ITEM_SENSOR_TEST = const(0) +MENU_ITEM_MOTOR_TEST = const(1) +MENU_ITEM_SETTINGS = const(2) +MENU_ITEM_ABOUT = const(3) +MENU_ITEM_EXIT = const(4) + +DEFAULT_BACKGROUND_UPDATE_PERIOD = const(100) # mS when not moving +_LOGGING = True +_AUTO_REPEAT_MS = const(200) # Time between auto-repeats, in ms +_AUTO_REPEAT_COUNT_THRES = const(10) # Number of auto-repeats before increasing level +_AUTO_REPEAT_SPEED_LEVEL_MAX = const(4) # Maximum level of auto-repeat speed increases +_AUTO_REPEAT_LEVEL_MAX = const(3) # Maximum level of auto-repeat digit increases + + +# Pages of information to show for each sensor (can be switched with up/down buttons) +_PAGE_RAW = const(0) +_PAGE_STATS = const(1) +_PAGE_DATA = const(2) +_PAGE_CAL = const(3) +_PAGE_NAMES = { + _PAGE_RAW: "Raw", + _PAGE_STATS: "Stats", + _PAGE_DATA: "Data", + _PAGE_CAL: "Cal", +} + +# Badge hexpansion HS pin to ESP32-S3 GPIO number mapping +# HexpansionConfig(port).pin[i] is Pin(gpio) where gpio = _HS_PIN_TO_GPIO[port][i] +_HS_PIN_TO_GPIO = { + 1: (39, 40, 41, 42), + 2: (35, 36, 37, 38), + 3: (34, 33, 47, 48), + 4: (11, 14, 13, 12), + 5: (18, 16, 15, 17), + 6: ( 3, 4, 5, 6), +} + + +class HexTestApp(app.App): # pylint: disable=no-member + """ HexTest Hexpansion App for BadgeBot.""" + VERSION = 1 # Increment this when making changes to the app that require the hexpansion app to be re-flashed with the new code. + + def __init__(self, config: HexpansionConfig | None = None): + super().__init__() + if config is None: + raise ValueError("HexTestApp requires a HexpansionConfig on initialisation") + + # 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 >= _MIN_BADGEOS_VERSION: + pass + else: + raise RuntimeError("HexTestApp requires BadgeOS Upgrade") + except Exception as e: # pylint: disable=broad-except + print(f"HT:Ver check failed {e}!") + + self.config: HexpansionConfig = config + self._logging: bool = True + self._foreground = False + + # UI Button Controls + self.button_states = Buttons(self) + # Overall app state (controls what is displayed and what user inputs are accepted) + self.current_state = STATE_MENU + self.previous_state = self.current_state + self.update_period: int = DEFAULT_BACKGROUND_UPDATE_PERIOD # mS + + # UI Feature Controls + self.refresh: bool = True # True so that we draw initial screen on first loop, then set to True whenever we want to trigger a screen update + self.notification: Notification | None = None + 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 + + # Settings + self.settings: dict = {} + self.edit_setting = None + self.edit_setting_value = None + self._auto_repeat_intervals = [ _AUTO_REPEAT_MS, _AUTO_REPEAT_MS//2, _AUTO_REPEAT_MS//4, _AUTO_REPEAT_MS//8, _AUTO_REPEAT_MS//16] # at the top end the loop is unlikley to cycle this fast + self._auto_repeat: int = 0 + self._auto_repeat_count: int = 0 + self.auto_repeat_level: int = 0 + + self._hexdrive_ports: list[int] = [] + self._hexdrive_in_use_port: int | None = None + self._hexdrive_app: HexDriveLike | None = None + + 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: list[Counter] = [] # hardware counters used to count photodiode pulses for rate testing + self._rotation_rate_rpms: list[int] = [] # computed RPM values derived from counter deltas + self._rotation_rate_measurement_period: int = _ROTATION_RATE_MEASUREMENT_PERIOD_MS + 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 + + # Auto scan test + self._scan_mode: bool = False # True = auto scanning, False = manual + self._unsaved_data = False + self._scan_direction: int = 1 # 1 = forwards, -1 = reverse + self._scan_step: int = 0 # current step index (0.._AUTO_SCAN_STEPS-1) + self._capture_settling: bool = True # True = in settle phase, False = in measure phase + self._rotation_detected: bool = False # True once motion has been observed during auto scan + self._capture_data: list[tuple[int, list[int], int | None]] = [] # list of (power, rpm list, current mA) + self._max_rpm: int = 0 # max rpm seen during scan + self._max_current_ma: int = 0 # max current seen during scan + self._last_current_ma: int = 0 # latest current sampled in auto mode + self._scan_done: bool = False # True = scan complete + self._motor_calibration_fit: list[tuple[float, float] | None] = [] # list of (slope, intercept) fits, indexed by motor number + self._hut_id: int = 0 # ID of the hexpansion under test (HUT) to include in the auto scan results file name + self._hut_id_seeded_ports = set() # ports already seen this mount cycle, so we only seed from UID once per detection + + 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_sample_count: int = 0 + + self._sensor_data: dict = {} + self._display_data: dict = {} + self._page_selected: int = _PAGE_RAW + self._page_count: int = 3 + self._read_timer: int = 0 # ms since last sensor read + self._sample_count: int = 0 + self._count_timer: int = 0 # ms + self._sample_rate: int = 0 # Hz + self._new_sample: bool = False + + if MySetting is not None: + # General settings + self.settings['logging'] = MySetting(self.settings, _LOGGING, False, True) + self.settings['path'] = MySetting(self.settings, 0, 0, len(_FILE_DEST_LABELS) - 1, labels=_FILE_DEST_LABELS) + self.settings['serialise'] = MySetting(self.settings, False, False, True) + self.settings['ir_pwm'] = MySetting(self.settings, _DEFAULT_ROTATION_RATE_EMITTER_DUTY, 0, 255) + self.settings['spokes'] = MySetting(self.settings, _DEFAULT_SPOKES_PER_ROTATION, 1, 20) + + self.update_settings() + + self.HEXPANSION_TYPES = [HexpansionType(0xCBCB, "HexDrive", motors=2, servos=4, sub_type="Uncommitted" ), + HexpansionType(0xCBCA, "HexDrive", motors=2, sub_type="2 Motor" ), + HexpansionType(0xCBCC, "HexDrive", servos=4, sub_type="4 Servo" ), + HexpansionType(0xCBCD, "HexDrive", motors=1, servos=2, sub_type="1 Mot 2 Srvo" ), + HexpansionType(0x10C8, "HexDrive2", vid=0xCBCB, motors=2, servos=2, sub_type="Uncommitted" ), + HexpansionType(0x10C9, "HexDrive2", vid=0xCBCB, servos=2, sub_type="2 Servo" ), + HexpansionType(0x10CA, "HexDrive2", vid=0xCBCB, motors=2, sub_type="2 Motor" ), + HexpansionType(0x11CE, "HexDrive2", vid=0xCBCB, motors=1, sub_type="Left Motor" ), + HexpansionType(0x12CE, "HexDrive2", vid=0xCBCB, motors=1, sub_type="Right Motor" ), + HexpansionType(0x10CF, "HexDrive2", vid=0xCBCB, motors=1, servos=1, sub_type="1 Mot 1 Srvo" )] + + # report app starting and which port it is running on + print(f"HT:HexTest App V{self.VERSION} by RobotMad on port {self.config.port}") + + self._rotation_rate_enable(False) # start with rotation rate emitter and sensors off until we enter motor test mode + + # Event handlers for gaining and losing focus + eventbus.on_async(RequestForegroundPushEvent, self._gain_focus, self) + eventbus.on_async(RequestForegroundPopEvent, self._lose_focus, self) + eventbus.on_async(RequestStopAppEvent, self._handle_stop_app, self) + eventbus.on_async(HexpansionMountedEvent, self._handle_mounted, self) + eventbus.on_async(HexpansionRemovalEvent, self._handle_removal, self) + + # Start with a Message state to show the app name and version + self.show_message(["HexTest", f"V{self.VERSION}", "By RobotMad"], [(0.2,1,0.2), (1,1,0), (1,1,1)], return_state=STATE_MENU) + + + + # ------------------------------------------------------------------ + + @property + def logging(self): + """Convenience property to access logging setting.""" + if 'logging' in self.settings: + return self.settings['logging'].v + return True + + # ------------------------------------------------------------------ + + @property + def rotation_rate_rounding(self) -> int: + return (self._rotation_rate_measurement_period * self.rotation_rate_spokes) // 2 + + @property + def rotation_rate_emitter_duty(self) -> int: + """Duty cycle (0-255) for the IR emitter when doing rotation rate testing.""" + if 'ir_pwm' in self.settings: + return self.settings['ir_pwm'].v + return _DEFAULT_ROTATION_RATE_EMITTER_DUTY + + @rotation_rate_emitter_duty.setter + def rotation_rate_emitter_duty(self, value: int): + self.settings['ir_pwm'].v = value + if self.config is not None: + for pin_num in _ROTATION_RATE_EMITTER_PINS: + self.config.ls_pin[pin_num].duty(value) + + @property + def rotation_rate_spokes(self) -> int: + """Number of times the photodiode will be triggered per full rotation of the wheel.""" + if 'spokes' in self.settings: + return self.settings['spokes'].v + return _DEFAULT_SPOKES_PER_ROTATION + + # ------------------------------------------------------------------ + + def update_settings(self): + """Update settings from EEPROM.""" + if self.logging: + print("HT:Updating settings from EEPROM") + for s in self.settings: + self.settings[s].v = platform_settings.get(f"{_PRE}{s}", self.settings[s].d) + if self.logging: + print(f"Setting {s} = {self.settings[s].v}") + + + def _rotation_rate_enable(self, enable: bool = True) -> bool: + if self.config is None: + return False + try: + if enable: + if self.logging: + print("HT:Enabling rotation rate emitters and sensors") + for pin_num in _ROTATION_RATE_EMITTER_PINS: + self.config.ls_pin[pin_num].init(mode=ePin.PWM) # Set LS pins to output mode to turn on the IR emitters + self.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.config.ls_pin[pin_num].init(mode=Pin.OUT) # Set LS pins to output mode to enable the phototransistors for rotation rate measurement + self.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("HT:Disabling rotation rate emitters and sensors") + for pin_num in _ROTATION_RATE_EMITTER_PINS: + self.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.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.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 deinitialise(self) -> bool: + """ De-initialise the app - return True if successful, False if failed.""" + eventbus.remove(HexpansionMountedEvent, self._handle_mounted, self) + eventbus.remove(HexpansionRemovalEvent, self._handle_removal, self) + eventbus.remove(RequestForegroundPushEvent, self._gain_focus, self) + eventbus.remove(RequestForegroundPopEvent, self._lose_focus, self) + # deinit any allocated Counters + for counter in self._rotation_rate_counters: + counter.deinit() + for hs_pin in self.config.pin: + hs_pin.init(mode=Pin.IN) + return True + + + def _exit_app(self): + """ Clean up and exit the app, returning to the main menu.""" + eventbus.emit(RequestStopAppEvent(self)) + + + # ------------------------------------------------------------------ + # Async event handlers (registered directly on eventbus) + # ------------------------------------------------------------------ + + async def _handle_removal(self, event: HexpansionRemovalEvent): + self._hut_id_seeded_ports.discard(event.port) + if event.port == self._hexdrive_in_use_port: + if self.logging: + print(f"H:Hexpansion removed from port {event.port}") + self._hexdrive_app = None + self._hexdrive_in_use_port = None + self._hexdrive_ports.remove(event.port) + self.notification = Notification("HexDrive Removed") + if self.current_state == STATE_MOTOR_TEST: + self._stop_motor_test_mode() + elif self.current_state == STATE_SENSOR: + self._stop_sensor_test_mode() + + + async def _handle_mounted(self, event: HexpansionMountedEvent): + if self._foreground and self.current_state in [STATE_MESSAGE, STATE_MENU]: + if self.logging: + print(f"H:Hexpansion mounted on port {event.port}") + # Check if it is a HexDrive we can use for testing + # make a simple list of vid, pid pairs that we can check against efficiently + vid_pid_pairs = [(type.vid, type.pid) for type in self.HEXPANSION_TYPES] + header = getattr(event, "header", None) + if header is not None and (header.vid, header.pid) in vid_pid_pairs: + self._seed_hut_id_from_detected_hexdrive(event.port, header) + if self.logging: + print(f"H:Attempting to use newly mounted HexDrive on port {event.port}") + + if self._motor_test_start(): + if self.logging: + print(f"H:Successfully started motor test with newly mounted HexDrive on port {event.port}") + self.current_state = STATE_MOTOR_TEST + self.set_menu(None) + else: + if self.logging: + print(f"H:Failed to start motor test with newly mounted HexDrive on port {event.port}") + else: + # Old BadgeOS didn't include the header in the event - so assume it is a HexDrive + if self._motor_test_start(): + if self.logging: + print(f"H:Successfully started motor test with newly mounted HexDrive on port {event.port} (no header in event, assumed HexDrive)") + self.current_state = STATE_MOTOR_TEST + self.set_menu(None) + + + async def _gain_focus(self, event: RequestForegroundPushEvent): + if event.app is self: + if self.logging: + print(f"HexTest gained focus in state {self.current_state}") + self._foreground = True + + + async def _lose_focus(self, event: RequestForegroundPopEvent): + if event.app is self: + if self.logging: + print(f"HexTest lost focus from state {self.current_state}") + self._foreground = False + + + 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 + + + async def background_task(self): + """Background task loop for handling time-based updates. This runs independently of the main update/draw loop + and is suitable for tasks that need to run at a consistent interval regardless of the current state or drawing performance.""" + last_time: int = time.ticks_ms() + + while True: + cur_time: int = time.ticks_ms() + delta_ticks: int = time.ticks_diff(cur_time, last_time) + self._background_update(delta_ticks) + 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 + + + ### NON-ASYNC FUNCTIONS ### + + def _background_update(self, delta: int): + """Perform background updates based on the current sub-state.""" + self._sample_ina226_in_background(delta) + # push motor outputs to HexDrive if we are in motor test mode + if self.current_state == STATE_MOTOR_TEST: + if self._hexdrive_app is not None: + try: + if not self._hexdrive_app.set_motors((self._rotation_rate_motor_power, self._rotation_rate_motor_power)): + if self.logging: + print("Failed to set motor outputs to HexDrive app") + except AttributeError: + pass + + + def set_logging(self, state: bool): + """ Set the logging state - True to enable logging, False to disable logging. """ + self._logging = state + + # multi level auto repeat + def auto_repeat_check(self, delta: int, speed_up: bool = True) -> bool: + """Check if the auto-repeat threshold has been reached for a button hold, and update the auto-repeat level accordingly. + If speed_up is True, the auto-repeat interval decreases as the level increases, allowing for faster repeats the + longer the button is held. If speed_up is False, the interval remains constant, but the level can still increase + to allow for larger increments/decrements in settings adjustments. + Returns True if the auto-repeat action should be triggered, False otherwise. + """ + self._auto_repeat += delta + # multi stage auto repeat - the repeat gets faster the longer the button is held + if self._auto_repeat > self._auto_repeat_intervals[self.auto_repeat_level if speed_up else 0]: + self._auto_repeat = 0 + self._auto_repeat_count += 1 + # variable threshold to count to increase level so that it is not too easy to get to the highest level as the auto repeat period is reduced + if self._auto_repeat_count > ((_AUTO_REPEAT_COUNT_THRES*_AUTO_REPEAT_MS) // self._auto_repeat_intervals[self.auto_repeat_level if speed_up else 0]): + self._auto_repeat_count = 0 + if self.auto_repeat_level < (_AUTO_REPEAT_SPEED_LEVEL_MAX if speed_up else _AUTO_REPEAT_LEVEL_MAX): + self.auto_repeat_level += 1 + if self.logging: + print(f"Auto Repeat Level: {self.auto_repeat_level}") + + return True + return False + + + def auto_repeat_clear(self): + """Reset the auto-repeat counters and level. This should be called when a button is released to ensure that the next button press starts with the initial auto-repeat interval and level.""" + self._auto_repeat = 1+ self._auto_repeat_intervals[0] # so that we trigger immediately on next press + + self._auto_repeat_count = 0 + self.auto_repeat_level = 0 + + + # ------------------------------------------------------------------ + # MOTOR TEST Entry point from menu + # ------------------------------------------------------------------ + + def _motor_test_start(self) -> bool: + """Enter the Sensor Test flow from the main menu.""" + self._sensor_data = {} + self._display_data = {} + self.refresh = True + hexpansion_type: HexpansionType | None = None + # Find HexDrive to test + # look for any type of hexdrive (including HexDrive2 variants) in any port by their VID/PID + for hexpansion_type in self.HEXPANSION_TYPES: + if self.logging: + print(f"HT:Looking for {hexpansion_type.name} (VID:PID {hexpansion_type.vid:04X}:{hexpansion_type.pid:04X}, Motors: {hexpansion_type.motors}, Servos: {hexpansion_type.servos})") + ports = get_slots_by_vid_pid(hexpansion_type.vid, hexpansion_type.pid) + if ports: + if self.logging: + print(f"HT:Found {hexpansion_type.name} on port(s): {ports}") + self._hexdrive_ports.extend(ports) + break + + for port in self._hexdrive_ports: + hexpansion_app = _as_hexdrive_app(get_app_by_slot(port)) + if hexpansion_app is not None: + self._hexdrive_in_use_port = port + if self.logging: + print(f"HT:Found HexDrive app to test on port {port}") + self._hexdrive_app = hexpansion_app + break + + if self._hexdrive_app is not None: + # First detect for this mounted hexpansion: seed HUT ID from non-zero UID once. + self._seed_hut_id_from_detected_hexdrive(self._hexdrive_in_use_port) + + #Setup UUT = HexDrive + try: + if self._hexdrive_app.initialise() and self._hexdrive_app.set_power(True) and self._hexdrive_app.set_freq(_MOTOR_PWM_FREQUENCY): + self._hexdrive_app.set_keep_alive(2000) # Updates can be quite slow as we are using the draw function + except AttributeError: + pass + + # Setup INA226: + if self._init_ina226_for_motor_test(): + if self._ina226_sensor_mgr is not None: + self.update_period = self._ina226_sensor_mgr.read_interval # update at the sensor read interval + + # 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 pin(s) + # Counter not yet available in this Micropython port so we have created our own... + gpio_num = _HS_PIN_TO_GPIO[self.config.port][pin_num] + # Is there a motor to test that we can measure the rotation rate of with the phototransistor sensors? + # If so, set up a counter to measure the rotation rate based on the phototransistor pulses. + # Assume first rotation rate sensor pin is for motor 1 and second (if present) is for motor 2. + # could be extended to cope with HexDrive variants for Left and Right motors... + if hexpansion_type is not None and hexpansion_type.motors < (pin_num + 1): + if self.logging: + print(f"HT:Not setting up rotation rate counter on pin {pin_num} (GPIO {gpio_num}) as this HexDrive type only has {hexpansion_type.motors} motors") + continue + counter = Counter(None, gpio_num, filter_ns=1000000, logging=False) # 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"HT:Failed to allocate PCNT counter for pin {pin_num} (GPIO {gpio_num})") + self.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"HT:Rate counter {counter}") + self._rotation_rate_measurement_period_elapsed = 0 + self._rotation_rate_rpms = [0] * len(self._rotation_rate_counters) + return True + if self.logging: + print("HT:Failed to initialise for motor test mode - no hexdrive to test") + self.notification = Notification("HexDrive not Found") + return False + + 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 _seed_hut_id_from_detected_hexdrive(self, port: int | None, header: HexpansionHeader | None = None) -> None: + if port is None or port in self._hut_id_seeded_ports: + return + + if header is None: + header = self._get_header_for_port(port) + + if header is not None: + try: + unique_id = int(header.unique_id) + except (TypeError, ValueError): + unique_id = 0 + + if unique_id != 0: + self._hut_id = unique_id + if self._logging: + print(f"HT:Initialised HUT ID from UID {unique_id} on port {port}") + + # Mark this port as handled for the current mount cycle so user edits are preserved. + self._hut_id_seeded_ports.add(port) + + + + def _stop_motor_test_mode(self): + if self._logging: + print("HT:Stopping Motor Test mode and cleaning up") + self._scan_mode = False + self._scan_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 + print("HT:INA226 sensor manager close failed:", exc) + self._ina226_sensor_mgr = None + self._ina226 = None + + if self._hexdrive_app is not None: + try: + self._hexdrive_app.set_motors((0, 0)) + self._hexdrive_app.set_power(False) + except AttributeError as e: + print(f"HT:Failed to set motor outputs off {e}") + self._hexdrive_in_use_port = None + + for c in self._rotation_rate_counters: + if c is not None: + c.deinit() + self._rotation_rate_counters = [] + self.update_period = DEFAULT_BACKGROUND_UPDATE_PERIOD + self._rotation_rate_enable(False) + self.return_to_menu() + + + def _stop_sensor_test_mode(self): + if self._logging: + print("HT:Stopping Sensor Test mode and cleaning up") + self._sensor_data = {} + self._display_data = {} + self._hexdrive_in_use_port = None + self.return_to_menu() + + + def _init_ina226_for_motor_test(self) -> bool: + self._ina226 = None + self._ina226_sensor_mgr = None + self._ina226_reading = {} + self._reset_ina226_accumulators() + try: + mgr = SensorManager(logging=self._logging) + # The INA226 sensor can't be on a port with an EEPROM because that would clash with the UUT EEPROM. + for port in range(1, 1+_SLOTS): + if not mgr.open(port): + mgr.close() + if self._logging: + print(f"HT:INA226 - no sensors found on port {port}") + continue + # 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"HT:INA226 found @ 0x{sensor.i2c_addr:02X} on port {port}") + return True + # No INA226 found; close the manager + mgr.close() + except Exception as e: # pylint: disable=broad-exception-caught + if self._logging: + print(f"HT: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_sample_count = -1 + + + def _sample_ina226_in_background(self, delta: int) -> None: # pylint: disable=unused-argument + sensor = self._ina226 + if sensor is None: + return + data = sensor.read_sample_if_ready() + if data is None: + return + try: + if self._ina226_sample_count >= 0: + # only use samples after the first one, to allow the INA226 to settle after a power change before we start accumulating data for averaging + self._ina226_sum_current_ma += int(data.get("mA", 0)) + self._ina226_sum_bus_mv += int(data.get("mV", 0)) + self._ina226_sample_count += 1 + except Exception as e: # pylint: disable=broad-exception-caught + if self._logging: + print(f"HT: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 + voltage_mv = self._ina226_sum_bus_mv // count + self._ina226_reading = { + "mA": current_ma, + "mV": voltage_mv, + } + self._reset_ina226_accumulators() + return current_ma + + + def _auto_rotation_rate_step(self): + # Advance to next power level + self._scan_step += 1 + self.refresh = True + if self._scan_step >= _AUTO_SCAN_STEPS: + if self._scan_direction == -1: + # Scan complete + print(f"HT:Completed scan") + self._scan_done = True + self._rotation_rate_motor_power = 0 + #self._auto_fit_calculate() + self._save_capture_data_csv() + return + else: + print(f"HT:Starting second scan pass in reverse direction") + self._scan_direction = -1 # reverse direction for second pass + self._scan_step = 0 + self._rotation_detected = False + # start measurement at the new power level + self._rotation_rate_motor_power = self._scan_direction * (_AUTO_SCAN_MIN_POWER + (((_MAX_POWER - _AUTO_SCAN_MIN_POWER) * self._scan_step) // (_AUTO_SCAN_STEPS - 1))) + self._rotation_rate_measurement_period_elapsed = 0 + self._capture_settling = True + + + def _data_save_path_option(self): + try: + return int(self.settings["path"].v) + except Exception: # pylint: disable=broad-exception-caught + return 0 + + + def _mount_current_fs(self): + if self.config is None or getattr(self.config, "port", None) is None: + print("HT:Hex fs save unavailable when running from badge") + return None, False + mountpoint = "/hextest" + header = self._get_header_for_port(self.config.port) + if header is None: + print("HT:Failed to read HexTest EEPROM header") + return None, False + try: + _, partition = get_hexpansion_block_devices(self.config.i2c, header, eeprom_addr, addr_len=addr_len) + except RuntimeError as exc: + print(f"HT:Failed to get block device: {exc}") + return None, False + + mounted_here = True + try: + vfs.mount(partition, mountpoint, readonly=False) + except OSError as exc: + if exc.args and exc.args[0] == 1: + mounted_here = False + else: + print(f"HT:Failed to mount {mountpoint}: {exc}") + return None, False + except Exception as exc: # pylint: disable=broad-exception-caught + print(f"HT:Failed to mount {mountpoint}: {exc}") + return None, False + return mountpoint, mounted_here + + + def _data_save_path(self): + if self._data_save_path_option() == 1: + mountpoint, mounted_here = self._mount_current_fs() + if mountpoint is None: + return None, None, False + return f"{mountpoint}/{_FILE}{self._hut_id}.{_EXT}", mountpoint, mounted_here + return f"/{_FILE}{self._hut_id}.{_EXT}", None, False + + + def _save_capture_data_csv(self): + if not self._capture_data and not self._unsaved_data: + return False + + output_path, mountpoint, mounted_here = self._data_save_path() + if output_path is None: + self.notification = Notification(" Save Failed") + return False + + rpm_count = len(self._rotation_rate_rpms) + header = ["power"] + [f"rpm{index + 1}" for index in range(rpm_count)] + ["ma"] + try: + with open(output_path, "wb") as csv_file: + csv_file.write((",".join(header) + "\n").encode()) + for power, rpms, current_ma in self._capture_data: + row = [str(power)] + row.extend(str(rpm) for rpm in rpms) + row.append(str(current_ma)) + csv_file.write((",".join(row) + "\n").encode()) + except Exception as exc: # pylint: disable=broad-exception-caught + print(f"HT:Failed to save CSV {output_path}: {exc}") + self.notification = Notification(" Save Failed") + return False + finally: + if mounted_here and mountpoint is not None: + try: + vfs.umount(mountpoint) + except Exception as exc: # pylint: disable=broad-exception-caught + print(f"HT:Failed to unmount {mountpoint}: {exc}") + self._unsaved_data = False + print(f"HT:Saved CSV to {output_path}") + self.notification = Notification(" Data Saved") + # auto increment HUT ID for next save + self._hut_id += 1 + return True + + + # ------------------------------------------------------------------ + # HEXPANSION operations + # ------------------------------------------------------------------ + + + ### MAIN APP CONTROL FUNCTIONS ### + + def update(self, delta: int): + """Main update function called from the main loop. Handles state transitions, user input, and delegates to functional area managers.""" + if not self._foreground: + # This triggers the automatic foreground display + eventbus.emit(RequestForegroundPushEvent(self)) + #self._foreground = True + + if self.notification: + self.notification.update(delta) + try: + # in case access to protected member _is_closed() is not allowed, we catch the exception and + # to prevent crashes - this means that in this case we won't be able to automatically clear + # notifications when they are closed, but at least the app won't crash. + if self.notification._is_closed(): # pylint: disable=protected-access + self.notification = None + except Exception as e: # pylint: disable=broad-exception-caught + if self.logging: + print(f"HT:Error: checking notification status: {e}") + + # Unfortunately, even though we can track if there is an active notification that we have triggered, + # we don't have a way to track if there are any other notifications active that we + # didn't trigger, so we need to perform extra display refresh cycles in case. + # As the draw function is VERY slow, and hence it stalls background updates + # we only do extra refresh cycles if the update period is long. + if self.update_period >= DEFAULT_BACKGROUND_UPDATE_PERIOD: + self.refresh = True + + ## DO STUFF BASED ON STATE ## + if self.current_state == STATE_MENU: + if self.current_menu is None: + self.set_menu() + self.refresh = True + else: + menu = self.menu + if menu is None: + self.set_menu() + self.refresh = True + return + menu.update(delta) + if menu.is_animating != "none": + if self.logging: + print("HT:Menu is animating") + self.refresh = True + elif self.button_states.get(BUTTON_TYPES["CANCEL"]) and self.current_state in MINIMISE_VALID_STATES: + self.button_states.clear() + self.minimise() + elif self.current_state == STATE_MESSAGE: + self._update_state_message(delta) + elif self.current_state == STATE_SETTINGS: + self._settings_mgr_update(delta) + elif self.current_state == STATE_MOTOR_TEST: + self._motor_test_update(delta) + + + if self.current_state != self.previous_state: + if self.logging: + print(f"HT:State: {self.previous_state} -> {self.current_state}") + self.previous_state = self.current_state + # something has changed - so worth redrawing + self.refresh = True + + + def _update_state_message(self, delta: int): # pylint: disable=unused-argument + if self.button_states.get(BUTTON_TYPES["CONFIRM"]): + if self.logging: + print("HT:Message acknowledged by user") + self.button_states.clear() + if self.message_type == "reboop": + # 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.current_state = self.message_return_state + else: + # Message has been acknowledged by the user - allow access to the menu + # refresh the menu in case available options have changed + self.set_menu() + self.refresh = True + self.current_state = STATE_MENU + self.message = [] + self.message_colours = [] + self.message_type = None + self.message_return_state = None + # "CANCEL" button is handled in common for all MINIMISE_VALID_STATES so no custom code here + + + + def _motor_test_update(self, delta: int): # pylint: disable=unused-argument + # CANCEL always exits motor test mode + if self.button_states.get(BUTTON_TYPES["CANCEL"]): + self.button_states.clear() + #self._show_auto_results_fit() + self._stop_motor_test_mode() + return + + # CONFIRM toggles between manual and auto mode + elif self.button_states.get(BUTTON_TYPES["CONFIRM"]): + self.button_states.clear() + self._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._scan_mode: + # Switch back to manual + #self._show_auto_results_fit() + self._rotation_rate_motor_power = 0 + self._rotation_rate_measurement_period = _ROTATION_RATE_MEASUREMENT_PERIOD_MS + self._rotation_rate_rpms = [0] * len(self._rotation_rate_counters) + self._ina226_reading = {} + self._scan_mode = False + self._scan_done = False + else: + # Start auto scan + self._scan_mode = True + self._scan_done = False + self._scan_step = 0 + self._scan_direction = 1 + self._rotation_rate_motor_power = self._scan_direction * _AUTO_SCAN_MIN_POWER + self._rotation_rate_measurement_period = _AUTO_SCAN_MEASURE_MS + self._capture_settling = True + self._capture_data = [] + self._unsaved_data = False + self._max_rpm = 0 + self._ina226_reading = {} + self._max_current_ma = 0 + self._rotation_detected = False + self.refresh = True + return + + if self._scan_mode: + if not self._scan_done: + self._rotation_rate_measurement_period_elapsed += delta + if self._capture_settling: + if self._rotation_rate_measurement_period_elapsed >= _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 and not self._rotation_detected: + + # There has been no motion from any motors - so we can skip the measure phase and move straight to the next power level + current_ma = self._consume_ina226_average() + if current_ma is not None: + current_abs = abs(current_ma) + self._last_current_ma = current_ma + if current_abs > self._max_current_ma: + self._max_current_ma = current_abs + power = self._rotation_rate_motor_power + self._rotation_rate_rpms = [0] * len(self._rotation_rate_counters) + if self._logging: + print(f"HT:Auto Scan Step {self._scan_step+1}/{_AUTO_SCAN_STEPS} - Power: {power}, Rate: 0 rpm, Current: {current_ma}mA") + self._capture_data.append((power, [0] * len(self._rotation_rate_counters), current_ma)) + self._auto_rotation_rate_step() + if not self._unsaved_data: + self._unsaved_data = True + + else: + self._rotation_detected = True + # estimate how long we need to measure for based on the count we got during the settle period, to ensure we get a good RPM (2%) + # reading even at low speeds, while still keeping the overall scan time reasonable# + cpm = (60000 * count) // self._rotation_rate_measurement_period_elapsed # rounded down - never displayed + self._rotation_rate_measurement_period = min(_AUTO_SCAN_MEASURE_MS, (60000 * 50) // cpm) if cpm > 0 else _AUTO_SCAN_MEASURE_MS + self._rotation_rate_measurement_period_elapsed = 0 + self._capture_settling = False + self._reset_ina226_accumulators() + else: + if self._rotation_rate_measurement_period_elapsed >= self._rotation_rate_measurement_period: + # Measure phase done — read counter and record result + self._rotation_rate_rpms = [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) + self.rotation_rate_rounding) // (self._rotation_rate_measurement_period_elapsed * self.rotation_rate_spokes) + if rpm > self._max_rpm: + self._max_rpm = rpm + self._rotation_rate_rpms[index] = rpm + + ### duplicate of block above - could be a method + current_ma = self._consume_ina226_average() + if current_ma is not None: + current_abs = abs(current_ma) + self._last_current_ma = current_ma + if current_abs > self._max_current_ma: + self._max_current_ma = current_abs + power = self._rotation_rate_motor_power + if self._logging: + print(f"HT:Auto Scan Step {self._scan_step+1}/{_AUTO_SCAN_STEPS} - Power: {power}, Rates: {self._rotation_rate_rpms} rpm, Current: {current_ma}mA") + self._capture_data.append((power, self._rotation_rate_rpms, current_ma)) + self._auto_rotation_rate_step() + if self._unsaved_data: + self._unsaved_data = True + + # 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 >= self._rotation_rate_measurement_period: + 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"HT:Rotation Rates: {self._rotation_rate_rpms}") + + # Manual mode button handling + if self.button_states.get(BUTTON_TYPES["UP"]): + self.button_states.clear() + if self.settings['serialise'].v: + self._hut_id += 1 + else: + self.rotation_rate_emitter_duty = min(255, self.rotation_rate_emitter_duty + _IR_EMITTER_PWM_STEP_SIZE) + if self.logging: + print(f"HT:IR+Emitter Duty: {self.rotation_rate_emitter_duty}") + self.refresh = True + elif self.button_states.get(BUTTON_TYPES["DOWN"]): + self.button_states.clear() + if self.settings['serialise'].v: + self._hut_id = max(0, self._hut_id - 1) + else: + self.rotation_rate_emitter_duty = max(0, self.rotation_rate_emitter_duty - _IR_EMITTER_PWM_STEP_SIZE) + if self.logging: + print(f"HT:IR-Emitter Duty: {self.rotation_rate_emitter_duty}") + self.refresh = True + elif self.button_states.get(BUTTON_TYPES["RIGHT"]): + self.button_states.clear() + self._rotation_rate_motor_power = min(_MAX_POWER, self._rotation_rate_motor_power + 1000) + if self.logging: + print(f"HT:Motor+Power: {self._rotation_rate_motor_power}") + self.refresh = True + elif self.button_states.get(BUTTON_TYPES["LEFT"]): + self.button_states.clear() + self._rotation_rate_motor_power = max(-_MAX_POWER, self._rotation_rate_motor_power - 1000) + if self.logging: + print(f"HT:Motor-Power: {self._rotation_rate_motor_power}") + self.refresh = True + + + + + ### End of Update ### + + + + def draw(self, ctx): + """Main draw function called from the main loop. Handles drawing the current state, including any notifications.""" + if self.current_state == STATE_MENU and self.menu is not None: + # These need to be drawn every frame as they contain animations + clear_background(ctx) + self.menu.draw(ctx) + elif self.refresh or self.notification: + self.refresh = False + clear_background(ctx) + ctx.font_size = label_font_size + if ctx.text_align != ctx.LEFT: + # See https://github.com/emfcamp/badge-2024-software/issues/181 + ctx.text_align = ctx.LEFT + ctx.text_baseline = ctx.BOTTOM + + # Common states for messages and errors, which can be triggered by any functional area manager and are displayed in a consistent way + if self.current_state == STATE_MESSAGE: + if self.message_colours == []: + self.message_colours = [(1,0,0)]*len(self.message) + self.draw_message(ctx, self.message, self.message_colours, label_font_size) + if self.message_type is None or self.message_type == "warning": + button_labels(ctx, confirm_label="OK", cancel_label="Exit") + elif self.current_state == STATE_SETTINGS: + self.settings_mgr_draw(ctx) + elif self.current_state == STATE_SENSOR: + #self.sensor_test_draw(ctx) + pass + elif self.current_state == STATE_MOTOR_TEST: + self._motor_test_draw(ctx) + + # Notifications are drawn on top of everything else, so that they are visible regardless of the current state. + # They also contain animations, so need to be drawn every frame when active. + # As they 'withdraw' they reveal whatever is underneath them so this must be redrawn every frame while they are active to avoid leaving visual glitches on the screen. + if self.notification: + self.notification.draw(ctx) + + + + @staticmethod + def draw_message(ctx, message, colours, size=label_font_size): + """Utility function to draw a multi-line message on the screen, with optional colour for each line. + The message is centred on the screen, and the y-position of each line is adjusted based on the total number of lines to ensure it is visually balanced.""" + ctx.font_size = size + num_lines = len(message) + for i_num, instr in enumerate(message): + text_line = str(instr) + width = ctx.text_width(text_line) + try: + colour = colours[i_num] + except IndexError: + colour = None + if colour is None: + colour = (1,1,1) + # Font is not central in the height allocated to it due to space for descenders etc... + # this is most obvious when there is only one line of text + # # position fine tuned to fit around button labels when showing 5 lines of text + y_position = int(0.35 * ctx.font_size) if num_lines == 1 else int((i_num-((num_lines-2)/2)) * ctx.font_size - 2) + ctx.rgb(*colour).move_to(-width//2, y_position).text(text_line) + + + def _motor_test_draw(self, ctx): + if self.config is None: + return + if self._scan_mode: + self._draw_auto_scan(ctx) + return + #print("DRAWING") + if self.settings['serialise'].v: + lines = [f"ID:{self._hut_id}"] # show the current HUT ID for data logging purposes + else: + # 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('mA', 0)}mA V:{format_voltage_mv(self._ina226_reading.get('mV', 0))}"] + colours += [(0.3, 0.8, 1.0)] + else: + lines += [""] + colours += [(0.3, 0.8, 1.0)] + self.draw_message(ctx, lines, colours, label_font_size) + button_labels(ctx, + up_label="ID+" if self.settings['serialise'].v else "IR+", + down_label="ID-" if self.settings['serialise'].v else "IR-", + cancel_label="Back", left_label="Pwr-", right_label="Pwr+", confirm_label="Scan") + + + 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 = 35 + 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._capture_data) + max_rpm = self._max_rpm if self._max_rpm > 0 else 1 + max_current_ma = self._max_current_ma if self._max_current_ma > 0 else 1 + + if n > 1: + # Plot data points as bars. + # Auto-scan results contain a list/tuple of per-counter RPMs. + bar_w = max(1, chart_w // _AUTO_SCAN_STEPS) + for i in range(n): + power, rpms, current_ma = self._capture_data[i] + # allow for the minimum power level being above zero, and for negative power levels, by using the absolute value of power and offsetting the X position accordingly + x = chart_left + ((abs(power) - _AUTO_SCAN_MIN_POWER) * chart_w) // (_MAX_POWER - _AUTO_SCAN_MIN_POWER) + 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 power < 0: + index = index + len(self._rotation_rate_counters) # offset index for negative power to differentiate on the graph + ctx.rgb(*self._colour_for_index(index)).rectangle(x, chart_bottom - h - 1, bar_w, 2).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, marker_y - 1, bar_w, 2).fill() + + # Title and max RPM label + ctx.font_size = label_font_size + if self._scan_done: + ctx.move_to(-30, chart_top - 25).text("Motors") + + ctx.font_size = label_font_size - 8 + ctx.rgb(0.0, 1.0, 1.0).move_to(chart_left, chart_bottom + 5 + ctx.font_size).text(f"{(100 * (_AUTO_SCAN_MIN_POWER + (_MAX_POWER//200)))//_MAX_POWER}%") + width = ctx.text_width("Power") + ctx.move_to(-width//2, chart_bottom + 5 + ctx.font_size).text("Power") + width = ctx.text_width("100%") + ctx.move_to(chart_right - width, chart_bottom + 5 + ctx.font_size).text("100%") + # provide a legend for the colours on the graph for the rpms only + for index in range(len(self._rotation_rate_counters)): + ctx.rgb(*self._colour_for_index(index)).move_to(chart_left+20, chart_bottom + 5 + ((index + 2) * (ctx.font_size))).text(f"Motor {index+1} RPM") + # Plot best fit line + fit = self._motor_calibration_fit[index] if index < len(self._motor_calibration_fit) else None + if fit is None: + continue + slope, intercept = fit + # get min and max power values from the scan range + left_power = self._capture_data[0][0] + right_power = self._capture_data[n-1][0] + # is intercept going to be with X or Y axis as only positive quadrant shown + if intercept < 0: + x1 = chart_left - ((intercept * max_rpm) // slope) + y1 = chart_bottom + else: + x1 = chart_left + y1 = chart_bottom - ((slope * left_power + intercept) * chart_h) // max_rpm + # is line going to leave chart along the top or right edge? + if slope * right_power + intercept > max_rpm: + x2 = chart_left + ((max_rpm - intercept) * right_power) // slope + y2 = chart_top + else: + x2 = chart_right + y2 = chart_bottom - ((slope * right_power + intercept) * chart_h) // max_rpm + print(f"ST:Motor {index+1} calibration line: slope={slope}, intercept={intercept}, x1={x1}, y1={y1}, x2={x2}, y2={y2}") + ctx.rgb(*self._colour_for_index(index)).move_to(x1, y1).line_to(x2, y2).stroke() + + else: + progress = (self._scan_step * 100) // _AUTO_SCAN_STEPS + ctx.rgb(1.0,1.0,1.0).move_to(-50, chart_top - 25).text(f"Scan {progress}%") + + # Instantaneous current label (updated live during the scan) + ctx.font_size = label_font_size - 8 + for index, rpm in enumerate(self._rotation_rate_rpms): + colour_index = index + len(self._rotation_rate_counters) if self._rotation_rate_motor_power < 0 else index # offset index for negative power to differentiate on the graph + ctx.rgb(*self._colour_for_index(colour_index)).move_to(chart_left+20, chart_bottom + 5 + ((index + 2) * (ctx.font_size))).text(f"Mtr{index+1}: {rpm}rpm") + ctx.rgb(1.0, 0.0, 1.0).move_to(chart_left+20, chart_bottom + 5 + ctx.font_size).text(f"PWM:{(100*abs(self._rotation_rate_motor_power)+(_MAX_POWER//2))//_MAX_POWER}%") + ctx.rgb(1.0, 0.2, 0.2).move_to(25, chart_bottom + 5 + ctx.font_size).text(f"{self._last_current_ma}mA") + + # Y axis Maximum RPM and Current labels + ctx.font_size = label_font_size - 8 + ctx.rgb(1.0, 1.0, 0.2).move_to(-15, chart_top - 5).text("Max") + ctx.rgb(0.2, 1.0, 1.0).move_to(chart_left+10, chart_top - 5).text(f"rpm:{self._max_rpm}") + ctx.rgb(1.0, 0.2, 0.2).move_to(25, chart_top - 5).text(f"mA:{self._max_current_ma}") + + button_labels(ctx, confirm_label="OK" if self._scan_done else "Quit") + + + def _colour_for_index(self, index: int) -> tuple[float, float, float]: + # lookup from table of colours, green, orange, blue, yellow + return { + 0: (0.0, 1.0, 0.5), + 1: (1.0, 0.5, 0.0), + 2: (0.0, 0.5, 1.0), + 3: (0.5, 1.0, 0.0), + }.get(index, (1.0, 1.0, 1.0)) # default to white if index out of range + + + + + + + def return_to_menu(self, menu_name: str | None = None): + """Utility function to return to the main menu from any state. This is used when the user cancels out of a submenu or after acknowledging a warning message.""" + if self.logging: + print("HT:Returning to menu") + if menu_name is not None: + self.set_menu(menu_name) + self.update_period = DEFAULT_BACKGROUND_UPDATE_PERIOD + self.current_state = STATE_MENU + self.refresh = True + + + 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"HT:Showing message: '{msg_content}' with type {msg_type}") + 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 + + + +### MENU FUNCTIONALITY ### + + + def set_menu(self, menu_name: str | None = "main"): #: Literal["main"]): does it work without the type hint? + """Set the current menu to the specified menu name, and construct the menu if necessary. + If menu_name is None, it will clear the current menu and return to the previous state + (e.g. from a submenu back to the main menu).""" + if self.logging: + print(f"HT:Set Menu {menu_name}") + if self.menu is not None: + try: + self.menu._cleanup() # pylint: disable=protected-access + except Exception: # pylint: disable=broad-except + # See badge-2024-software PR#168 + # in case badge s/w changes and this is done within the menu s/w + # and then access to this function is removed + pass + self.current_menu = menu_name + if menu_name == "main": + # construct the main menu based on template + menu_items = MAIN_MENU_ITEMS.copy() + self.menu = Menu( + self, + menu_items, + select_handler=self._main_menu_select_handler, + back_handler=self._menu_back_handler, + ) + elif menu_name == MAIN_MENU_ITEMS[MENU_ITEM_SETTINGS]: # "Settings" + # construct the settings menu + _settings_menu_items = ["SAVE ALL", "DEFAULT ALL"] + for _, setting in enumerate(self.settings): + _settings_menu_items.append(f"{setting}") + self.menu = Menu( + self, + _settings_menu_items, + select_handler=self._settings_menu_select_handler, + back_handler=self._menu_back_handler, + ) + + + # 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"HT:Main Menu {item} at index {idx}") + if item == MAIN_MENU_ITEMS[MENU_ITEM_MOTOR_TEST]: # Motor Test + self.button_states.clear() + if self._motor_test_start(): + self.set_menu(None) + self.current_state = STATE_MOTOR_TEST + elif item == MAIN_MENU_ITEMS[MENU_ITEM_SENSOR_TEST]: # Sensor Test + self.button_states.clear() + self.set_menu(None) + self.show_message(["Sensor test","not implemented","yet"], [(1,0.5,0)]*3, msg_type="warning") + #if self._sensor_test_start(): + # self.current_state = STATE_SENSOR + elif item == MAIN_MENU_ITEMS[MENU_ITEM_SETTINGS]: # Settings + self.set_menu(MAIN_MENU_ITEMS[MENU_ITEM_SETTINGS]) + elif item == MAIN_MENU_ITEMS[MENU_ITEM_ABOUT]: # About + self.button_states.clear() + self.set_menu(None) + self.show_message(["HexTest", f"V{self.VERSION}", "By RobotMad"], [(0.2,1,0.2), (1,1,0), (1,1,1)]) + elif item == MAIN_MENU_ITEMS[MENU_ITEM_EXIT]: # Exit + self._exit_app() + + + def _settings_menu_select_handler(self, item: str, idx: int): + if self.logging: + print(f"HT:Setting {item} @ {idx}") + if idx == 0: #Save + if self.logging: + print("HT:Settings Save All") + platform_settings.save() + self.notification = Notification(" Settings Saved") + self.set_menu() + elif idx == 1: #Default + if self.logging: + print("HT:Settings Default All") + for s in self.settings: + self.settings[s].v = self.settings[s].d + self.settings[s].persist() + self.notification = Notification(" Settings Defaulted") + self.set_menu() + elif self.settings_mgr_start(item): + self.current_state = STATE_SETTINGS + + + def _menu_back_handler(self): + if self.current_menu == "main": + self.minimise() + # for submenus, just return to the main menu + self.set_menu() + + + + +# -------------------------------------------------- +# Private methods for internal use only. +# -------------------------------------------------- + + @staticmethod + 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 + + + + # ------------------------------------------------------------------ + + def settings_mgr_start(self, item: str) -> bool: + """Enter Settings editing mode from the main menu.""" + self.set_menu(None) + self.button_states.clear() + self.refresh = True + self.auto_repeat_clear() + if self._logging: + print("HT:Entered Settings editing mode") + self.edit_setting = item + self.edit_setting_value = self.settings[item].v + return True + + # ------------------------------------------------------------------ + # Per-tick update + # ------------------------------------------------------------------ + + def _settings_mgr_update(self, delta): + """Handle Settings editing UI. Returns True if this module handled the state.""" + + if self.button_states.get(BUTTON_TYPES["UP"]): + if self.auto_repeat_check(delta, False): + self.edit_setting_value = self.settings[self.edit_setting].inc(self.edit_setting_value, self.auto_repeat_level) + if self._logging: + print(f"HT:Setting: {self.edit_setting} (+) Value: {self.edit_setting_value}") + self.refresh = True + elif self.button_states.get(BUTTON_TYPES["DOWN"]): + if self.auto_repeat_check(delta, False): + self.edit_setting_value = self.settings[self.edit_setting].dec(self.edit_setting_value, self.auto_repeat_level) + if self._logging: + print(f"HT:Setting: {self.edit_setting} (-) Value: {self.edit_setting_value}") + self.refresh = True + else: + self.auto_repeat_clear() + if self.button_states.get(BUTTON_TYPES["RIGHT"]) or self.button_states.get(BUTTON_TYPES["LEFT"]): + self.button_states.clear() + self.edit_setting_value = self.settings[self.edit_setting].d + if self._logging: + print(f"HT:Setting: {self.edit_setting} Default: {self.edit_setting_value}") + self.refresh = True + self.notification = Notification("Default") + elif self.button_states.get(BUTTON_TYPES["CANCEL"]): + self.button_states.clear() + if self._logging: + print(f"HT:Setting: {self.edit_setting} Cancelled") + self.return_to_menu(MAIN_MENU_ITEMS[MENU_ITEM_SETTINGS]) + elif self.button_states.get(BUTTON_TYPES["CONFIRM"]): + self.button_states.clear() + if self._logging: + print(f"HT:Setting: {self.edit_setting} = {self.edit_setting_value}") + self.settings[self.edit_setting].v = self.edit_setting_value + self.settings[self.edit_setting].persist() + self.notification = Notification(f" Setting: {self.edit_setting}={self.edit_setting_value}") + self.return_to_menu(MAIN_MENU_ITEMS[MENU_ITEM_SETTINGS]) + return True + + + # ------------------------------------------------------------------ + # Draw + # ------------------------------------------------------------------ + + def settings_mgr_draw(self, ctx): + """Render Settings editing UI. Returns True if handled.""" + disp_val = self.settings[self.edit_setting].label(self.edit_setting_value) + self.draw_message(ctx, ["Edit Setting", f"{self.edit_setting}:", f"{disp_val}"], [(1, 1, 0), (0, 0, 1), (0, 1, 0)], label_font_size) + button_labels(ctx, up_label="+", down_label="-", confirm_label="Set", cancel_label="Cancel", right_label="Default") + return True + + + # ------------------------------------------------------------------ + + +class HexpansionType: + """Descriptor for known hexpansion types, used for detection and EEPROM programming. + + Parameters + ---------- + pid: the PID value to identify the hexpansion type from its EEPROM header + name: human-friendly name of the hexpansion type (e.g. "HexDrive") + vid: the VID value to identify the hexpansion type from its EEPROM header (default 0xCAFE) + motors, servos, sensors: the capabilities of this hexpansion type, used to configure the app when detected + sub_type: a human-friendly string describing the specific variant of this hexpansion type + app_mpy_name: the filename of the .mpy to copy to the hexpansion EEPROM for this type (if any) + app_mpy_version: the version string to report for the .mpy copied to the hexpansion EEPROM for this type (if any) + app_name: the name of the App class to look for when checking if a detected hexpansion's app is running (if any) + """ + def __init__(self, pid: int, name: str, vid: int =0xCAFE, motors: int =0, servos: int =0, sensors: int =0, sub_type: str | None =None): + self.vid: int = vid + self.pid: int = pid + self._name: str = name + self._sub_type: str | None = sub_type + self._motors: int = motors + self._servos: int = servos + self._sensors: int = sensors + + @property + def name(self): + return self._name + + @property + def sub_type(self): + return self._sub_type + + @property + def motors(self): + return self._motors + + @property + def servos(self): + return self._servos + + @property + def sensors(self): + return self._sensors + + + + +class MySetting: + """Class to represent a single setting, including its current value, default value, min/max values, and optional labels for display. """ + def __init__(self, container, default, minimum, maximum, labels=None): + self._container = container + self.d = default + self.v = default + self._min = minimum + self._max = maximum + self._labels = labels + + def __str__(self): + return str(self.v) + + def _index(self): + for k, v in self._container.items(): + if v == self: + return k + return None + + def label(self, index: int | None = None): + """ Return the label for the given index, or the current value if no index is provided. """ + if index is not None: + if self._labels is not None and index < len(self._labels): + return self._labels[int(index)] + return str(index) + if self._labels is not None and self.v is not None and self.v < len(self._labels): + return self._labels[int(self.v)] + return str(self.v) + + @staticmethod + def _quantize_tenths(value: float) -> float: + """Round to 0.1 steps deterministically to avoid float drift artifacts.""" + scaled = int((value * 10) + (0.5 if value >= 0 else -0.5)) + return scaled / 10.0 + + def inc(self, v, l=0): + """ Increment the setting value. If l > 0, increment by the next highest order of magnitude (e.g. 10s place for l=1, 100s place for l=2, etc.)""" + if isinstance(self.v, bool): + v = not v + elif isinstance(self.v, int): + if l == 0: + v += 1 + else: + d = 10 ** l + v = ((v // d) + 1) * d + if v > self._max: + if self._labels is not None: + # settings that are purely label-based wrap around + v = 0 + else: + v = self._max + elif isinstance(self.v, float): + v = self._quantize_tenths(v) + 0.1 + if v > self._max: + v = self._max + v = self._quantize_tenths(v) + elif self._container['logging'].v: + print(f"H:inc type: {type(self.v)}") + return v + + def dec(self, v, l=0): + """Decrement the setting value. If l > 0, decrement by the next highest order of magnitude (e.g. 10s place for l=1, 100s place for l=2, etc.)""" + if isinstance(self.v, bool): + v = not v + elif isinstance(self.v, int): + if l == 0: + v -= 1 + else: + d = 10 ** l + v = (((v + (9 * (10 ** (l - 1)))) // d) - 1) * d + if v < self._min: + if self._labels is not None: + # settings that are purely label-based wrap around + v = len(self._labels) - 1 + else: + v = self._min + elif isinstance(self.v, float): + v = self._quantize_tenths(v) - 0.1 + if v < self._min: + v = self._min + v = self._quantize_tenths(v) + elif self._container['logging'].v: + print(f"H: dec type: {type(self.v)}") + return v + + 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"{_PRE}.{index}" + try: + 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 {key}: {e}") + + # ------------------------------------------------------------------ + +#------------------------------------------------------------------ +# ESP32S3 PCNT (Pulse Counter) hardware register definitions and bit masks +# Supports all 4 PCNT units (0-3) on the ESP32-S3. +#------------------------------------------------------------------- + +_SYSTEM_BASE = const(0x600C0000) +_GPIO_BASE = const(0x60004000) +_PCNT_BASE = const(0x60017000) + +_PCNT_NUM_UNITS = const(4) # ESP32-S3 has 4 PCNT units + +_PCNT_CLK_BIT = const(1 << 10) # SYSTEM_PCNT_CLK_EN / SYSTEM_PCNT_RST (bit 10) + +# System/Clock registers +_CLK_EN0_REG = const(_SYSTEM_BASE + 0x0018) +_RST_EN0_REG = const(_SYSTEM_BASE + 0x0020) + +# GPIO Matrix Base +_GPIO_FUNC_IN_SEL_CFG_BASE = const(_GPIO_BASE + 0x0154) +_SIG_IN_SEL_BIT = const(1 << 6) # Enable routing via GPIO Matrix + +# PCNT register offsets (per-unit, relative to _PCNT_BASE) +# CONF0: _PCNT_BASE + unit * 0x0C +# CONF1: _PCNT_BASE + unit * 0x0C + 0x04 +# CONF2: _PCNT_BASE + unit * 0x0C + 0x08 +# CNT: _PCNT_BASE + 0x30 + unit * 4 +# STATUS:_PCNT_BASE + 0x50 + unit * 4 +_PCNT_CTRL_REG = const(_PCNT_BASE + 0x0060) + +# _PCNT_CTRL_REG bits — per-unit reset and pause at (unit * 2) and (unit * 2 + 1) +_PCNT_CTRL_CLK_EN = const(1 << 16) # Register clock gate — must be 1 for register access + +# CONF0 bit layout (same layout for all units) +_CONF0_FILTER_THRES_M = const(0x3FF) # bits [9:0] +_CONF0_FILTER_EN = const(1 << 10) +_CONF0_CH0_POS_MODE_S = const(18) # bits [19:18] + +# GPIO signal index base for PCNT: Unit N, CH0 pulse = 33 + N*4, CH0 ctrl = 35 + N*4 +_PCNT_SIG_BASE = const(33) + +# APB clock frequency for filter calculation (Hz) +_APB_CLK_HZ = const(80_000_000) + + +# Reverse lookup: GPIO number -> (port, pin_index) for diagnostics +_GPIO_TO_HS = {} +for _port, _gpios in _HS_PIN_TO_GPIO.items(): + for _idx, _gpio in enumerate(_gpios): + _GPIO_TO_HS[_gpio] = (_port, _idx) + + +# ------------------------------------------------------------------ + +class Counter: + """Wrapper around ESP32-S3 PCNT hardware for counting rising edges. + + Parameters + ---------- + src : int + The ESP32-S3 GPIO number to count pulses on. + Use ``_HS_PIN_TO_GPIO[port][index]`` to convert from a badge HS pin. + id : int | None + PCNT unit to use (0-3). If ``None``, the first available (unused) unit + is auto-selected. If the requested unit is already in use, ``__init__`` + sets ``self.unit = None`` to signal failure. + filter_ns : int + Minimum pulse width in nanoseconds. Pulses shorter than this are + rejected by the hardware glitch filter. Set to 0 to disable filtering. + logging : bool + Print diagnostic messages to the console. + + CURRENTLY ONLY COUNTS UP ON RISING EDGES + """ + + def __init__(self, unit: int | None, src: int, filter_ns: int = 0, logging: bool = False): + self.logging = 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 + + if not self.init(src, filter_ns): + if self.logging: + print(f"PCNT: failed to configure unit {self.unit}") + self.unit = None + + + def _unit_in_use(self, unit: int) -> bool: + """Check whether a PCNT unit appears to already be in use. + + 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 + + ctrl = mem32[_PCNT_CTRL_REG] + + # 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 + + # 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 + + # 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 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}, count={cnt}, routed to GPIO {routed_gpio}") + return True + + + 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})" + + + 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 + + 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}") + + 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 + + # --- 5. RELEASE FROM RESET --- + ctrl = mem32[_PCNT_CTRL_REG] + ctrl &= ~rst_bit + mem32[_PCNT_CTRL_REG] = ctrl + + self._configured = True + + except Exception as e: # pylint: disable=broad-exception-caught + print(f"PCNT U{unit}: error configuring: {e}") + return False + + if self.logging: + 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 0 + + unit = self.unit + if unit is None: + return 0 + + 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 + + + def deinit(self): + """Release the PCNT unit: hold it in reset and clear its CONF0.""" + 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 + + if self.logging: + print(f"PCNT U{unit}: released") + + # 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") + + + + + + + + + + + +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, 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) + # ------------------------------------------------------------------ + + def begin(self, i2c) -> bool: + """Initialise the sensor on the given I2C bus. + + Returns True if the sensor is found and configured successfully. + Store the i2c object for later use in read(). + """ + self._i2c = i2c + self._ready = False + try: + self._ready = self._init() + except Exception as e: # pylint: disable=broad-exception-caught + print(f"S:{self.NAME} begin error: {e}") + self._ready = False + return self._ready + + 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. + """ + if not self._ready: + return {"Error": "not ready"} + try: + 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: + self._shutdown() + except Exception as e: # pylint: disable=broad-exception-caught + print(f"S:{self.NAME} reset error: {e}") + self._ready = False + + def shutdown(self): + """Put the sensor into a low-power state without changing ready state.""" + if self._i2c is None: + return + try: + self._shutdown() + except Exception as e: # pylint: disable=broad-exception-caught + print(f"S:{self.NAME} shutdown error: {e}") + + @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 + + # ------------------------------------------------------------------ + # Internal helpers - override in sub-classes + # ------------------------------------------------------------------ + + def _init(self) -> bool: + """Hardware initialisation. Return True on success.""" + raise NotImplementedError + + def _measure(self, timeout: int = 0) -> dict: + """Perform measurement. Return dict of {label: value_str}.""" + raise NotImplementedError + + def _shutdown(self): + """Optional power-down hook. + + Subclasses can implement register writes here when hardware supports + an explicit shutdown mode. Drivers without dedicated power management + can leave this as a no-op. + """ + return + + # ------------------------------------------------------------------ + # Utility helpers available to all drivers + # ------------------------------------------------------------------ + + 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: + return self._read_reg(reg, 1)[0] + + def _read_u16_le(self, reg: int) -> int: + d = self._read_reg(reg, 2) + return d[0] | (d[1] << 8) + + def _read_u16_be(self, reg: int) -> int: + d = self._read_reg(reg, 2) + return (d[0] << 8) | d[1] + + def _read_s16_be(self, reg: int) -> int: + value = self._read_u16_be(reg) + if value & 0x8000: + value -= 0x10000 + return value + + def _write_u8(self, reg: int, value: int): + self._write_reg(reg, bytes([value & 0xFF])) + + def _write_u16_be(self, reg: int, value: int): + self._write_reg(reg, bytes([(value >> 8) & 0xFF, value & 0xFF])) + + + + + +# Register map +_REG_CONFIGURATION = const(0x00) # Configuration register +_REG_SHUNT_VOLTAGE = const(0x01) # Shunt voltage result (signed) +_REG_BUS_VOLTAGE = const(0x02) # Bus voltage result (unsigned) +_REG_POWER = const(0x03) # Power result (unsigned) +_REG_CURRENT = const(0x04) # Current result (signed) +_REG_CALIBRATION = const(0x05) # Calibration register +_REG_MASK_ENABLE = const(0x06) # Alert mask/enable register +_REG_ALERT_LIMIT = const(0x07) # Alert threshold register +_REG_MANUFACTURER_ID = const(0xFE) # Manufacturer ID register +_REG_DIE_ID = const(0xFF) # Die ID register + + +# Configuration register bits (0x00) +_CFG_RESET_BIT = const(0x8000) # Software reset bit +_CFG_AVG_SHIFT = const(9) # Averaging field shift (bits 11:9) +_CFG_VBUSCT_SHIFT = const(6) # Bus voltage conversion time field shift (bits 8:6) +_CFG_VSHCT_SHIFT = const(3) # Shunt voltage conversion time field shift (bits 5:3) +_CFG_MODE_SHIFT = const(0) # Operating mode field shift (bits 2:0) + +# AVG field values (bits 11:9) +_CFG_AVG_1 = const(0b000) # 1 sample average +_CFG_AVG_4 = const(0b001) # 4 sample average +_CFG_AVG_16 = const(0b010) # 16 sample average +_CFG_AVG_64 = const(0b011) # 64 sample average +_CFG_AVG_128 = const(0b100) # 128 sample average +_CFG_AVG_256 = const(0b101) # 256 sample average +_CFG_AVG_512 = const(0b110) # 512 sample average +_CFG_AVG_1024 = const(0b111) # 1024 sample average + +# Conversion time field values for VBUSCT/VSHCT (bits 8:6 and 5:3) +_CFG_CT_140US = const(0b000) # 140 us conversion time +_CFG_CT_204US = const(0b001) # 204 us conversion time +_CFG_CT_332US = const(0b010) # 332 us conversion time +_CFG_CT_588US = const(0b011) # 588 us conversion time +_CFG_CT_1100US = const(0b100) # 1.1 ms conversion time +_CFG_CT_2116US = const(0b101) # 2.116 ms conversion time +_CFG_CT_4156US = const(0b110) # 4.156 ms conversion time +_CFG_CT_8244US = const(0b111) # 8.244 ms conversion time + +# Operating mode field values (bits 2:0) +_CFG_MODE_POWER_DOWN = const(0b000) # Power-down mode +_CFG_MODE_SHUNT_TRIG = const(0b001) # Shunt voltage, triggered +_CFG_MODE_BUS_TRIG = const(0b010) # Bus voltage, triggered +_CFG_MODE_SHUNT_BUS_TRIG = const(0b011) # Shunt and bus, triggered +_CFG_MODE_ADC_OFF = const(0b100) # ADC off (disabled) +_CFG_MODE_SHUNT_CONT = const(0b101) # Shunt voltage, continuous +_CFG_MODE_BUS_CONT = const(0b110) # Bus voltage, continuous +_CFG_MODE_SHUNT_BUS_CONT = const(0b111) # Shunt and bus, continuous + + +# Mask/Enable register bits (0x06) +_MASK_SOL = const(0x8000) # Shunt over-voltage alert flag +_MASK_SUL = const(0x4000) # Shunt under-voltage alert flag +_MASK_BOL = const(0x2000) # Bus over-voltage alert flag +_MASK_BUL = const(0x1000) # Bus under-voltage alert flag +_MASK_POL = const(0x0800) # Power over-limit alert flag +_MASK_CNVR = const(0x0400) # Conversion ready alert flag +_MASK_AFF = const(0x0010) # Alert function flag +_MASK_CVRF = const(0x0008) # Conversion ready flag +_MASK_OVF = const(0x0004) # Math overflow flag +_MASK_APOL = const(0x0002) # Alert pin polarity select +_MASK_LEN = const(0x0001) # Alert latch enable + + +# Device identification +_MANUFACTURER_ID_TI = const(0x5449) # Texas Instruments manufacturer ID + + +# Driver configuration constants (100 mΩ shunt) +_SHUNT_RESISTOR_MILLIOHM = const(100) +_CALIBRATION_VALUE = const(0x0200) # 512 => 0.1 mA current register LSB with 100 mΩ shunt +_CURRENT_LSB_UA = const(100) # 0.1 mA current LSB in microamps +_POWER_LSB_UW = const(2500) # 2.5 mW power LSB in microwatts +_READ_TIMEOUT_MS = const(50) + +# Default operating configuration: +# - shunt conversion: 8.244 ms +# - bus conversion: 1.1 ms +# - averaging: 16 sample +_DEFAULT_CONFIGURATION = ( + (_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) +) + + +class INA226(SensorBase): + """INA226 sensor driver with integer fixed-point outputs.""" + + I2C_ADDR = 0x40 + I2C_ADDRS = tuple(range(0x40, 0x43)) # only allow the addresses we actually expect (full range is to 0x50) + NAME = "INA226" + READ_INTERVAL_MS = 150 + TYPE = "Power" + + 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) + + # 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 + + #print(f"S:{self.NAME} {bus_mv}mV, {current_ma}mA") + return { + "mV": bus_mv, + "mA": current_ma, + } + + def read_sample_if_ready(self) -> dict[str, int] | None: + """Return one sample in integer units when a new conversion is ready. + + This helper is intended for high-rate internal consumers (for example + background averaging in motor test mode). The public SensorBase `read()` + API still returns string values for UI rendering consistency. + """ + if not self._ready: + 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})") + 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: + return False + + self._write_u16_be(_REG_CONFIGURATION, _DEFAULT_CONFIGURATION) + self._write_u16_be(_REG_CALIBRATION, _CALIBRATION_VALUE) + self._write_u16_be(_REG_MASK_ENABLE, _MASK_CNVR | _MASK_LEN) # Enable conversion ready alert with latching + return True + + + 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 { + "mV": str(sample["mV"]), + "mA": str(sample["mA"]), + } + if time.ticks_diff(deadline, time.ticks_ms()) <= 0: + return {"Error": "timeout"} + time.sleep_ms(1) + + def _shutdown(self) -> None: + self._write_u16_be(_REG_CONFIGURATION, _CFG_MODE_POWER_DOWN) + + + + + + +_LED_PIN = const(2) # LED to illumiinate area under colour sensor to measure reflected light from surface below. +_COLOUR_INT_PIN = const(1) # Not currently used, but we can set it up as an input for future interrupt-based drivers +_DIST_INT_PIN = const(3) # Not currently used, but we can set it up as an input for future interrupt-based drivers + +ALL_SENSOR_CLASSES = [INA226] + +class SensorManager: + """Manages detection, initialisation, and reading of sensors on a hexpansion I2C port.""" + def __init__(self, logging: bool = False): + self._logging: bool = logging + self._i2c = None + self._port: int | None = None + self._sensors: list[SensorBase] = [] # list of initialised SensorBase instances + self._index: int = 0 # currently selected sensor + self._last_data = {} + self._read_interval_ms = 10 + self._type = "Generic" + if self._logging: + print("HT:SensorManager initialised") + + + # ------------------------------------------------------------------ + + @property + def read_interval(self) -> int: + """Return the recommended read interval in milliseconds for the currently selected sensor.""" + return self._read_interval_ms + + @property + def type(self) -> str: + """Return the type of the currently selected sensor, or "Generic" if no sensors or only unknown sensors are found.""" + return self._type + + + # ------------------------------------------------------------------ + # Lifecycle + # ------------------------------------------------------------------ + + def open(self, port: int) -> bool: + """Open hexpansion I2C port (1–6), scan, and initialise any found sensors. + Returns True if at least one sensor was found.""" + self.close() + self._port = port + + try: + self._i2c = I2C(port) + except Exception as e: # pylint: disable=broad-exception-caught + if self._logging: + print(f"HT:Cannot open I2C port {port}: {e}") + return False + + try: + found_addrs = set(self._i2c.scan()) + except Exception as e: # pylint: disable=broad-exception-caught + if self._logging: + print(f"HT:I2C scan failed on port {port}: {e}") + return False + + if self._logging: + print(f"HT: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, 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"HT: + {cls.NAME} @ 0x{sensor.i2c_addr:02X} {cls.TYPE}") + elif self._logging: + print(f"HT: - {cls.NAME} @ 0x{address:02X} begin() failed") + + self._index = 0 + self._last_data = {} + + # 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') + else: + self._read_interval_ms = 250 + self._type = "Generic" + + # 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): + config = HexpansionConfig(port) + if self._logging: + print(f"HT: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[_COLOUR_INT_PIN].init(mode=Pin.IN) + config.ls_pin[_DIST_INT_PIN].init(mode=Pin.IN) + + if len(self._sensors) > 0 and any(getattr(s, 'TYPE', '') == 'Colour' for s in self._sensors): + config = HexpansionConfig(port) + config.ls_pin[_DIST_INT_PIN].init(mode=Pin.IN) + return len(self._sensors) > 0 + + + def close(self): + """Shutdown all sensors and release the I2C bus.""" + for s in self._sensors: + try: + s.reset() + except Exception: # pylint: disable=broad-exception-caught + pass + if self._port is not None: + if len(self._sensors) > 0 and any(getattr(s, 'TYPE', '') == 'Colour' for s in self._sensors): + if self._logging: + print(f"HT:LED Off port {self._port}") + config = HexpansionConfig(self._port) + if config is not None: + config.ls_pin[_LED_PIN].value(0) + config.ls_pin[_LED_PIN].init(mode=Pin.IN) + self._sensors = [] + self._index = 0 + self._last_data = {} + self._i2c = None + self._port = None + + + # ------------------------------------------------------------------ + # Sensor selection + # ------------------------------------------------------------------ + + def next_sensor(self): + """Select the next sensor in the list.""" + if self._sensors: + self._index = (self._index + 1) % len(self._sensors) + self._last_data = {} + self._read_interval_ms = getattr(self._sensors[self._index], 'READ_INTERVAL_MS', 250) + self._type = getattr(self._sensors[self._index], 'TYPE', 'Generic') + + + def prev_sensor(self): + """Select the previous sensor in the list.""" + if self._sensors: + self._index = (self._index - 1) % len(self._sensors) + self._last_data = {} + self._read_interval_ms = getattr(self._sensors[self._index], 'READ_INTERVAL_MS', 250) + self._type = getattr(self._sensors[self._index], 'TYPE', 'Generic') + + + # ------------------------------------------------------------------ + # Reading + # ------------------------------------------------------------------ + + def read_current(self) -> dict: + """Read the currently selected sensor; cache result in last_data.""" + if not self._sensors: + return {"Error": "no sensors"} + self._last_data = self._sensors[self._index].read() + #self.report_interrupt() + return self._last_data + + + # ------------------------------------------------------------------ + # Properties + # ------------------------------------------------------------------ + + @property + def num_sensors(self) -> int: + """Return the number of initialised sensors.""" + return len(self._sensors) + + @property + def current_sensor_name(self) -> str: + """Return the name of the currently selected sensor, or 'none' if no sensors.""" + if not self._sensors: + return "none" + sensor = self._sensors[self._index] + return f"{sensor.NAME}" + #return f"{sensor.NAME}@0x{sensor.i2c_addr:02X}" + + @property + def current_sensor_index(self) -> int: + """Return the index of the currently selected sensor.""" + return self._index + + @property + def last_data(self) -> dict: + """Return the last data read from the currently selected sensor.""" + return self._last_data + + @property + def port(self) -> int | None: + """Return the currently open port number, or None if no port is open.""" + return self._port + + @property + def is_open(self) -> bool: + """True if the I2C bus is open and at least one sensor is initialised.""" + return self._i2c is not None and len(self._sensors) > 0 + + 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) -> SensorBase | None: + """Return the first sensor instance whose NAME matches, or None.""" + for s in self._sensors: + if s.NAME == name: + return s + return None + + +def format_voltage_mv(voltage_mv: int | None) -> str: + if voltage_mv is None: + return "--" + sign = "-" if voltage_mv < 0 else "" + absolute = abs(int(voltage_mv)) + whole = absolute // 1000 + fraction = (absolute % 1000) // 10 + return f"{sign}{whole}.{fraction:02d}V" + +__app_export__ = HexTestApp diff --git a/README.md b/README.md index e863449..ca98a97 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` | +| `EEPROM/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 EEPROM/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..6166d62 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,14 @@ #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 = 7 +HEXDRIVE2_APP_VERSION = 1 +HEXTEST_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 +75,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 +106,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 +155,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 +191,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 +214,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 +241,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,35 +253,31 @@ 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, sensors=2, sub_type="2 Line Sensors" ), + HexpansionType(0x3000, "HexTest", vid=0xCBCB, eeprom_total_size=65536, eeprom_page_size=128, app_mpy_name="hextest", app_mpy_version=HEXTEST_APP_VERSION, app_name="HexTestApp", sub_type="Motor Test" ), + HexpansionType(0x4000, "HexDiag", vid=0xCBCB, eeprom_total_size=65536, eeprom_page_size=128)] 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.HEXTEST_HEXPANSION_INDEX = 11 # Index in the HEXPANSION_TYPES list which corresponds to the HexTest type + self.HEXDIAG_HEXPANSION_INDEX = 12 # Index in the HEXPANSION_TYPES list which corresponds to the HexDiag 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] # 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 = [] @@ -286,11 +290,8 @@ def __init__(self): # including timing measurements for the rotation rate measurement feature in the Sensor Test self.hextest_port = None - # GPS hexpansion - #self.hexgps_port = None - # Diagnostics hexpansion - self.hexdiag_port = _DIAG_PORT + self.hexdiag_port = None self._diag_config = None self.hexdiag_setup() @@ -351,6 +352,45 @@ 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: + 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 +418,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 +479,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 +492,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 +568,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 +621,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 +686,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 +743,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 +762,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 +774,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 +846,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 +899,7 @@ def draw(self, ctx): if self.notification: self.notification.draw(ctx) - self.diagnostics_output(2, 0) + diagnostics_output(2, 0) @@ -843,8 +914,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 +977,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 +985,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 +1066,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 +1078,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 +1183,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/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..2720bab 100644 --- a/dev/build_release.py +++ b/dev/build_release.py @@ -2,10 +2,17 @@ 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", @@ -13,10 +20,12 @@ "autotune_mgr", "settings_mgr", "hexpansion_mgr", + "bluetooth_mgr", "line_follow", "motor_moves", "servo_test", "utils", + "diagnostics", "motor_controller", "sensor_manager", "sensor_test", @@ -27,17 +36,21 @@ 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/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/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..10a8504 100644 --- a/dev/dev_requirements.txt +++ b/dev/dev_requirements.txt @@ -1,5 +1,7 @@ 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 diff --git a/dev/download_to_device.py b/dev/download_to_device.py index 0ca1f71..7b8b563 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,22 @@ 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("EEPROM/hexdrive.py"), Path("EEPROM/hexdrive.mpy"), minify=True), + ModuleSpec(Path("vendor/HexDrive2/hexdrive2.py"), Path("EEPROM/hexdrive2.mpy"), minify=True), + ModuleSpec(Path("EEPROM/hextest.py"), Path("EEPROM/hextest.mpy"), minify=True), 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 +60,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 +84,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 +137,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 +219,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 +271,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 +291,7 @@ def _list_mpremote_devices() -> list[str]: def _probe_mpremote_device(port: str) -> bool: command = [ - "mpremote", + _tool("mpremote"), "connect", port, "exec", @@ -305,7 +381,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 +433,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 +496,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 +575,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 +622,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 +700,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..c33e359 --- /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 / "EEPROM" / "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..2281ae4 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"), + ("hextest_port", "HexTest", "HEXTEST_HEXPANSION_INDEX"), + ("hexdiag_port", "HexDiag", "HEXDIAG_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,25 @@ 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 + self._hexpansion_init_type = (self._hexpansion_init_type - 1) % len(app.HEXPANSION_TYPES) 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 - app.refresh = True - - - def _update_state_erase_confirm(self, delta): # pylint: disable=unused-argument + #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 + # app.refresh = True + + + 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 +516,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 +550,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 +559,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 +577,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 +594,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 +612,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"\thextest_port:{app.hextest_port}") + print(f"\thexdiag_port:{app.hexdiag_port}") + print(f"\thexdrive_ports:{app.hexdrive_ports}") + print(f"\thexpansion_update_required = {app.hexpansion_update_required}") + print(f"\tmode = {self._mode}") @@ -656,6 +664,7 @@ 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 @@ -667,11 +676,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 +692,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 +705,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 +717,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 +748,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 +765,36 @@ 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: + unique_id = self._get_header_for_port(port).unique_id if self._get_header_for_port(port) 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 +814,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 +832,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 +850,9 @@ 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") + # left_label=app.HEXPANSION_TYPES[app.HEXDRIVE_HEXPANSION_INDEX].name, \ + # right_label=app.HEXPANSION_TYPES[app.HEXSENSE_HEXPANSION_INDEX].name) return True elif self._sub_state == _SUB_PORT_SELECT: self._draw_port_select(ctx) @@ -845,20 +862,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 +887,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 +945,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 +984,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 +1025,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 +1042,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,37 +1071,32 @@ 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) + # 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(hexpansion_app, "VERSION", getattr(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: @@ -1107,7 +1121,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 +1321,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 +1365,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 + hexpansion_app = self._check_hexpansion_app_on_port(port, type_index) + if 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 +1403,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..1ac6252 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,22 @@ 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): + # If no HexDrive, but a HexSense is present, try its 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 +208,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 +252,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 +347,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 +370,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 +429,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 +469,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 +599,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 +611,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 +644,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 +682,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 +702,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 +717,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 +729,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 +742,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 +758,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 +773,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..344d099 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") 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..370bf86 100644 --- a/tests/conftest.py +++ b/tests/conftest.py @@ -91,7 +91,6 @@ 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 """ @@ -100,10 +99,7 @@ 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 diff --git a/tests/test_opt4048.py b/tests/test_opt4060.py similarity index 74% rename from tests/test_opt4048.py rename to tests/test_opt4060.py index e432c90..dfa9e16 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 @@ -231,14 +231,14 @@ def test_measure_returns_xyz(sensor, opt4048_module): assert result['z'] == '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 +250,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 +283,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 +294,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..6ddc6ef 100644 --- a/tests/test_smoke.py +++ b/tests/test_smoke.py @@ -1,14 +1,21 @@ +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 @@ -19,6 +26,14 @@ def test_import_hexdrive_app_and_app_export(): from sim.apps.BadgeBot.EEPROM.hexdrive import HexDriveApp assert HexDrive.__app_export__ == HexDriveApp +def test_hexdrive_instance_exposes_version(): + from sim.apps.BadgeBot.EEPROM.hexdrive import HexDriveApp + assert getattr(HexDriveApp(), "VERSION", None) == HexDriveApp.VERSION + +def test_gps_instance_exposes_version(): + from sim.apps.BadgeBot.EEPROM.gps import GPSApp + assert getattr(GPSApp(), "VERSION", None) == GPSApp.VERSION + def test_badgebot_app_init(): from sim.apps.BadgeBot import BadgeBotApp BadgeBotApp() @@ -30,9 +45,27 @@ def test_hexdrive_app_init(port): 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.EEPROM.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. @@ -107,8 +140,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(): @@ -135,6 +169,6 @@ def test_all_sensor_classes_populated(): assert len(ALL_SENSOR_CLASSES) >= 5 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 '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..b2498b3 --- /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_mm": "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/HexDrive2 b/vendor/HexDrive2 new file mode 160000 index 0000000..4176da3 --- /dev/null +++ b/vendor/HexDrive2 @@ -0,0 +1 @@ +Subproject commit 4176da3e1bc56f9879ed38fc7908453b9a3d8e81