From 6dc9272a23daf54ca0eb42c06083993ef2aae05f Mon Sep 17 00:00:00 2001 From: lincoltd7 <117526452+lincoltd7@users.noreply.github.com> Date: Mon, 27 Apr 2026 21:29:23 +0100 Subject: [PATCH 01/48] a few UI issues with Hexpansion Manager when plugging and removing hexpansions. --- hexpansion_mgr.py | 39 +++++++++++++++++++++++---------------- 1 file changed, 23 insertions(+), 16 deletions(-) diff --git a/hexpansion_mgr.py b/hexpansion_mgr.py index ddbee5c..fde78e8 100644 --- a/hexpansion_mgr.py +++ b/hexpansion_mgr.py @@ -435,6 +435,7 @@ def _update_state_programming(self, delta): # pylint: disable=unused-argumen 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._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: @@ -512,7 +513,7 @@ 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 @@ -522,7 +523,7 @@ def _update_state_erase(self, delta): # pylint: disable=unused-argument 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}") @@ -532,7 +533,7 @@ def _update_state_erase(self, delta): # pylint: disable=unused-argument 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)}") @@ -553,7 +554,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 @@ -576,7 +577,7 @@ def _update_state_upgrade(self, delta): # pylint: disable=unused-argument 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) + 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() @@ -588,7 +589,7 @@ def _update_state_upgrade(self, delta): # pylint: disable=unused-argument app.button_states.clear() 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: @@ -698,7 +699,6 @@ def _update_state_check(self, delta): # pylint: disable=unused-argument # 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: @@ -728,7 +728,7 @@ def _update_state_check(self, delta): # pylint: disable=unused-argument 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) @@ -797,12 +797,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 @@ -858,7 +858,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, [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: @@ -1352,13 +1353,13 @@ def _check_ports_to_upgrade(self, delta) -> bool: 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") + print("H:Timeout waiting for Hexpansion app to be started - assume it needs installing") 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) + #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: @@ -1367,10 +1368,16 @@ def _check_ports_to_upgrade(self, delta) -> bool: self._hexpansion_state_by_slot[port - 1] = _HEXPANSION_STATE_RECOGNISED_APP_OK self._sub_state = _SUB_CHECK else: - 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}") self._upgrade_port = port - app.notification = Notification("Upgrade?", port=self._upgrade_port) + if hexpansion_app_version == 0: + if self._logging: + print(f"H:Hexpansion [{port}] install {app.HEXPANSION_TYPES[self._hexpansion_type_by_slot[port - 1]].app_mpy_name} app") + self._hexpansion_state_by_slot[port - 1] = _HEXPANSION_STATE_RECOGNISED_NO_APP + else: + 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}") + self._hexpansion_state_by_slot[port - 1] = _HEXPANSION_STATE_RECOGNISED_OLD_APP + #app.notification = Notification("Upgrade?", port=self._upgrade_port) self._sub_state = _SUB_UPGRADE_CONFIRM self._waiting_app_port = None self._hexpansion_app_startup_timer = 0 From 817ffb5f9c0469fe8c86a3946299d72a5e1da7e8 Mon Sep 17 00:00:00 2001 From: lincoltd7 <117526452+lincoltd7@users.noreply.github.com> Date: Tue, 28 Apr 2026 00:06:24 +0100 Subject: [PATCH 02/48] Add encoder smoke test and rotation rate sensor pair functionality Co-authored-by: Copilot --- sensor_test.py | 682 ++++++++++++++++++++++++++++++++++++------------- 1 file changed, 507 insertions(+), 175 deletions(-) diff --git a/sensor_test.py b/sensor_test.py index 733cdc8..5ce2eda 100644 --- a/sensor_test.py +++ b/sensor_test.py @@ -10,6 +10,8 @@ # 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 @@ -56,6 +58,15 @@ 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) @@ -220,6 +231,100 @@ def hextest_setup(self, port: int | None): self._rotation_rate_enable(False) # start with rotation rate emitter and sensors off until we enter motor test mode + def _rotation_rate_sensor_pair(self, pair_index: int = 0) -> tuple[int, int] | None: + """Return the requested HS sensor pin pair from `_ROTATION_RATE_SENSOR_PINS`.""" + start = pair_index * 2 + if start < 0 or start + 1 >= len(_ROTATION_RATE_SENSOR_PINS): + return None + return _ROTATION_RATE_SENSOR_PINS[start], _ROTATION_RATE_SENSOR_PINS[start + 1] + + + def encoder_smoke_test( + self, + samples: int = 12, + interval_ms: int = 250, + filter_ns: int = 1_000_000, + max: int | None = 3, + min: int = 0, + ) -> bool: + """Run a short console-based encoder smoke test on the first HexTest sensor pair.""" + if samples <= 0: + print("S:Encoder smoke test requires at least one sample") + return False + if interval_ms < 0: + print("S:Encoder smoke test requires interval_ms >= 0") + return False + if self._sub_state == _SUB_MOTOR_TEST: + print("S:Encoder smoke test unavailable while motor test mode is active") + return False + + config = self._test_support_hexpansion_config + if config is None: + print("S:Encoder smoke test requires a HexTest Hexpansion") + return False + + hs_pair = self._rotation_rate_sensor_pair(0) + if hs_pair is None: + print("S:Encoder smoke test requires at least one HS sensor pin pair") + return False + + gpios = _HS_PIN_TO_GPIO.get(config.port) + if gpios is None: + print(f"S:Encoder smoke test does not know the GPIO mapping for port {config.port}") + return False + + phase_a_pin, phase_b_pin = hs_pair + phase_a_gpio = gpios[phase_a_pin] + phase_b_gpio = gpios[phase_b_pin] + range_desc = "hardware range" if max is None else f"min={min}, max={max}" + + self._rotation_rate_enable(True) + encoder = Encoder( + None, + phase_a_gpio, + phase_b_gpio, + filter_ns=filter_ns, + max=max, + min=min, + logging=True, + ) + if encoder.unit is None: + print( + f"S:Encoder smoke test failed on HexTest port {config.port} " + f"HS pins {phase_a_pin}/{phase_b_pin}" + ) + self._rotation_rate_enable(False) + return False + + print( + f"S:Encoder smoke test on HexTest port {config.port}, " + f"HS pins {phase_a_pin}/{phase_b_pin}, GPIOs {phase_a_gpio}/{phase_b_gpio}, {range_desc}" + ) + print("S:Rotate the wheel by hand and watch position/cycles for direction and wrap behaviour") + + try: + print(f"S:Encoder initial: position={encoder.value()}, cycles={encoder.cycles()}") + for sample_index in range(samples): + _sleep_ms(interval_ms) + print( + f"S:Encoder sample {sample_index + 1}/{samples}: " + f"position={encoder.value()}, cycles={encoder.cycles()}" + ) + + final_position = encoder.value() + final_cycles = encoder.cycles() + encoder.value(0) + print( + f"S:Encoder reset after position={final_position}, cycles={final_cycles}; " + f"now position={encoder.value()}, cycles={encoder.cycles()}" + ) + print("S:Encoder smoke test complete") + return True + finally: + encoder.deinit() + self._rotation_rate_enable(False) + + # ------------------------------------------------------------------ # Entry point from menu # ------------------------------------------------------------------ @@ -893,7 +998,11 @@ def _start_motor_test_mode(self) -> bool: # 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 + counter = None + if True: + self.encoder_smoke_test() + else: + 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: @@ -1151,7 +1260,6 @@ def _draw_reading(self, ctx): # 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 = 33 @@ -1176,225 +1284,449 @@ def _draw_reading(self, ctx): for _idx, _gpio in enumerate(_gpios): _GPIO_TO_HS[_gpio] = (_port, _idx) -class Counter: - """Wrapper around ESP32-S3 PCNT hardware for counting rising edges. +_PCNT_UNIT_STRIDE = const(0x0C) +_PCNT_CONF1_OFFSET = const(0x04) +_PCNT_CONF2_OFFSET = const(0x08) +_PCNT_CNT_OFFSET = const(0x30) - 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 - """ +_CONF0_CH0_NEG_MODE_S = const(16) +_CONF0_CH0_POS_MODE_S = const(18) +_CONF0_CH0_HCTRL_MODE_S = const(20) +_CONF0_CH0_LCTRL_MODE_S = const(22) +_CONF0_CH1_NEG_MODE_S = const(24) +_CONF0_CH1_POS_MODE_S = const(26) +_CONF0_CH1_HCTRL_MODE_S = const(28) +_CONF0_CH1_LCTRL_MODE_S = const(30) - def __init__(self, unit: int | None, src: int, filter_ns: int = 0, logging: bool = False): +_PCNT_COUNT_DISABLE = const(0) +_PCNT_COUNT_INCREMENT = const(1) +_PCNT_COUNT_DECREMENT = const(2) + +_PCNT_CTRL_KEEP = const(0) +_PCNT_CTRL_REVERSE = const(1) +_PCNT_CTRL_HOLD = const(2) + +_PCNT_GPIO_CONST_HIGH = const(0x38) +_PCNT_COUNTER_MASK = const(0xFFFF) +_PCNT_COUNTER_SIGN_BIT = const(0x8000) +_PCNT_COUNTER_MODULO = const(0x10000) +_PCNT_COUNTER_MAX = const(0x7FFF) +_PCNT_DEFAULT_MAX = const(0x7FFF) + + +def _pcnt_conf0_addr(unit: int) -> 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}") 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 From f185519249840057c5a3651810770127c85292d4 Mon Sep 17 00:00:00 2001 From: lincoltd7 <117526452+lincoltd7@users.noreply.github.com> Date: Tue, 28 Apr 2026 00:08:23 +0100 Subject: [PATCH 03/48] make dev download more robust for where things are --- dev/dev_requirements.txt | 1 + dev/download_to_device.py | 86 +++++++++++++++++++++++++++++++++++---- download.bat | 19 ++++++++- download.sh | 17 +++++++- 4 files changed, 114 insertions(+), 9 deletions(-) diff --git a/dev/dev_requirements.txt b/dev/dev_requirements.txt index b1a9b1b..7870b62 100644 --- a/dev/dev_requirements.txt +++ b/dev/dev_requirements.txt @@ -1,5 +1,6 @@ pylint isort pytest +mpremote mpy-cross micropython-esp32-stubs==1.27.0.post1 \ No newline at end of file diff --git a/dev/download_to_device.py b/dev/download_to_device.py index 0ca1f71..a5975d6 100644 --- a/dev/download_to_device.py +++ b/dev/download_to_device.py @@ -16,7 +16,9 @@ import json import os import re +import shutil import subprocess +import sys from dataclasses import dataclass from pathlib import Path @@ -77,6 +79,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 +132,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 +214,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 +266,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 +286,7 @@ def _list_mpremote_devices() -> list[str]: def _probe_mpremote_device(port: str) -> bool: command = [ - "mpremote", + _tool("mpremote"), "connect", port, "exec", @@ -305,7 +376,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, ) @@ -359,7 +430,7 @@ def _compile_changed_modules( _log("INFO", f"compile {spec.source} -> {spec.artifact}") _run_command( - ["mpy-cross", "-v", str(spec.source), "-o", str(spec.artifact)], + [_tool("mpy-cross"), "-v", str(spec.source), "-o", str(spec.artifact)], dry_run=dry_run, ) @@ -411,7 +482,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 +561,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 +608,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 +686,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/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 From 510f7c2891ab02a425d71276b747be6244214c5c Mon Sep 17 00:00:00 2001 From: robotmad Date: Thu, 30 Apr 2026 01:57:42 +0100 Subject: [PATCH 04/48] Refactor version retrieval logic in HexpansionMgr for improved clarity and reliability --- hexpansion_mgr.py | 59 +++++++++++++++++++++++++++++------------------ 1 file changed, 37 insertions(+), 22 deletions(-) diff --git a/hexpansion_mgr.py b/hexpansion_mgr.py index fde78e8..ea4ba02 100644 --- a/hexpansion_mgr.py +++ b/hexpansion_mgr.py @@ -925,15 +925,11 @@ def _draw_port_select(self, ctx): # 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() + 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)) @@ -1064,22 +1060,17 @@ def _check_hexpansion_app_on_port(self, port: int, type_index: int) -> object | 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: + # Read version from VERSION (module-level constant, preferred) or + # lowercase version attribute as a fallback for older apps. + 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._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}") + 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: @@ -1387,6 +1378,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. From 54db4fac76d456af861cbbab0c9bd9f01736237c Mon Sep 17 00:00:00 2001 From: robotmad Date: Thu, 30 Apr 2026 01:57:50 +0100 Subject: [PATCH 05/48] Remove unused get_version method from GPSApp to streamline code --- EEPROM/gps.py | 5 ----- 1 file changed, 5 deletions(-) diff --git a/EEPROM/gps.py b/EEPROM/gps.py index ee09d4b..e92b292 100644 --- a/EEPROM/gps.py +++ b/EEPROM/gps.py @@ -68,11 +68,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: From e06f22204062db86025106b6fb480277e62bef92 Mon Sep 17 00:00:00 2001 From: robotmad Date: Thu, 30 Apr 2026 01:57:57 +0100 Subject: [PATCH 06/48] Remove unused get_version method from HexDriveApp to simplify code --- EEPROM/hexdrive.py | 5 ----- 1 file changed, 5 deletions(-) diff --git a/EEPROM/hexdrive.py b/EEPROM/hexdrive.py index 7cb0a83..8bf906f 100644 --- a/EEPROM/hexdrive.py +++ b/EEPROM/hexdrive.py @@ -176,11 +176,6 @@ def background_update(self, delta: int): # we keep retriggering in case anything else has corrupted the PWM outputs - def get_version(self) -> int: - """ Get the version of the app - this is used to determine if an upgrade is required. """ - return VERSION - - def get_status(self) -> bool: """ Get the current status of the app - True if the app is running and able to respond to commands, False if not. """ return (self._pwm_setup) From 549c52b201a3c33ffb799b0a5b5e0413af6a995b Mon Sep 17 00:00:00 2001 From: robotmad Date: Thu, 30 Apr 2026 23:08:46 +0100 Subject: [PATCH 07/48] colour sensor calibration, use hexpansion EEPROM VERSION, Switch to OPT406 for HexDrive2, align on badge directory to match app store naming, Co-authored-by: Copilot --- EEPROM/hexdrive.py | 54 +++--- app.py | 31 ++-- dev/build_release.py | 4 +- dev/download_to_device.py | 4 +- hexpansion_mgr.py | 114 ++++++------- motor_moves.py | 32 ++-- sensor_manager.py | 15 +- sensor_test.py | 184 ++++++++++++++++++--- sensors/__init__.py | 6 +- sensors/opt4048.py | 7 +- sensors/opt4060.py | 11 +- sensors/sensor_base.py | 4 + tests/conftest.py | 6 +- tests/{test_opt4048.py => test_opt4060.py} | 116 ++++++------- tests/test_smoke.py | 30 +++- 15 files changed, 389 insertions(+), 229 deletions(-) rename tests/{test_opt4048.py => test_opt4060.py} (74%) diff --git a/EEPROM/hexdrive.py b/EEPROM/hexdrive.py index 8bf906f..5405f29 100644 --- a/EEPROM/hexdrive.py +++ b/EEPROM/hexdrive.py @@ -12,13 +12,15 @@ import app -# HexDrive.py App Version - used to check if upgrade is required -VERSION = 7 - # HexDrive Hexpansion constants # Hardware defintions: _ENABLE_PIN = 0 # First LS pin used to enable the SMPSU -#_DETECT_PIN = 1 # Second LS pin used to sense if the SMPSU has a source of power + +# Hardware Version 2 +_COLOUR_INT_PIN = 1 # Second LS pin used to detect interrupts from the colour sensor to trigger readings without polling +_LED_PIN = 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 = 3 # Fourth LS pin used to detect interrupts from the distance sensor to trigger readings without polling +_DIST_XSHUT_PIN = 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 # 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 @@ -38,29 +40,30 @@ _EEPROM_NUM_ADDRESS_BYTES = 2 # Number of bytes used for the memory address when reading from the EEPROM (e.g. 2 for 16-bit addressing) _PID_ADDR = 0x12 # Address in the EEPROM where the Product ID (PID) byte is stored - used to identify the type of Hexpansion -# PID MSByte value for RobotMad HexDrive Hexpansion modules (used to identify the type of HexDrive when reading the EEPROM) -_HEXDRIVE_PID_MSB = 0xCB - class HexDriveType: """Represents a sub-type of HexDrive Hexpansion module.""" - __slots__ = ("pid", "name", "motors", "servos") + __slots__ = ("pid", "name", "motors", "servos", "hw_ver") def __init__(self, pid_byte: int, motors: int = 0, servos: int = 0, name: str = "Unknown"): self.pid: int = pid_byte # Product ID byte read from the EEPROM to identify the type of HexDrive self.name: str = name # A friendly name for the type of HexDrive self.motors: int = motors # Number of motor channels supported by this type of HexDrive (0, 1 or 2) self.servos: int = servos # Number of servo channels supported by this type of HexDrive (0, 2 or 4) + self.hw_ver: int = 0 # Hardware version of this type of HexDrive _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(0xCE, motors=1, name="1 Motor"), ) class HexDriveApp(app.App): # pylint: disable=no-member """ HexDrive Hexpansion App for BadgeBot.""" + VERSION = 6 # 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 @@ -75,11 +78,12 @@ def __init__(self, config: HexpansionConfig | None = None): 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") + #print("D:No Config") return # LS Pins - #self._power_detect = self.config.ls_pin[_DETECT_PIN] self._power_control = self.config.ls_pin[_ENABLE_PIN] + self._led_control = self.config.ls_pin[_LED_PIN] + self._dist_xshut = self.config.ls_pin[_DIST_XSHUT_PIN] self._servo_centre = [_SERVO_CENTRE] * _MAX_NUM_CHANNELS eventbus.on_async(RequestStopAppEvent, self._handle_stop_app, self) @@ -104,29 +108,34 @@ def initialise(self) -> bool: self._pwm_setup = False if self.config is None: return False + + # read hexpansion header from EEPROM to find out which sub-type we are + self._hexdrive_type = self._check_port_for_hexdrive(self.config.port) + if self._hexdrive_type is None: + print(f"D:{self.config.port}:Unknown HexDrive type - initialisation failed") + 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}") + print(f"D:HexDrive{'2' if 2 == self._hexdrive_type.hw_ver else ''} Type:'{self._hexdrive_type.name}' V{self.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) + self._led_control.init(mode=Pin.OUT) + self._dist_xshut.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}'") - + # allocate PWM outputs according to the type of HexDrive return self._pwm_init() @@ -134,6 +143,8 @@ 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._led_control.deinit() + self._dist_xshut.deinit() self._pwm_deinit() for hs_pin in self.config.pin: hs_pin.init(mode=Pin.IN) @@ -488,12 +499,7 @@ def _check_port_for_hexdrive(self, port: int) -> HexDriveType | None: # 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 + # 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: return hexpansion_type diff --git a/app.py b/app.py index f2bd957..7cdd195 100644 --- a/app.py +++ b/app.py @@ -28,8 +28,8 @@ #micropython.alloc_emergency_exception_buf(100) from .utils import draw_logo_animated, parse_version -from .EEPROM.hexdrive import VERSION as HEXDRIVE_APP_VERSION +HEXDRIVE_APP_VERSION = 6 _SETTINGS_NAME_PREFIX = "badgebot." # Prefix for settings keys in EEPROM APP_VERSION = "1.5" # BadgeBot App Version Number @@ -249,31 +249,28 @@ def __init__(self): 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(0x10CB, "HexDrive2", 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(0x10CA, "HexDrive2", 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(0x10CC, "HexDrive2", 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(0x11CE, "HexDrive2", 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=1, sub_type="Left Motor" ), + HexpansionType(0x12CE, "HexDrive2", 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="Right Motor" ), + 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), + HexpansionType(0x4000, "HexDiag", vid=0xCBCB, eeprom_total_size=65536, eeprom_page_size=128), HexpansionType(0x0000, "Unknown", sub_type=""), # Virtual type to represent unrecognised hexpansions HexpansionType(0xFFFF, "Blank", sub_type="")] # Virtual type to represent blank EEPROMs 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.HEXDRIVE_V2_HEXPANSION_INDEX = 4 # Index in the HEXPANSION_TYPES list which corresponds to the basic HexDrive2 type + self.HEXSENSE_HEXPANSION_INDEX = 9 # Index in the HEXPANSION_TYPES list which corresponds to the HexSense type + self.HEXTEST_HEXPANSION_INDEX = 10 # Index in the HEXPANSION_TYPES list which corresponds to the HexTest type + self.HEXDIAG_HEXPANSION_INDEX = 11 # Index in the HEXPANSION_TYPES list which corresponds to the HexDiag 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.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 = [] diff --git a/dev/build_release.py b/dev/build_release.py index ee239df..f70b79a 100644 --- a/dev/build_release.py +++ b/dev/build_release.py @@ -31,7 +31,7 @@ "sensors/tcs3472", "sensors/vl53l0x", "sensors/vl6180x", - "sensors/opt4048", + "sensors/opt4060", "sensors/ina226", } @@ -101,7 +101,7 @@ def find_files(top_level_dir): 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/download_to_device.py b/dev/download_to_device.py index 0ca1f71..b308873 100644 --- a/dev/download_to_device.py +++ b/dev/download_to_device.py @@ -21,7 +21,7 @@ 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 @@ -57,7 +57,7 @@ class ModuleSpec: 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/opt4060.py"), Path("sensors/opt4060.mpy")), ModuleSpec(Path("sensors/ina226.py"), Path("sensors/ina226.mpy")), ) diff --git a/hexpansion_mgr.py b/hexpansion_mgr.py index ea4ba02..6029bec 100644 --- a/hexpansion_mgr.py +++ b/hexpansion_mgr.py @@ -390,9 +390,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 @@ -577,6 +577,8 @@ def _update_state_upgrade(self, delta): # pylint: disable=unused-argument app = self._app upgrade_port = self._upgrade_port if upgrade_port is None: + 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"]): @@ -587,7 +589,7 @@ 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_INIT if self._mode == _MODE_INIT else _SUB_CHECK @@ -614,7 +616,6 @@ def _report_hexpansion_states(self): 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}") @@ -668,9 +669,10 @@ def _update_state_check(self, delta): # pylint: disable=unused-argument self._report_hexpansion_states() + # For hexpansiosn 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): # check if there is a hexpansion of a type that can be a HexDrive on this port @@ -683,20 +685,6 @@ 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 @@ -710,7 +698,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,8 +710,9 @@ 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: @@ -751,7 +741,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: @@ -1055,13 +1045,14 @@ def _check_port_for_known_hexpansions(self, port) -> bool: - 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: - # Read version from VERSION (module-level constant, preferred) or - # lowercase version attribute as a fallback for older apps. + # 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 @@ -1069,12 +1060,13 @@ def _check_hexpansion_app_on_port(self, port: int, type_index: int) -> object | # 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._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) + 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: @@ -1332,44 +1324,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 installing") - 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}] upgrade to {app.HEXPANSION_TYPES[type_index].app_mpy_version}?") self._upgrade_port = port - if hexpansion_app_version == 0: - if self._logging: - print(f"H:Hexpansion [{port}] install {app.HEXPANSION_TYPES[self._hexpansion_type_by_slot[port - 1]].app_mpy_name} app") - self._hexpansion_state_by_slot[port - 1] = _HEXPANSION_STATE_RECOGNISED_NO_APP - else: - 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}") - self._hexpansion_state_by_slot[port - 1] = _HEXPANSION_STATE_RECOGNISED_OLD_APP - #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 diff --git a/motor_moves.py b/motor_moves.py index 4c46c49..00b4f69 100644 --- a/motor_moves.py +++ b/motor_moves.py @@ -246,6 +246,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) @@ -359,22 +361,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: diff --git a/sensor_manager.py b/sensor_manager.py index 25f0488..9285822 100644 --- a/sensor_manager.py +++ b/sensor_manager.py @@ -16,9 +16,9 @@ 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. +_INTERRUPT_PIN = 1 # Not currently used, but we can set it up as an input for future interrupt-based drivers class SensorManager: @@ -111,9 +111,9 @@ 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) @@ -121,14 +121,13 @@ def open(self, port: int) -> bool: 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 + print(f"[{self._port}] INT pin value: {v}") def close(self): diff --git a/sensor_test.py b/sensor_test.py index 733cdc8..0810d84 100644 --- a/sensor_test.py +++ b/sensor_test.py @@ -14,6 +14,7 @@ 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 try: from egpio import ePin except ImportError: @@ -84,12 +85,17 @@ def enable_irq(_state: int) -> None: _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 = [ @@ -132,6 +138,7 @@ def __init__(self, app, hextest_port: int | None = _ROTATION_RATE_PORT, logging: self._display_data: dict = {} self._page_selected: int = _PAGE_RAW self._page_count: int = 3 + self._white_gains: dict[tuple[int, int], tuple[int, int, int, int]] = {} self._logging: bool = logging self._read_timer: int = 0 # ms since last sensor read self._sample_count: int = 0 @@ -410,6 +417,22 @@ 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, clear: int = 0, + white_gains: tuple[int, int, int, int] | None = None) -> tuple[int, int, int, int]: + if white_gains is None: + return r, g, b, clear + + gain_r, gain_g, gain_b, gain_clear = white_gains + + return ( + max(0, ((r * gain_r) + (_WHITE_CAL_SCALE // 2)) // _WHITE_CAL_SCALE), + max(0, ((g * gain_g) + (_WHITE_CAL_SCALE // 2)) // _WHITE_CAL_SCALE), + max(0, ((b * gain_b) + (_WHITE_CAL_SCALE // 2)) // _WHITE_CAL_SCALE), + max(0, ((clear * gain_clear) + (_WHITE_CAL_SCALE // 2)) // _WHITE_CAL_SCALE) if clear > 0 else 0, + ) + + # ------------------------------------------------------------------ # Background update (called from the fast loop) # ------------------------------------------------------------------ @@ -735,9 +758,112 @@ def _update_select_port(self, delta: int): # pylint: disable=unused-argument app.return_to_menu() + @staticmethod + def _ordered_display_data(sensor_data: dict) -> dict: + ordered = {} + for key in ("r", "g", "b"): + if key in sensor_data: + ordered[key] = str(sensor_data[key]) + for key, value in sensor_data.items(): + if key not in ordered: + ordered[key] = str(value) + return ordered + + @staticmethod + def _ordered_display_items(display_data: dict) -> list[tuple[str, str]]: + 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 + + def _sensor_reference_key(self) -> tuple[int, int] | None: + sensor_mgr = self._sensor_mgr + if sensor_mgr is None: + return None + return (self._port_selected, sensor_mgr.current_sensor_index) + + @staticmethod + def _white_gain_setting_keys(reference_key: tuple[int, int]) -> tuple[str, str, str, str]: + port, sensor_index = reference_key + base = f"{_WHITE_CAL_GAIN_PREFIX}{port}{sensor_index}" + return (f"{base}r", f"{base}g", f"{base}b", f"{base}w") + + @staticmethod + def _reference_to_gains(r: int, g: int, b: int, clear: 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_clear = max(int(clear), 1) if clear > 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_clear // 2)) // ref_clear, + ) + + def _get_white_gains(self) -> tuple[int, int, int, int] | None: + key = self._sensor_reference_key() + if key is None: + return None + if key in self._white_gains: + return self._white_gains[key] + + setting_keys = self._white_gain_setting_keys(key) + values = [] + for setting_key in setting_keys: + value = platform_settings.get(f"badgebot.{setting_key}", None) + if value is None: + return None + values.append(int(value)) + + gains = (values[0], values[1], values[2], values[3]) + self._white_gains[key] = gains + 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 + + key = self._sensor_reference_key() + if key is None: + 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)), + ) + self._white_gains[key] = gains + setting_keys = self._white_gain_setting_keys(key) + for setting_key, gain in zip(setting_keys, gains): + platform_settings.set(f"badgebot.{setting_key}", gain) + if self._logging: + print(f"S:Stored white gains for port {key[0]} sensor {key[1]}: {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 +893,11 @@ 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._ordered_display_data(self._sensor_data) + elif self._page_selected == _PAGE_CAL: + self._display_data["mode"] = "XYZ sensor" + self._display_data["ref"] = "N/A" + self._display_data["press"] = "Use Raw/Data" #convert CIE1931 XYZ to RGB using a simple matrix transform r = int( 3.2406 * x - 1.5372 * y - 0.4986 * z) @@ -779,21 +909,28 @@ def _update_display_values(self): # pylint: disable=unused-argument print(f"S: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"]) + clear = int(self._sensor_data.get("w", 0)) + white_gains = self._get_white_gains() + calibrated_r, calibrated_g, calibrated_b, calibrated_clear = self._apply_white_reference( + r, g, b, clear, 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_clear) self._display_data["colour"] = colour_name + if white_gains is not None: + self._display_data["cal"] = "white ref" elif self._page_selected == _PAGE_RAW: - self._display_data = {k: str(v) for k, v in self._sensor_data.items()} + self._display_data = self._ordered_display_data(self._sensor_data) + elif self._page_selected == _PAGE_CAL: + self._display_data["ref"] = "saved" if white_gains is not None else "none" + self._display_data["hold"] = "white card" + self._display_data["press"] = "CONFIRM" except Exception as e: # pylint: disable=broad-exception-caught print(f"S:Colour conversion error: {e}") @@ -827,7 +964,7 @@ def _update_display_values(self): # pylint: disable=unused-argument except Exception as e: # pylint: disable=broad-exception-caught print(f"S:Distance processing error: {e}") elif self._page_selected == _PAGE_RAW: - self._display_data = {k: str(v) for k, v in self._sensor_data.items()} + self._display_data = self._ordered_display_data(self._sensor_data) if self._page_selected == _PAGE_STATS: if self._sample_rate > 0: @@ -867,6 +1004,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 @@ -1070,7 +1212,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,7 +1228,7 @@ 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] else: @@ -1101,12 +1243,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=" bool: 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}") + print(f"PCNT: unit {unit} - IN USE: CONF0=0x{conf0:08X}, count={cnt}, routed to GPIO {routed_gpio}") return True @@ -1347,10 +1490,7 @@ def init(self, src: int, filter_ns: int | None = None) -> bool: 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 OK, CONF0=0x{mem32[conf0_addr]:08X}, CTRL=0x{mem32[_PCNT_CTRL_REG]:08X}, CNT={mem32[cnt_addr] & 0xFFFF}") return True 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/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..883eb00 100644 --- a/sensors/opt4060.py +++ b/sensors/opt4060.py @@ -279,6 +279,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 READ_INTERVAL_MS deadline = time.ticks_add(time.ticks_ms(), self.READ_INTERVAL_MS) while True: @@ -298,10 +301,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..706edeb 100644 --- a/sensors/sensor_base.py +++ b/sensors/sensor_base.py @@ -108,9 +108,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/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_smoke.py b/tests/test_smoke.py index 7094fb0..95c23d1 100644 --- a/tests/test_smoke.py +++ b/tests/test_smoke.py @@ -3,7 +3,7 @@ import pytest # Add badge software to pythonpath -sys.path.append("../../../") +sys.path.append("../../../") import sim.run from system.hexpansion.config import HexpansionConfig @@ -19,6 +19,29 @@ 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(): + import sim.apps.BadgeBot.EEPROM.hexdrive as HexDrive + from sim.apps.BadgeBot.EEPROM.hexdrive import HexDriveApp + assert getattr(HexDriveApp(), "VERSION", None) == HexDrive.VERSION + +def test_gps_instance_exposes_version(): + import sim.apps.BadgeBot.EEPROM.gps as GPS + from sim.apps.BadgeBot.EEPROM.gps import GPSApp + assert getattr(GPSApp(), "VERSION", None) == GPS.VERSION + +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_badgebot_app_init(): from sim.apps.BadgeBot import BadgeBotApp BadgeBotApp() @@ -32,7 +55,6 @@ 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. def test_hexdrive_type_pids_consistent(): """Verify HexDriveType PIDs in hexdrive.py are consistent with HexpansionType PIDs in app.py. @@ -135,6 +157,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 From e7066a9eebe4ca48554e794cb1dfb564943ccf98 Mon Sep 17 00:00:00 2001 From: robotmad Date: Fri, 1 May 2026 00:21:33 +0100 Subject: [PATCH 08/48] working driver Co-authored-by: Copilot --- sensor_test.py | 2 + sensors/vl53l0x.py | 251 ++++++++++++++++++++++++++++++++------ tests/test_sensor_test.py | 41 +++++++ tests/test_smoke.py | 32 ++--- tests/test_vl53l0x.py | 105 ++++++++++++++++ 5 files changed, 369 insertions(+), 62 deletions(-) create mode 100644 tests/test_sensor_test.py create mode 100644 tests/test_vl53l0x.py diff --git a/sensor_test.py b/sensor_test.py index 0810d84..8ece73e 100644 --- a/sensor_test.py +++ b/sensor_test.py @@ -963,6 +963,8 @@ def _update_display_values(self): # pylint: disable=unused-argument self._display_data["Distance"] = distance_str except Exception as e: # pylint: disable=broad-exception-caught print(f"S:Distance processing error: {e}") + elif self._page_selected == _PAGE_RAW: + self._display_data = self._ordered_display_data(self._sensor_data) elif self._page_selected == _PAGE_RAW: self._display_data = self._ordered_display_data(self._sensor_data) diff --git a/sensors/vl53l0x.py b/sensors/vl53l0x.py index 1da8f32..7e88e17 100644 --- a/sensors/vl53l0x.py +++ b/sensors/vl53l0x.py @@ -18,71 +18,246 @@ _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), +) + + +def _ticks_ms() -> int: + ticks_ms = getattr(time, "ticks_ms", None) + if ticks_ms is not None: + return ticks_ms() + return int(getattr(time, "monotonic")() * 1000) + + +def _ticks_add(base: int, delta: int) -> int: + if hasattr(time, "ticks_add"): + return time.ticks_add(base, delta) + return base + delta + + +def _ticks_diff(finish: int, now: int) -> int: + if hasattr(time, "ticks_diff"): + return time.ticks_diff(finish, now) + return finish - now + + +def _sleep_ms(delay_ms: int): + sleep_ms = getattr(time, "sleep_ms", None) + if sleep_ms is not None: + sleep_ms(delay_ms) + return + getattr(time, "sleep")(delay_ms / 1000) + class VL53L0X(SensorBase): 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})") 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) - # 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 - self._write_u8(_SYSTEM_INTERRUPT_CLEAR, 0x01) + 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 + + 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)) - # Disable MSRC and TCC - self._write_u8(_MSRC_CONFIG_CONTROL, 0x12) + 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) - # 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 + 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 - if time.ticks_diff(deadline, time.ticks_ms()) <= 0: + deadline = _ticks_add(_ticks_ms(), _RANGE_TIMEOUT_MS) + while self._read_u8(_SYSRANGE_START) & 0x01: + if _ticks_diff(deadline, _ticks_ms()) <= 0: return {"dist_mm": "timeout"} - time.sleep_ms(1) + _sleep_ms(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) + + print(f"S:VL53L0X measured {dist_mm} mm") + + self._write_u8(_SYSTEM_INTERRUPT_CLEAR, 0x01) + + return {"dist_mm": f"{dist_mm}"} + + def _open_stop_variable_window(self): + self._write_u8(0x80, 0x01) + self._write_u8(0xFF, 0x01) + self._write_u8(0x00, 0x00) - # 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] + 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 = _ticks_add(_ticks_ms(), _RANGE_TIMEOUT_MS) + while (self._read_u8(_RESULT_INTERRUPT_STATUS) & _INTERRUPT_READY_MASK) == 0: + if _ticks_diff(deadline, _ticks_ms()) <= 0: + return False + _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 = _ticks_add(_ticks_ms(), _RANGE_TIMEOUT_MS) + while self._read_u8(_SPAD_POLL_REG) == 0x00: + if _ticks_diff(deadline, _ticks_ms()) <= 0: + return None + _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/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 95c23d1..7e0b274 100644 --- a/tests/test_smoke.py +++ b/tests/test_smoke.py @@ -1,11 +1,9 @@ import sys -import pytest - # Add badge software to pythonpath sys.path.append("../../../") -import sim.run +import sim.run as _sim_run from system.hexpansion.config import HexpansionConfig @@ -20,27 +18,12 @@ def test_import_hexdrive_app_and_app_export(): assert HexDrive.__app_export__ == HexDriveApp def test_hexdrive_instance_exposes_version(): - import sim.apps.BadgeBot.EEPROM.hexdrive as HexDrive from sim.apps.BadgeBot.EEPROM.hexdrive import HexDriveApp - assert getattr(HexDriveApp(), "VERSION", None) == HexDrive.VERSION + assert getattr(HexDriveApp(), "VERSION", None) == HexDriveApp.VERSION def test_gps_instance_exposes_version(): - import sim.apps.BadgeBot.EEPROM.gps as GPS from sim.apps.BadgeBot.EEPROM.gps import GPSApp - assert getattr(GPSApp(), "VERSION", None) == GPS.VERSION - -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) + assert getattr(GPSApp(), "VERSION", None) == GPSApp.VERSION def test_badgebot_app_init(): from sim.apps.BadgeBot import BadgeBotApp @@ -53,8 +36,8 @@ 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 + from sim.apps.BadgeBot.EEPROM.hexdrive import HexDriveApp + assert BadgeBot.HEXDRIVE_APP_VERSION == HexDriveApp.VERSION def test_hexdrive_type_pids_consistent(): """Verify HexDriveType PIDs in hexdrive.py are consistent with HexpansionType PIDs in app.py. @@ -129,8 +112,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(): 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"} From 3f77c1764159a9c57e7cfb15d4642d69b5f84cdf Mon Sep 17 00:00:00 2001 From: robotmad Date: Fri, 1 May 2026 21:38:35 +0100 Subject: [PATCH 09/48] wider use of diagnostics outputs, simplify white gains --- app.py | 50 ++++++++++++---------- dev/build_release.py | 7 +-- dev/download_to_device.py | 7 +-- diagnostics.py | 15 +++++++ hexpansion_mgr.py | 1 - sensor_manager.py | 16 ++++--- sensor_test.py | 90 +++++++++++++++------------------------ sensors/sensor_base.py | 3 +- sensors/vl53l0x.py | 11 +++-- settings_mgr.py | 7 +-- 10 files changed, 110 insertions(+), 97 deletions(-) create mode 100644 diagnostics.py diff --git a/app.py b/app.py index 7cdd195..7d389ae 100644 --- a/app.py +++ b/app.py @@ -31,11 +31,9 @@ HEXDRIVE_APP_VERSION = 6 -_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 = [ @@ -151,14 +149,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.""" @@ -283,11 +281,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() @@ -428,9 +423,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 @@ -508,7 +503,7 @@ 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}") @@ -553,7 +548,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) @@ -618,7 +613,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) @@ -772,7 +767,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 @@ -825,7 +820,7 @@ def draw(self, ctx): if self.notification: self.notification.draw(ctx) - self.diagnostics_output(2, 0) + diagnostics_output(2, 0) @@ -1109,5 +1104,16 @@ def _menu_back_handler(self): # for submenus, just return to the main menu 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/dev/build_release.py b/dev/build_release.py index f70b79a..9b75e04 100644 --- a/dev/build_release.py +++ b/dev/build_release.py @@ -17,6 +17,7 @@ "motor_moves", "servo_test", "utils", + "diagnostics", "motor_controller", "sensor_manager", "sensor_test", @@ -27,10 +28,10 @@ SENSOR_MODULES = { "sensors/__init__", "sensors/sensor_base", - "sensors/tcs3430", - "sensors/tcs3472", + #"sensors/tcs3430", + #"sensors/tcs3472", "sensors/vl53l0x", - "sensors/vl6180x", + #"sensors/vl6180x", "sensors/opt4060", "sensors/ina226", } diff --git a/dev/download_to_device.py b/dev/download_to_device.py index b308873..dc792cd 100644 --- a/dev/download_to_device.py +++ b/dev/download_to_device.py @@ -42,6 +42,7 @@ class ModuleSpec: 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("line_follow.py"), Path("line_follow.mpy")), @@ -53,10 +54,10 @@ 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/vl6180x.py"), Path("sensors/vl6180x.mpy")), ModuleSpec(Path("sensors/opt4060.py"), Path("sensors/opt4060.mpy")), ModuleSpec(Path("sensors/ina226.py"), Path("sensors/ina226.mpy")), ) 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/hexpansion_mgr.py b/hexpansion_mgr.py index 6029bec..58bcf30 100644 --- a/hexpansion_mgr.py +++ b/hexpansion_mgr.py @@ -90,7 +90,6 @@ ("hexsense_port", "HexSense", "HEXSENSE_HEXPANSION_INDEX"), ("hextest_port", "HexTest", "HEXTEST_HEXPANSION_INDEX"), ("hexdiag_port", "HexDiag", "HEXDIAG_HEXPANSION_INDEX"), - #("hexgps_port", "HexGPS", "HEXGPS_HEXPANSION_INDEX"), ) # ---- Settings initialisation ----------------------------------------------- diff --git a/sensor_manager.py b/sensor_manager.py index 9285822..5298d85 100644 --- a/sensor_manager.py +++ b/sensor_manager.py @@ -18,8 +18,8 @@ _LED_PIN = 2 # LED to illumiinate area under colour sensor to measure reflected light from surface below. -_INTERRUPT_PIN = 1 # Not currently used, but we can set it up as an input for future interrupt-based drivers - +_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): @@ -87,7 +87,7 @@ def open(self, port: int) -> bool: if address not in found_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): @@ -116,7 +116,8 @@ def open(self, port: int) -> bool: 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 @@ -126,8 +127,10 @@ def report_interrupt(self): if self._port is None: return False config = HexpansionConfig(self._port) - v = config.ls_pin[_INTERRUPT_PIN].value() - print(f"[{self._port}] INT pin value: {v}") + 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): @@ -181,6 +184,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 diff --git a/sensor_test.py b/sensor_test.py index 8ece73e..3a9d262 100644 --- a/sensor_test.py +++ b/sensor_test.py @@ -21,7 +21,8 @@ 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 +from .app import SETTINGS_NAME_PREFIX, DEFAULT_BACKGROUND_UPDATE_PERIOD, MOTOR_PWM_FREQ + try: from machine import Pin, mem32, disable_irq, enable_irq except ImportError: @@ -138,14 +139,14 @@ def __init__(self, app, hextest_port: int | None = _ROTATION_RATE_PORT, logging: self._display_data: dict = {} self._page_selected: int = _PAGE_RAW self._page_count: int = 3 - self._white_gains: dict[tuple[int, int], tuple[int, int, int, int]] = {} self._logging: bool = logging 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 - self._colour: tuple = (1.0, 1.0, 0.0) # default to yellow for non-colour sensors + 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._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 @@ -418,18 +419,16 @@ def lookup_colour_RGB(r: int, g: int, b: int, clear: int = 0) -> str: #pylint @staticmethod - def _apply_white_reference(r: int, g: int, b: int, clear: int = 0, - white_gains: tuple[int, int, int, int] | None = None) -> tuple[int, int, int, int]: + 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]: if white_gains is None: - return r, g, b, clear - - gain_r, gain_g, gain_b, gain_clear = white_gains + return (r, g, b, w) + """Apply white reference gains to raw RGBC values and return adjusted RGBC tuple.""" return ( - max(0, ((r * gain_r) + (_WHITE_CAL_SCALE // 2)) // _WHITE_CAL_SCALE), - max(0, ((g * gain_g) + (_WHITE_CAL_SCALE // 2)) // _WHITE_CAL_SCALE), - max(0, ((b * gain_b) + (_WHITE_CAL_SCALE // 2)) // _WHITE_CAL_SCALE), - max(0, ((clear * gain_clear) + (_WHITE_CAL_SCALE // 2)) // _WHITE_CAL_SCALE) if clear > 0 else 0, + 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, ) @@ -710,8 +709,6 @@ def _update_motor_test_mode(self, delta: int): # pylint: disable=unused-argumen print(f"S:Motor-Power: {self._rotation_rate_motor_power}") - - def _update_select_port(self, delta: int): # pylint: disable=unused-argument app = self._app if app.button_states.get(BUTTON_TYPES["RIGHT"]): @@ -745,6 +742,11 @@ def _update_select_port(self, delta: int): # pylint: disable=unused-argument 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") + # 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"S:Loaded white gains from settings: {self._white_gains}") self._sub_state = _SUB_READING else: app.notification = Notification(" No Sensors", port=self._port_selected) @@ -782,49 +784,34 @@ def _ordered_display_items(display_data: dict) -> list[tuple[str, str]]: items.append((key, str(value))) return items - def _sensor_reference_key(self) -> tuple[int, int] | None: - sensor_mgr = self._sensor_mgr - if sensor_mgr is None: - return None - return (self._port_selected, sensor_mgr.current_sensor_index) - @staticmethod - def _white_gain_setting_keys(reference_key: tuple[int, int]) -> tuple[str, str, str, str]: - port, sensor_index = reference_key - base = f"{_WHITE_CAL_GAIN_PREFIX}{port}{sensor_index}" + 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, clear: int = 0) -> tuple[int, int, int, int]: + 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_clear = max(int(clear), 1) if clear > 0 else _WHITE_CAL_SCALE + 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_clear // 2)) // ref_clear, + (gain_scale + (ref_w // 2)) // ref_w, ) - def _get_white_gains(self) -> tuple[int, int, int, int] | None: - key = self._sensor_reference_key() - if key is None: - return None - if key in self._white_gains: - return self._white_gains[key] - + 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"badgebot.{setting_key}", None) + 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]) - self._white_gains[key] = gains return gains def _update_page_count(self) -> None: @@ -840,22 +827,21 @@ def _capture_white_reference(self) -> bool: if not all(key in self._sensor_data for key in ("r", "g", "b")): return False - key = self._sensor_reference_key() - if key is None: - 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)), ) - self._white_gains[key] = gains - setting_keys = self._white_gain_setting_keys(key) + # 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"badgebot.{setting_key}", gain) + platform_settings.set(f"{SETTINGS_NAME_PREFIX}.{setting_key}", gain) if self._logging: - print(f"S:Stored white gains for port {key[0]} sensor {key[1]}: {gains}") + print(f"S:Stored white gains: {gains}") self._app.notification = Notification("White Cal Saved", port=self._port_selected) return True @@ -896,7 +882,7 @@ def _update_display_values(self): # pylint: disable=unused-argument self._display_data = self._ordered_display_data(self._sensor_data) elif self._page_selected == _PAGE_CAL: self._display_data["mode"] = "XYZ sensor" - self._display_data["ref"] = "N/A" + #self._display_data["ref"] = "N/A" self._display_data["press"] = "Use Raw/Data" #convert CIE1931 XYZ to RGB using a simple matrix transform @@ -914,22 +900,16 @@ def _update_display_values(self): # pylint: disable=unused-argument r = int(self._sensor_data["r"]) g = int(self._sensor_data["g"]) b = int(self._sensor_data["b"]) - clear = int(self._sensor_data.get("w", 0)) - white_gains = self._get_white_gains() - calibrated_r, calibrated_g, calibrated_b, calibrated_clear = self._apply_white_reference( - r, g, b, clear, white_gains - ) + 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: - colour_name = self.lookup_colour_RGB(calibrated_r, calibrated_g, calibrated_b, calibrated_clear) + colour_name = self.lookup_colour_RGB(calibrated_r, calibrated_g, calibrated_b, calibrated_w) self._display_data["colour"] = colour_name - if white_gains is not None: - self._display_data["cal"] = "white ref" elif self._page_selected == _PAGE_RAW: self._display_data = self._ordered_display_data(self._sensor_data) elif self._page_selected == _PAGE_CAL: - self._display_data["ref"] = "saved" if white_gains is not None else "none" - self._display_data["hold"] = "white card" + self._display_data["ref"] = "white" self._display_data["press"] = "CONFIRM" except Exception as e: # pylint: disable=broad-exception-caught diff --git a/sensors/sensor_base.py b/sensors/sensor_base.py index 706edeb..8df1bca 100644 --- a/sensors/sensor_base.py +++ b/sensors/sensor_base.py @@ -20,10 +20,11 @@ class SensorBase: 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) diff --git a/sensors/vl53l0x.py b/sensors/vl53l0x.py index 7e88e17..9abeaac 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 @@ -109,7 +110,8 @@ def __init__(self, i2c_addr: int | None = None): def _init(self) -> bool: 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 # The VL53L0X needs a substantial startup sequence before single-shot @@ -176,6 +178,7 @@ def _init(self) -> bool: return True def _measure(self) -> dict: + diagnostics_output(1,0) self._prepare_single_shot() self._write_u8(_SYSRANGE_START, 0x01) @@ -192,11 +195,13 @@ def _measure(self) -> dict: # ST's register map; this offset matches the reference driver. dist_mm = self._read_u16_be(_RESULT_RANGE_STATUS + 10) - print(f"S:VL53L0X measured {dist_mm} mm") + if self._logging: + print(f"S:VL53L0X measured {dist_mm} mm") self._write_u8(_SYSTEM_INTERRUPT_CLEAR, 0x01) + diagnostics_output(1,1) - return {"dist_mm": f"{dist_mm}"} + return {"dist": f"{dist_mm}"} def _open_stop_variable_window(self): self._write_u8(0x80, 0x01) diff --git a/settings_mgr.py b/settings_mgr.py index fb9f441..7fab96b 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" @@ -103,9 +104,9 @@ 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.""" try: if self.v != self.d: - platform_settings.set(f"badgebot.{self._index()}", self.v) + platform_settings.set(f"{SETTINGS_NAME_PREFIX}.{self._index()}", self.v) else: - platform_settings.set(f"badgebot.{self._index()}", None) + platform_settings.set(f"{SETTINGS_NAME_PREFIX}.{self._index()}", None) except Exception as e: # pylint: disable=broad-except print(f"H:Failed to persist setting {self._index()}: {e}") @@ -133,7 +134,7 @@ 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 From f36498d73bbf4fabfdb1bb7b8f475e31053a68ef Mon Sep 17 00:00:00 2001 From: robotmad Date: Sat, 2 May 2026 08:32:24 +0100 Subject: [PATCH 10/48] hexpansion management improvements - cope with hexpansiosn that don't have config as an attribute. --- app.py | 20 +++++++-------- hexpansion_mgr.py | 63 ++++++++++++++++++++++++++++++----------------- 2 files changed, 50 insertions(+), 33 deletions(-) diff --git a/app.py b/app.py index 7d389ae..7b85702 100644 --- a/app.py +++ b/app.py @@ -243,16 +243,16 @@ 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(0x10CB, "HexDrive2", 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(0x10CA, "HexDrive2", 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(0x10CC, "HexDrive2", 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(0x11CE, "HexDrive2", 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=1, sub_type="Left Motor" ), - HexpansionType(0x12CE, "HexDrive2", 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="Right Motor" ), - HexpansionType(0x2000, "HexSense", vid=0xCBCB, eeprom_total_size=65536, eeprom_page_size=128, sensors=2, sub_type="2 Line Sensors"), + 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(0x10CB, "HexDrive2", vid=0xCBCB, eeprom_total_size=32768, eeprom_page_size= 64, app_mpy_name="hexdrive", app_mpy_version=HEXDRIVE_APP_VERSION, app_name="HexDriveApp", motors=2, servos=2, sub_type="Uncommitted" ), + HexpansionType(0x10CA, "HexDrive2", vid=0xCBCB, eeprom_total_size=32768, eeprom_page_size= 64, app_mpy_name="hexdrive", app_mpy_version=HEXDRIVE_APP_VERSION, app_name="HexDriveApp", motors=2, sub_type="2 Motor" ), + HexpansionType(0x10CC, "HexDrive2", vid=0xCBCB, eeprom_total_size=32768, eeprom_page_size= 64, app_mpy_name="hexdrive", app_mpy_version=HEXDRIVE_APP_VERSION, app_name="HexDriveApp", servos=2, sub_type="2 Servo" ), + HexpansionType(0x11CE, "HexDrive2", vid=0xCBCB, eeprom_total_size=32768, eeprom_page_size= 64, app_mpy_name="hexdrive", app_mpy_version=HEXDRIVE_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="hexdrive", app_mpy_version=HEXDRIVE_APP_VERSION, app_name="HexDriveApp", motors=2, sub_type="Right Motor" ), + 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), HexpansionType(0x4000, "HexDiag", vid=0xCBCB, eeprom_total_size=65536, eeprom_page_size=128), HexpansionType(0x0000, "Unknown", sub_type=""), # Virtual type to represent unrecognised hexpansions diff --git a/hexpansion_mgr.py b/hexpansion_mgr.py index 58bcf30..4b3d3df 100644 --- a/hexpansion_mgr.py +++ b/hexpansion_mgr.py @@ -231,23 +231,26 @@ def _should_claim_single_port_hexpansion(self, type_index: int) -> bool: async def _handle_removal(self, event): 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): @@ -526,6 +529,8 @@ def _update_state_erase(self, delta): # pylint: disable=unused-argument return if self._logging: print(f"H:Erasing EEPROM on port {erase_port}") + if self._hexpansion_type_by_slot[erase_port - 1] is not None: + self._hexpansion_init_type = self._hexpansion_type_by_slot[erase_port - 1] 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] @@ -913,9 +918,11 @@ 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: - ver = getattr(running_app, "VERSION", - getattr(running_app, "version", None)) + 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)) @@ -1052,8 +1059,7 @@ def _check_hexpansion_app_on_port(self, port: int, type_index: int, ) -> object # 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)) + 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. @@ -1090,7 +1096,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: @@ -1290,11 +1296,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 # ------------------------------------------------------------------ From dc37d2c142c948fe11ee4500e38550c5d338fe81 Mon Sep 17 00:00:00 2001 From: robotmad Date: Sat, 2 May 2026 23:35:11 +0100 Subject: [PATCH 11/48] small mods --- EEPROM/gps.py | 4 ++-- hexpansion_mgr.py | 17 +++++++++-------- 2 files changed, 11 insertions(+), 10 deletions(-) diff --git a/EEPROM/gps.py b/EEPROM/gps.py index e92b292..5f44a22 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 @@ -28,6 +26,8 @@ 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 diff --git a/hexpansion_mgr.py b/hexpansion_mgr.py index 4b3d3df..da7534b 100644 --- a/hexpansion_mgr.py +++ b/hexpansion_mgr.py @@ -611,19 +611,20 @@ def _report_hexpansion_states(self): if not self._logging: return app = self._app + print("H:Current Hexpansion States:") for port in range(0, _NUM_HEXPANSION_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"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}") From cc02f4c6266ab87b590850b11095eb9a2f3ad506 Mon Sep 17 00:00:00 2001 From: robotmad Date: Sun, 3 May 2026 15:58:27 +0100 Subject: [PATCH 12/48] turn off print output --- EEPROM/gps.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/EEPROM/gps.py b/EEPROM/gps.py index 5f44a22..7ea1dd6 100644 --- a/EEPROM/gps.py +++ b/EEPROM/gps.py @@ -20,6 +20,9 @@ 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 From 7e8e1a113bb96c06025d8605e5523768b0536085 Mon Sep 17 00:00:00 2001 From: robotmad Date: Sun, 3 May 2026 22:05:17 +0100 Subject: [PATCH 13/48] tidy up to remove dummy hexpansion types now that we have explicit per slot hexpansion state tracking. --- hexpansion_mgr.py | 31 +++++++++++++------------------ 1 file changed, 13 insertions(+), 18 deletions(-) diff --git a/hexpansion_mgr.py b/hexpansion_mgr.py index da7534b..50321ab 100644 --- a/hexpansion_mgr.py +++ b/hexpansion_mgr.py @@ -306,23 +306,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 <= _NUM_HEXPANSION_SLOTS else None + if state_idx is not None: + if state_idx == self.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 >= self.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 @@ -483,11 +481,11 @@ 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() @@ -548,7 +546,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") @@ -1016,7 +1013,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 @@ -1046,7 +1042,6 @@ 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 From 584f68f96879553250fb03ede3335710d6ac80b2 Mon Sep 17 00:00:00 2001 From: robotmad Date: Sun, 3 May 2026 22:05:58 +0100 Subject: [PATCH 14/48] app part of removign dummy hexpansion types --- app.py | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/app.py b/app.py index 7b85702..0d80f75 100644 --- a/app.py +++ b/app.py @@ -254,9 +254,7 @@ def __init__(self): HexpansionType(0x12CE, "HexDrive2", vid=0xCBCB, eeprom_total_size=32768, eeprom_page_size= 64, app_mpy_name="hexdrive", app_mpy_version=HEXDRIVE_APP_VERSION, app_name="HexDriveApp", motors=2, sub_type="Right Motor" ), 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), - HexpansionType(0x4000, "HexDiag", vid=0xCBCB, eeprom_total_size=65536, eeprom_page_size=128), - HexpansionType(0x0000, "Unknown", sub_type=""), # Virtual type to represent unrecognised hexpansions - HexpansionType(0xFFFF, "Blank", sub_type="")] # Virtual type to represent blank EEPROMs + 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 = 4 # Index in the HEXPANSION_TYPES list which corresponds to the basic HexDrive2 type @@ -264,8 +262,6 @@ def __init__(self): self.HEXTEST_HEXPANSION_INDEX = 10 # Index in the HEXPANSION_TYPES list which corresponds to the HexTest type self.HEXDIAG_HEXPANSION_INDEX = 11 # Index in the HEXPANSION_TYPES list which corresponds to the HexDiag 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.hexpansion_update_required: bool = False # flag from async to main loop 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 From 00bc81e52be9e12049f51cd4a51b244144509cd4 Mon Sep 17 00:00:00 2001 From: robotmad Date: Tue, 5 May 2026 01:24:01 +0100 Subject: [PATCH 15/48] updates with real HexDrive2 s Co-authored-by: Copilot --- .vscode/settings.json | 27 +---- EEPROM/hexdrive.py | 240 ++++++++++++++++++++++++---------------- app.py | 23 ++-- hexpansion_mgr.py | 75 +++++++------ motor_moves.py | 2 +- pyrightconfig.json | 9 ++ sensor_manager.py | 10 +- sensor_test.py | 245 +++++++++++++++++++++-------------------- sensors/ina226.py | 21 ++-- sensors/sensor_base.py | 4 + 10 files changed, 361 insertions(+), 295 deletions(-) create mode 100644 pyrightconfig.json 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/hexdrive.py b/EEPROM/hexdrive.py index 5405f29..892c134 100644 --- a/EEPROM/hexdrive.py +++ b/EEPROM/hexdrive.py @@ -9,9 +9,12 @@ from system.eventbus import eventbus from system.hexpansion.config import HexpansionConfig from system.scheduler.events import RequestStopAppEvent - +from tildagon import Pin as ePin import app +# 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 @@ -43,32 +46,50 @@ class HexDriveType: """Represents a sub-type of HexDrive Hexpansion module.""" - __slots__ = ("pid", "name", "motors", "servos", "hw_ver") + __slots__ = ("pid", "name", "motors", "servos", "hw_ver", "servo_pin_map") - def __init__(self, pid_byte: int, motors: int = 0, servos: int = 0, name: str = "Unknown"): + def __init__(self, pid_byte: int, motors: int = 0, servos: int = 0, name: str = "Unknown", 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.hw_ver: int = 0 # Hardware version of this type of HexDrive + 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(0xCE, motors=1, name="1 Motor"), + HexDriveType(0xCB, motors=2, servos=4, servo_pins=(0, 1, 2, 3)), # uncommitted version can be used for anything + HexDriveType(0xCC, servos=4, name="4 Servo", servo_pins=(0, 1, 2, 3)), + HexDriveType(0xCD, motors=1, servos=2, name="1 Mot 2 Srvo", servo_pins=(2, 3, -1, -1)), + HexDriveType(0xCE, motors=1, name="1 Motor", servo_pins=(-1, -1, -1, -1)), + HexDriveType(0xCF, motors=1, servos=1, name="1 Mot 1 Srvo", servo_pins=(2, -1, -1, -1)), ) + class HexDriveApp(app.App): # pylint: disable=no-member """ HexDrive Hexpansion App for BadgeBot.""" - VERSION = 6 # Increment this when making changes to the app that require the hexpansion app to be re-flashed with the new code. + 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 + + # read hexpansion header from EEPROM 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{'2' if 2 == _hexdrive_type.hw_ver else ''} Type:'{_hexdrive_type.name}' V{self.VERSION} by RobotMad on port {self.config.port}") + + self._hexdrive_type: HexDriveType = _hexdrive_type + self._hw_ver: int = _hexdrive_type.hw_ver + 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 @@ -76,47 +97,37 @@ 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_control = self.config.ls_pin[_ENABLE_PIN] - self._led_control = self.config.ls_pin[_LED_PIN] - self._dist_xshut = self.config.ls_pin[_DIST_XSHUT_PIN] + self._power_control: ePin = self.config.ls_pin[_ENABLE_PIN] + if self._hw_ver >= 1: + self._led_control: ePin = self.config.ls_pin[_LED_PIN] + self._dist_xshut: ePin = self.config.ls_pin[_DIST_XSHUT_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 + if ver >= _MIN_BADGEOS_VERSION: pass else: - print("D:BadgeOS Upgrade to v1.9.0+ required") + print("D:BadgeOS Upgrade 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 initialisation 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 - - # read hexpansion header from EEPROM to find out which sub-type we are - self._hexdrive_type = self._check_port_for_hexdrive(self.config.port) - if self._hexdrive_type is None: - print(f"D:{self.config.port}:Unknown HexDrive type - initialisation failed") - return False - - # report app starting and which port it is running on - print(f"D:HexDrive{'2' if 2 == self._hexdrive_type.hw_ver else ''} Type:'{self._hexdrive_type.name}' V{self.VERSION} by RobotMad on port {self.config.port}") # Initialise HS Pins for _, hs_pin in enumerate(self.config.pin): @@ -127,13 +138,15 @@ def initialise(self) -> bool: # Initialise LS Pins try: self._power_control.init(mode=Pin.OUT) - self._led_control.init(mode=Pin.OUT) - self._dist_xshut.init(mode=Pin.OUT) + if self._hw_ver >= 1: + self._led_control.init(mode=Pin.OUT) + self._dist_xshut.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) + self.power = False # allocate PWM outputs according to the type of HexDrive return self._pwm_init() @@ -142,12 +155,13 @@ def initialise(self) -> bool: 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._led_control.deinit() - self._dist_xshut.deinit() + self.power = False self._pwm_deinit() for hs_pin in self.config.pin: hs_pin.init(mode=Pin.IN) + if self._hw_ver >= 1: + self._led_control.deinit() + self._dist_xshut.deinit() return True @@ -164,7 +178,7 @@ async def _handle_stop_app(self, event): 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: # 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 @@ -197,15 +211,20 @@ def set_logging(self, state: bool): self._logging = state - def set_power(self, state: bool) -> bool: - """ Turn the SMPSU on or off. Returns the new power state. - Note that just because the SMPSU is turned off does not mean that the outputs are NOT energised as there could be external battery power. """ - if (self.config is None) or (state == self._power_state): + + + @property + def power(self) -> bool: + """ Get the current state of the SMPSU enable pin. Returns True if enabled, False if disabled. """ + return self._power_state + + @power.setter + def power(self, state: bool) -> bool: + """ Turn the SMPSU on or off. Returns success or failure. """ + if state == self._power_state: return False if self._logging: print(f"D:{self.config.port}:Power={'On' if state else 'Off'}") - #if self.get_booster_power(): - # if the power detect pin is high then the SMPSU has a power source so enable it try: self._power_control.init(mode=Pin.OUT) self._power_control.value(state) @@ -213,28 +232,47 @@ def set_power(self, state: bool) -> bool: 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 + @property + def keep_alive_period(self) -> int: + """ Get the current keep alive period in milliseconds. """ + return self._keep_alive_period - def set_keep_alive(self, period: int): + @keep_alive_period.setter + def keep_alive_period(self, period: int): """ Set the keep alive period in milliseconds: This is the period of time that can elapse without any commands being received before the app automatically turns off all outputs to prevent damage to motors or servos if something goes wrong. """ self._keep_alive_period = period - def set_freq(self, freq: int, channel: int | None = None) -> bool: + 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 + self._freq[channel] = freq + if not servo: + self._freq[channel<<1] = freq # two physical channels per motor channel + # map from logical channel to physical channel for servos and motors (only one physical channel in use in both cases) + physical_channel = self._servo_pin_map[channel] if servo else (channel << 1) + (self._motor_output[channel] > 0) + 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: + if (physical_channel is None or (this_channel == physical_channel)) and pwm is not None: try: pwm.freq(freq) if self._logging: @@ -243,7 +281,6 @@ def set_freq(self, freq: int, channel: int | None = None) -> bool: print(self._pwm_log_string(this_channel) + f"set freq {freq} failed {e}") print(f"pwm: {pwm}") return False - self._freq[this_channel] = freq return True @@ -261,14 +298,12 @@ 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 @@ -280,8 +315,12 @@ def set_servoposition(self, channel: int | None = None, position: int | None = N elif channel < 0 or channel >= self._hexdrive_type.servos: return False else: + channel = self._servo_pin_map[channel] + pwm = self.PWMOutput[channel] + if pwm is None: + return False try: - self.PWMOutput[channel].duty_ns(0) + pwm.duty_ns(0) if self._logging: print(self._pwm_log_string(channel) + "Off") except Exception as e: # pylint: disable=broad-except @@ -294,41 +333,45 @@ def set_servoposition(self, channel: int | None = None, position: int | None = N 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 + self._freq[physical_channel] = self._freq[physical_channel] if (0 < self._freq[physical_channel]) and (self._freq[physical_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) + self.PWMOutput[physical_channel] = PWM(self.config.pin[physical_channel], freq = self._freq[physical_channel], duty_ns = pulse_width_in_ns) if self._logging: - print(self._pwm_log_string(channel) + f"{self.PWMOutput[channel]} init") + 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) + self._freq[physical_channel] = _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 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(channel) + f"{pulse_width_in_ns}ns") - self.PWMOutput[channel].duty_ns(pulse_width_in_ns) + 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(channel) + f"{self.PWMOutput[channel]} duty") + 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 @@ -360,7 +403,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 not self._pwm_setup or len(outputs) > self._hexdrive_type.motors: return False for motor, output in enumerate(outputs): if abs(output) > 65535: @@ -374,10 +417,11 @@ def set_motors(self, outputs: tuple[int, ...]) -> bool: output_to_enable = (motor<<1) if output > 0 else ((motor<<1)+1) output_to_disable = (motor<<1)+1 if output > 0 else (motor<<1) # switch off the currently active output before switching the other one on to prevent both outputs being on at the same time - if self.PWMOutput[output_to_disable] is not None: + 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._set_pwmoutput(output_to_enable, abs(output)) @@ -390,17 +434,17 @@ def set_motors(self, outputs: tuple[int, ...]) -> bool: # Set all 4 PWM duty cycles in one go using a tuple (0-65535) - def set_pwm(self, duty_cycles: tuple[int, ...]) -> bool: - """ Set the PWM duty cycle for all outputs at once using a tuple of values. Returns True if successful, False if failed. - The duty_cycles are values from 0 to 65535. """ - if not self._pwm_setup: - return False - self._outputs_energised = any(duty_cycles) - for channel, duty_cycle in enumerate(duty_cycles): - if not self._set_pwmoutput(channel, duty_cycle): - return False - self._time_since_last_update = 0 - return True + #def set_pwm(self, duty_cycles: tuple[int, ...]) -> bool: + # """ Set the PWM duty cycle for all outputs at once using a tuple of values. Returns True if successful, False if failed. + # The duty_cycles are values from 0 to 65535. """ + # if not self._pwm_setup: + # return False + # self._outputs_energised = any(duty_cycles) + # for channel, duty_cycle in enumerate(duty_cycles): + # if not self._set_pwmoutput(channel, duty_cycle): + # return False + # self._time_since_last_update = 0 + # return True # -------------------------------------------------- @@ -426,7 +470,7 @@ def _pwm_init(self) -> bool: # 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 + # Remaining channels are for servos (can be 4, 2, 1 or 0 servos self._freq[channel] = _DEFAULT_SERVO_FREQ #print(f"D:{self.config.port}:Servo PWM[{channel}]") else: @@ -472,17 +516,21 @@ def _check_outputs_energised(self): # Set a single PWM duty cycle (0-65535) for a specific output # if the channel has not been setup yet then we initialise it from scratch, otherwise we just change the duty cycle - def _set_pwmoutput(self, channel: int, duty_cycle: int) -> bool: + def _set_pwmoutput(self, channel: int, duty_cycle: int, servo: bool = False) -> bool: if duty_cycle < 0 or duty_cycle > 65535: return False try: if self.PWMOutput[channel] is None: # Channel hasn't been setup yet so we need to initialise it from scratch - self.PWMOutput[channel] = PWM(self.config.pin[channel], freq = self._freq[channel], duty_u16 = duty_cycle) + pin = self.config.pin[self._servo_pin_map[channel]] if servo else self.config.pin[channel] + self.PWMOutput[channel] = PWM(pin, 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) + 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 diff --git a/app.py b/app.py index 0d80f75..018d3b9 100644 --- a/app.py +++ b/app.py @@ -29,7 +29,7 @@ from .utils import draw_logo_animated, parse_version -HEXDRIVE_APP_VERSION = 6 +HEXDRIVE_APP_VERSION = 7 SETTINGS_NAME_PREFIX = "badgebot." # Prefix for settings keys in EEPROM APP_VERSION = "1.5" # BadgeBot App Version Number @@ -247,20 +247,23 @@ def __init__(self): 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(0x10CB, "HexDrive2", vid=0xCBCB, eeprom_total_size=32768, eeprom_page_size= 64, app_mpy_name="hexdrive", app_mpy_version=HEXDRIVE_APP_VERSION, app_name="HexDriveApp", motors=2, servos=2, sub_type="Uncommitted" ), + + HexpansionType(0x10C8, "HexDrive2", vid=0xCBCB, eeprom_total_size=32768, eeprom_page_size= 64, app_mpy_name="hexdrive", app_mpy_version=HEXDRIVE_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="hexdrive", app_mpy_version=HEXDRIVE_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="hexdrive", app_mpy_version=HEXDRIVE_APP_VERSION, app_name="HexDriveApp", motors=2, sub_type="2 Motor" ), - HexpansionType(0x10CC, "HexDrive2", vid=0xCBCB, eeprom_total_size=32768, eeprom_page_size= 64, app_mpy_name="hexdrive", app_mpy_version=HEXDRIVE_APP_VERSION, app_name="HexDriveApp", servos=2, sub_type="2 Servo" ), HexpansionType(0x11CE, "HexDrive2", vid=0xCBCB, eeprom_total_size=32768, eeprom_page_size= 64, app_mpy_name="hexdrive", app_mpy_version=HEXDRIVE_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="hexdrive", app_mpy_version=HEXDRIVE_APP_VERSION, app_name="HexDriveApp", motors=2, sub_type="Right Motor" ), - HexpansionType(0x2000, "HexSense", vid=0xCBCB, eeprom_total_size=65536, eeprom_page_size=128, sensors=2, sub_type="2 Line Sensors"), + HexpansionType(0x12CE, "HexDrive2", vid=0xCBCB, eeprom_total_size=32768, eeprom_page_size= 64, app_mpy_name="hexdrive", app_mpy_version=HEXDRIVE_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="hexdrive", app_mpy_version=HEXDRIVE_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), 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 = 4 # Index in the HEXPANSION_TYPES list which corresponds to the basic HexDrive2 type - self.HEXSENSE_HEXPANSION_INDEX = 9 # Index in the HEXPANSION_TYPES list which corresponds to the HexSense type - self.HEXTEST_HEXPANSION_INDEX = 10 # Index in the HEXPANSION_TYPES list which corresponds to the HexTest type - self.HEXDIAG_HEXPANSION_INDEX = 11 # Index in the HEXPANSION_TYPES list which corresponds to the HexDiag 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 @@ -435,7 +438,9 @@ def background_update(self, delta: int): 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)) + 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 diff --git a/hexpansion_mgr.py b/hexpansion_mgr.py index 50321ab..58fbcd6 100644 --- a/hexpansion_mgr.py +++ b/hexpansion_mgr.py @@ -86,10 +86,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"), + ("hextest_port", "HexTest", "HEXTEST_HEXPANSION_INDEX"), + ("hexdiag_port", "HexDiag", "HEXDIAG_HEXPANSION_INDEX"), ) # ---- Settings initialisation ----------------------------------------------- @@ -179,7 +181,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: @@ -193,6 +194,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 @@ -202,6 +204,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 @@ -229,7 +232,7 @@ 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 port = event.port self._hexpansion_type_by_slot[port - 1] = None @@ -253,7 +256,7 @@ async def _handle_removal(self, event): print(f"H:Hexpansion removed from port {port}") - async def _handle_insertion(self, event): + 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 @@ -308,11 +311,11 @@ 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.""" state_idx = self._hexpansion_state_by_slot[self._port_selected - 1] if 1 <= self._port_selected <= _NUM_HEXPANSION_SLOTS else None if state_idx is not None: - if state_idx == self.HEXPANSION_STATE_UNRECOGNISED: + 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 >= self.HEXPANSION_STATE_RECOGNISED: + 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 @@ -328,7 +331,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 @@ -404,7 +407,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 +432,14 @@ 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 @@ -467,7 +472,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"]): @@ -487,19 +492,19 @@ def _update_state_detected(self, delta): # pylint: disable=unused-argumen app.button_states.clear() 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 @@ -516,7 +521,7 @@ def _update_state_erase_confirm(self, delta): # pylint: disable=unused-arg 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 @@ -527,8 +532,9 @@ def _update_state_erase(self, delta): # pylint: disable=unused-argument return if self._logging: print(f"H:Erasing EEPROM on port {erase_port}") - if self._hexpansion_type_by_slot[erase_port - 1] is not None: - self._hexpansion_init_type = self._hexpansion_type_by_slot[erase_port - 1] + 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] @@ -573,7 +579,7 @@ 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 @@ -595,7 +601,7 @@ def _update_state_upgrade(self, delta): # pylint: disable=unused-argument 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): if self._hexpansion_type_by_slot[port] == hexpansion_type: @@ -660,6 +666,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 @@ -671,7 +678,7 @@ def _update_state_check(self, delta): # pylint: disable=unused-argument self._report_hexpansion_states() - # For hexpansiosn of which we only need to know where one is we track movements between ports and update the assigned port accordingly + # 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 - to allow for more than one being present and used: @@ -825,9 +832,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) diff --git a/motor_moves.py b/motor_moves.py index 00b4f69..b9a6595 100644 --- a/motor_moves.py +++ b/motor_moves.py @@ -318,7 +318,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 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 5298d85..795a139 100644 --- a/sensor_manager.py +++ b/sensor_manager.py @@ -100,7 +100,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') @@ -184,7 +184,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() + #self.report_interrupt() return self._last_data @@ -213,18 +213,18 @@ def last_data(self) -> dict: return self._last_data @property - def port(self): + def port(self) -> str | 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 3a9d262..53cdbed 100644 --- a/sensor_test.py +++ b/sensor_test.py @@ -68,8 +68,7 @@ def enable_irq(_state: int) -> None: _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 @@ -130,7 +129,7 @@ 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, hextest_port: int | None = None, logging: bool = False): self._app = app self._sub_state = _SUB_SELECT_PORT self._sensor_mgr = None # SensorManager instance (lazy-imported) @@ -172,7 +171,7 @@ def __init__(self, app, hextest_port: int | None = _ROTATION_RATE_PORT, logging: 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_sum_power_mw: int = 0 self._ina226_sample_count: int = 0 # Use HS pins on a spare Hexpansion to measure rotation rate @@ -241,7 +240,7 @@ 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 + self._colour = (1.0, 1.0, 0.0) # reset to yellow when starting sensor test # If a HexDrive is present, try its port first if app.hexdrive_ports is not None: for port in app.hexdrive_ports: @@ -420,10 +419,9 @@ def lookup_colour_RGB(r: int, g: int, b: int, clear: int = 0) -> str: #pylint @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) - - """Apply white reference gains to raw RGBC values and return adjusted RGBC tuple.""" 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), @@ -445,7 +443,7 @@ def background_update(self, delta) -> tuple[int, int] | None: # pylint: disable # 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 @@ -524,43 +522,45 @@ def _rotation_rate_enable(self, enable: bool = True) -> bool: 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): + # 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, 7): + if not mgr.open(port): + mgr.close() + if self._logging: + print(f"ST: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"ST:INA226 found @ 0x{sensor.i2c_addr:02X} on port {port}") + return True + # No INA226 found; close the manager 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}") + print(f"ST: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_sum_power_mw = 0 self._ina226_sample_count = 0 + def _sample_ina226_in_background(self) -> None: sensor = self._ina226 if sensor is None: @@ -569,25 +569,26 @@ def _sample_ina226_in_background(self) -> None: 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_sum_current_ma += int(data.get("mA", 0)) + self._ina226_sum_bus_mv += int(data.get("mV", 0)) + #self._ina226_sum_power_mw += int(data.get("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}") + print(f"ST: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 = { - "current_mA": current_ma, - "bus_mV": self._ina226_sum_bus_mv // count, - "power_mW": self._ina226_sum_power_mw // count, + "mA": current_ma, + "mV": voltage_mv, } self._reset_ina226_accumulators() return current_ma @@ -668,6 +669,8 @@ def _update_motor_test_mode(self, delta: int): # pylint: disable=unused-argumen if current_abs > self._auto_max_current_ma: self._auto_max_current_ma = current_abs power = self._rotation_rate_motor_power + if self._logging: + print(f"ST:Auto Scan Step {self._auto_step}/{_AUTO_SCAN_STEPS} - Power: {power}, Rate: {rate} rpm, Current: {current_ma}mA") self._auto_results.append((power, rate, current_ma)) self._auto_rotation_rate_step() # In auto mode, no manual button control for power/IR @@ -684,29 +687,55 @@ def _update_motor_test_mode(self, delta: int): # pylint: disable=unused-argumen self._rotation_rate_measurement_period_elapsed = 0 self._consume_ina226_average() if self.logging: - print(f"S:Rotation Rates: {self._rotation_rate_rpms}") + print(f"ST: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}") + print(f"ST: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}") + print(f"ST: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}") + print(f"ST: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}") + print(f"ST:Motor-Power: {self._rotation_rate_motor_power}") + + + def _setup_for_sensor_type(self): + sensor_mgr = self._sensor_mgr + if sensor_mgr is None: + return + + 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 @@ -722,31 +751,19 @@ def _update_select_port(self, delta: int): # pylint: disable=unused-argument 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 + if self._port_selected == motor_test_port: + if self._start_motor_test_mode(): + app.notification = Notification("Motor Test", port=self._port_selected) + if self.logging: + print(f"ST:Entering Motor Test mode on port {self._port_selected}") + self._sub_state = _SUB_MOTOR_TEST + app.refresh = True 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") - # 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"S:Loaded white gains from settings: {self._white_gains}") + + self._setup_for_sensor_type() self._sub_state = _SUB_READING else: app.notification = Notification(" No Sensors", port=self._port_selected) @@ -760,19 +777,9 @@ def _update_select_port(self, delta: int): # pylint: disable=unused-argument app.return_to_menu() - @staticmethod - def _ordered_display_data(sensor_data: dict) -> dict: - ordered = {} - for key in ("r", "g", "b"): - if key in sensor_data: - ordered[key] = str(sensor_data[key]) - for key, value in sensor_data.items(): - if key not in ordered: - ordered[key] = str(value) - return ordered - @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"): @@ -784,11 +791,13 @@ def _ordered_display_items(display_data: dict) -> list[tuple[str, str]]: 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) @@ -803,6 +812,7 @@ def _reference_to_gains(r: int, g: int, b: int, w: int = 0) -> tuple[int, int, i (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 = [] @@ -814,12 +824,14 @@ def _load_white_gains(self, key: str) -> tuple[int, int, int, int] | None: 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": @@ -841,7 +853,7 @@ def _capture_white_reference(self) -> bool: for setting_key, gain in zip(setting_keys, gains): platform_settings.set(f"{SETTINGS_NAME_PREFIX}.{setting_key}", gain) if self._logging: - print(f"S:Stored white gains: {gains}") + print(f"ST:Stored white gains: {gains}") self._app.notification = Notification("White Cal Saved", port=self._port_selected) return True @@ -879,11 +891,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 = self._ordered_display_data(self._sensor_data) + self._display_data = self._sensor_data elif self._page_selected == _PAGE_CAL: - self._display_data["mode"] = "XYZ sensor" - #self._display_data["ref"] = "N/A" - self._display_data["press"] = "Use Raw/Data" + 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) @@ -892,7 +903,7 @@ 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 ("r", "g", "b")): @@ -907,13 +918,13 @@ def _update_display_values(self): # pylint: disable=unused-argument 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 = self._ordered_display_data(self._sensor_data) + 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 @@ -925,28 +936,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._ordered_display_data(self._sensor_data) + self._display_data = self._sensor_data elif self._page_selected == _PAGE_RAW: - self._display_data = self._ordered_display_data(self._sensor_data) + self._display_data = self._sensor_data if self._page_selected == _PAGE_STATS: if self._sample_rate > 0: @@ -962,17 +974,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() @@ -1002,13 +1010,13 @@ def _update_reading(self, delta: int): # pylint: disable=unused-argument def _start_motor_test_mode(self) -> bool: - # enable HexDrive power + # 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. + #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) @@ -1022,7 +1030,7 @@ def _start_motor_test_mode(self) -> bool: 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})") + print(f"ST: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: @@ -1031,21 +1039,21 @@ def _start_motor_test_mode(self) -> bool: self._rotation_rate_counters = [] return False if self.logging: - print(f"S:Rate counter {self._rotation_rate_counters}") + print(f"ST: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") + print("ST: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") + print("ST:Stopping Motor Test mode and cleaning up") app = self._app self._auto_mode = False self._auto_done = False @@ -1063,7 +1071,7 @@ def _stop_motor_test_mode(self): self._ina226 = None if len(app.hexdrive_apps) > 0: - app.hexdrive_apps[0].set_pwm((0, 0, 0, 0)) + app.hexdrive_apps[0].set_freq(0) app.hexdrive_apps[0].set_power(False) for c in self._rotation_rate_counters: @@ -1112,9 +1120,9 @@ def _draw_motor_test_mode(self, ctx): lines += [f"{index}: {rpm}rpm"] colours += [(1, 0, 1)] if self._ina226_reading: - lines += [f"I:{self._ina226_reading.get('current_mA', 0)}mA"] + lines += [f"I:{self._ina226_reading.get('mA', 0)}mA"] colours += [(1, 0.3, 0.3)] - lines += [f"V:{self._ina226_reading.get('bus_mV', 0)}mV"] + lines += [f"V:{self._ina226_reading.get('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", @@ -1127,7 +1135,7 @@ def _draw_auto_scan(self, ctx): chart_left = -90 chart_right = 90 chart_top = -65 - chart_bottom = 65 + chart_bottom = 35 chart_w = chart_right - chart_left chart_h = chart_bottom - chart_top @@ -1160,28 +1168,31 @@ def _draw_auto_scan(self, ctx): 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() + ctx.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 + bar_w, marker_y - 1, 2, 2).fill() + ctx.rectangle(x, marker_y - 1, bar_w, 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") + ctx.move_to(-50, chart_top - 25).text("Complete") else: progress = (self._auto_step * 100) // _AUTO_SCAN_STEPS - ctx.move_to(-55, chart_top - 5).text(f"Scan {progress}%") + ctx.move_to(-50, chart_top - 25).text(f"Scan {progress}%") + # Instantaneous current label (updated live during the scan) + ctx.font_size = label_font_size - 10 + ctx.rgb(1.0, 0.2, 0.2).move_to(-50, chart_bottom + label_font_size + 2).text(f"{self._auto_last_current_ma}mA") - 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") + # Y axis Maximum RPM and Current labels + ctx.font_size = label_font_size - 10 + ctx.rgb(0.0, 1.0, 1.0).move_to(chart_left, chart_top - 5).text(f"rpm:{max_rpm}") + ctx.rgb(1.0, 0.2, 0.2).move_to(15, chart_top - 5).text(f"mA:{max_current_ma}") - button_labels(ctx, cancel_label="Back", confirm_label="Manual") + #button_labels(ctx, cancel_label="Back", confirm_label="Manual") def _draw_select_port(self, ctx): @@ -1212,7 +1223,7 @@ def _draw_reading(self, ctx): if self._display_data: 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)] diff --git a/sensors/ina226.py b/sensors/ina226.py index d98520e..009a05b 100644 --- a/sensors/ina226.py +++ b/sensors/ina226.py @@ -105,7 +105,7 @@ 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 @@ -131,19 +131,19 @@ 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) + #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 - + #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, + #"power_mW": power_mw, } def read_sample_if_ready(self) -> dict[str, int] | None: @@ -161,6 +161,7 @@ def read_sample_if_ready(self) -> dict[str, int] | None: return None return self._measure_from_registers() + def _init(self) -> bool: manufacturer = self._read_u16_be(_REG_MANUFACTURER_ID) if manufacturer != _MANUFACTURER_ID_TI: @@ -178,9 +179,9 @@ def _measure(self) -> dict: 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"]), + #"power_mW": str(sample["power_mW"]), } if _ticks_diff(deadline, _ticks_ms()) <= 0: return {"Error": "timeout"} diff --git a/sensors/sensor_base.py b/sensors/sensor_base.py index 8df1bca..9435bbe 100644 --- a/sensors/sensor_base.py +++ b/sensors/sensor_base.py @@ -58,6 +58,10 @@ def read(self) -> dict: 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: From e836a2c702ae026d0105ec9a05d7e41888759c3e Mon Sep 17 00:00:00 2001 From: robotmad Date: Tue, 5 May 2026 01:24:26 +0100 Subject: [PATCH 16/48] Big refactor to mange servos and motors shared use of PWM better. --- EEPROM/hexdrive.py | 345 +++++++++++++++++++++++---------------------- 1 file changed, 177 insertions(+), 168 deletions(-) diff --git a/EEPROM/hexdrive.py b/EEPROM/hexdrive.py index 892c134..3a2faa1 100644 --- a/EEPROM/hexdrive.py +++ b/EEPROM/hexdrive.py @@ -17,7 +17,7 @@ # HexDrive Hexpansion constants # Hardware defintions: -_ENABLE_PIN = 0 # First LS pin used to enable the SMPSU +_ENABLE_PIN = 0 # First LS pin used to enable the SMPSU # Hardware Version 2 _COLOUR_INT_PIN = 1 # Second LS pin used to detect interrupts from the colour sensor to trigger readings without polling @@ -41,28 +41,30 @@ # 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) +_VID_ADDR = 0x10 # Address in the EEPROM where the Vendor ID (VID) byte is stored - used to identify the hardware version of the HexDrive _PID_ADDR = 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", "hw_ver", "servo_pin_map") + __slots__ = ("pid", "name", "motors", "servos", "servo_pin_map") - def __init__(self, pid_byte: int, motors: int = 0, servos: int = 0, name: str = "Unknown", servo_pins: tuple[int, int, int, int] = (-1, -1, -1, -1)): + 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.hw_ver: int = 0 # Hardware version of this type of HexDrive 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(0xC8, motors=2, servos=2, servo_pins=(3, 1, -1, -1)), # uncommitted version (2) can be used for anything + HexDriveType(0xC9, servos=2, name="2 Servo", servo_pins=(3, 1, -1, -1)), HexDriveType(0xCA, motors=2, name="2 Motor"), - HexDriveType(0xCB, motors=2, servos=4, servo_pins=(0, 1, 2, 3)), # uncommitted version can be used for anything - HexDriveType(0xCC, servos=4, name="4 Servo", servo_pins=(0, 1, 2, 3)), - HexDriveType(0xCD, motors=1, servos=2, name="1 Mot 2 Srvo", servo_pins=(2, 3, -1, -1)), - HexDriveType(0xCE, motors=1, name="1 Motor", servo_pins=(-1, -1, -1, -1)), - HexDriveType(0xCF, motors=1, servos=1, name="1 Mot 1 Srvo", servo_pins=(2, -1, -1, -1)), + 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)), + HexDriveType(0xCE, motors=1, name="1 Motor"), + HexDriveType(0xCF, motors=1, servos=1, name="1 Mot 1 Srvo", servo_pins=(1, -1, -1, -1)), ) @@ -78,17 +80,30 @@ def __init__(self, config: HexpansionConfig | None = None): self.config: HexpansionConfig = config self._logging: bool = True + # 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("HexDriveApp requires BadgeOS Upgrade") + except Exception as e: # pylint: disable=broad-except + print(f"D:Ver check failed {e}!") + # read hexpansion header from EEPROM to find out which sub-type we are - _hexdrive_type = self._check_port_for_hexdrive(self.config.port) + _hexdrive_type, hw_ver = 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") + self._hw_ver = hw_ver + # report app starting and which port it is running on - print(f"D:HexDrive{'2' if 2 == _hexdrive_type.hw_ver else ''} Type:'{_hexdrive_type.name}' V{self.VERSION} by RobotMad on port {self.config.port}") + print(f"D:HexDrive{'2' if 2 == self._hw_ver else ''} Type:'{_hexdrive_type.name}' App V{self.VERSION} by RobotMad on port {self.config.port}") self._hexdrive_type: HexDriveType = _hexdrive_type - self._hw_ver: int = _hexdrive_type.hw_ver 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 @@ -101,7 +116,7 @@ def __init__(self, config: HexpansionConfig | None = None): # LS Pins self._power_control: ePin = self.config.ls_pin[_ENABLE_PIN] - if self._hw_ver >= 1: + if self._hw_ver > 1: self._led_control: ePin = self.config.ls_pin[_LED_PIN] self._dist_xshut: ePin = self.config.ls_pin[_DIST_XSHUT_PIN] @@ -110,18 +125,7 @@ def __init__(self, config: HexpansionConfig | None = None): self._servo_centre: list[int] = [_SERVO_CENTRE] * self._hexdrive_type.servos 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 >= _MIN_BADGEOS_VERSION: - pass - else: - print("D:BadgeOS Upgrade required") - return - except Exception as e: # pylint: disable=broad-except - print(f"D:Ver check failed {e}") + if not self.initialise(): raise RuntimeError("HexDriveApp initialisation failed") @@ -138,7 +142,7 @@ def initialise(self) -> bool: # Initialise LS Pins try: self._power_control.init(mode=Pin.OUT) - if self._hw_ver >= 1: + if self._hw_ver > 1: self._led_control.init(mode=Pin.OUT) self._dist_xshut.init(mode=Pin.OUT) except Exception as e: # pylint: disable=broad-except @@ -148,8 +152,28 @@ def initialise(self) -> bool: # ensure SMPSU is turned off to start with self.power = False + # ensure distance sensor is enabled to start with (if we have a version of the hardware with a distance sensor) + self.set_dist_xshut(True) + # allocate PWM outputs according to the type of HexDrive - return self._pwm_init() + #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 + return True def deinitialise(self) -> bool: @@ -159,7 +183,7 @@ def deinitialise(self) -> bool: self._pwm_deinit() for hs_pin in self.config.pin: hs_pin.init(mode=Pin.IN) - if self._hw_ver >= 1: + if self._hexdrive_type.hw_ver >= 1: self._led_control.deinit() self._dist_xshut.deinit() return True @@ -178,27 +202,24 @@ async def _handle_stop_app(self, event): 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 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 + 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: @@ -211,18 +232,10 @@ def set_logging(self, state: bool): self._logging = state - - - @property - def power(self) -> bool: - """ Get the current state of the SMPSU enable pin. Returns True if enabled, False if disabled. """ - return self._power_state - - @power.setter - def power(self, state: bool) -> bool: + def set_power(self, state: bool) -> bool: """ Turn the SMPSU on or off. Returns success or failure. """ if state == self._power_state: - return False + return True # No change needed if self._logging: print(f"D:{self.config.port}:Power={'On' if state else 'Off'}") try: @@ -234,13 +247,38 @@ def power(self, state: bool) -> bool: self._power_state = state return True - @property - def keep_alive_period(self) -> int: - """ Get the current keep alive period in milliseconds. """ - return self._keep_alive_period - @keep_alive_period.setter - def keep_alive_period(self, period: int): + def set_dist_xshut(self, state: bool) -> bool: + """ Set the state of the distance sensor XSHUT pin to power cycle it for reset or power saving. Returns success or failure. """ + if self._hw_ver <= 1: + return False + try: + self._dist_xshut.init(mode=Pin.OUT) + self._dist_xshut.value(state) + if self._logging: + print(f"D:{self.config.port}:Distance Sensor XSHUT={'On' if state else 'Off'}") + return True + except Exception as e: # pylint: disable=broad-except + print(f"D:{self.config.port}:Distance Sensor XSHUT control failed {e}") + return False + + + def set_sensor_led(self, state: bool) -> bool: + """ Set the state of the colour sensor LED pin to turn on or off the LED to illuminate the area under the colour sensor. Returns success or failure. """ + if self._hw_ver <= 1: + return False + try: + self._led_control.init(mode=Pin.OUT) + self._led_control.value(state) + if self._logging: + print(f"D:{self.config.port}:Colour Sensor LED={'On' if state else 'Off'}") + return True + except Exception as e: # pylint: disable=broad-except + print(f"D:{self.config.port}:Colour Sensor LED control failed {e}") + return False + + + def set_keep_alive(self, period: int): """ Set the keep alive period in milliseconds: This is the period of time that can elapse without any commands being received before the app automatically turns off all outputs to prevent damage to motors or servos if something goes wrong. """ @@ -256,11 +294,14 @@ def set_freq(self, freq: int, channel: int | None = None, servo: bool = False) - _max_channel = self._hexdrive_type.servos if servo else self._hexdrive_type.motors if channel < 0 or channel >= _max_channel: return False - self._freq[channel] = freq - if not servo: - self._freq[channel<<1] = freq # two physical channels per motor channel - # map from logical channel to physical channel for servos and motors (only one physical channel in use in both cases) - physical_channel = self._servo_pin_map[channel] if servo else (channel << 1) + (self._motor_output[channel] > 0) + # 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): @@ -270,9 +311,17 @@ def set_freq(self, freq: int, channel: int | None = None, servo: bool = False) - 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 (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") try: pwm.freq(freq) if self._logging: @@ -315,19 +364,19 @@ def set_servoposition(self, channel: int | None = None, position: int | None = N elif channel < 0 or channel >= self._hexdrive_type.servos: return False else: - channel = self._servo_pin_map[channel] - pwm = self.PWMOutput[channel] + physical_channel = self._servo_pin_map[channel] + pwm = self.PWMOutput[physical_channel] if pwm is None: return False try: pwm.duty_ns(0) if self._logging: - print(self._pwm_log_string(channel) + "Off") + 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 @@ -337,9 +386,9 @@ def set_servoposition(self, channel: int | None = None, position: int | None = N pulse_width_in_ns = (self._servo_centre[channel] + position) * 1000 # convert from us to ns if self.PWMOutput[physical_channel] is None: # Channel hasn't been setup yet so we need to initialise it from scratch - self._freq[physical_channel] = self._freq[physical_channel] if (0 < self._freq[physical_channel]) and (self._freq[physical_channel] <= _MAX_SERVO_FREQ) else _DEFAULT_SERVO_FREQ + 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[physical_channel] = PWM(self.config.pin[physical_channel], freq = self._freq[physical_channel], duty_ns = pulse_width_in_ns) + self.PWMOutput[physical_channel] = PWM(self.config.pin[physical_channel], freq = self._freq[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 @@ -355,7 +404,7 @@ def set_servoposition(self, channel: int | None = None, position: int | None = N 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[physical_channel] = _DEFAULT_SERVO_FREQ + self._freq[channel] = _DEFAULT_SERVO_FREQ pwm.freq(_DEFAULT_SERVO_FREQ) if self._logging: print(self._pwm_log_string(physical_channel) + f"{_DEFAULT_SERVO_FREQ}Hz for Servo") @@ -384,14 +433,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 @@ -403,7 +450,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: @@ -414,145 +461,107 @@ 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 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] 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}") self._motor_output[motor] = output - self._check_outputs_energised() + if output != 0: + self._outputs_energised = True + #self._check_outputs_energised() self._time_since_last_update = 0 return True - # Set all 4 PWM duty cycles in one go using a tuple (0-65535) - #def set_pwm(self, duty_cycles: tuple[int, ...]) -> bool: - # """ Set the PWM duty cycle for all outputs at once using a tuple of values. Returns True if successful, False if failed. - # The duty_cycles are values from 0 to 65535. """ - # if not self._pwm_setup: - # return False - # self._outputs_energised = any(duty_cycles) - # for channel, duty_cycle in enumerate(duty_cycles): - # if not self._set_pwmoutput(channel, duty_cycle): - # return False - # self._time_since_last_update = 0 - # return True - - # -------------------------------------------------- # Private methods for internal use only. # -------------------------------------------------- - def _pwm_init(self) -> bool: - self._pwm_setup = False - # HS Pins - if self.config.pin is not None and len(self.config.pin) == 4: - # Allocate PWM generation to pins - for channel, _ in enumerate(self.config.pin): - self._freq[channel] = 0 - if self._hexdrive_type is not None: - if channel < (2 * self._hexdrive_type.motors): - # First channels are for motors (can be 0, 1 or 2 motors) - if 0 == channel % 2: - # initialise motor PWM output on even channel - self._motor_output[(channel>>1)] = 0 - self._freq[channel] = _DEFAULT_PWM_FREQ - #print(f"D:{self.config.port}:Motor PWM[{channel}]") - else: - # ignore the motor PWM output on odd channel - we will switch it on when needed - pass - elif channel < ((2 * self._hexdrive_type.motors) + self._hexdrive_type.servos): - # Remaining channels are for servos (can be 4, 2, 1 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 + # are any of the PWM outputs energised? + #def _check_outputs_energised(self): + # energised_output = False + # for channel, pwm in enumerate(self.PWMOutput): + # if pwm is not None: + # try: + # if 0 < pwm.duty_ns(): + # energised_output = True + # break + # except Exception as e: # pylint: disable=broad-except + # print(self._pwm_log_string(channel) + f"Check failed {e}") + # if self._outputs_energised != energised_output: + # if self._logging: + # print(f"D:{self.config.port}:Outputs {'Energised' if energised_output else 'De-energised'}") + # self._outputs_energised = energised_output + + + # Set a single PWM duty cycle (0-65535) for a specific 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, servo: bool = False) -> 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 - pin = self.config.pin[self._servo_pin_map[channel]] if servo else self.config.pin[channel] - self.PWMOutput[channel] = PWM(pin, freq = self._freq[channel], duty_u16 = duty_cycle) + pin = self.config.pin[_channel] + self.PWMOutput[_channel] = PWM(pin, freq = self._freq[_channel], duty_u16 = _duty_cycle) if self._logging: - print(self._pwm_log_string(channel) + f"{self.PWMOutput[channel]} init") - pwm = self.PWMOutput[channel] + 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 _duty_cycle != pwm.duty_u16(): + pwm.duty_u16(_duty_cycle) if self._logging: - print(self._pwm_log_string(channel) + f"{duty_cycle}") + 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 + def _check_port_for_hexdrive(self, port: int) -> tuple[HexDriveType | None, int]: + #just read the part of the header which contains the VID & PID try: - pid_bytes = self.config.i2c.readfrom_mem(_EEPROM_ADDR, _PID_ADDR, 2, addrsize = (8*_EEPROM_NUM_ADDRESS_BYTES)) + vid_and_pid_bytes = self.config.i2c.readfrom_mem(_EEPROM_ADDR, _VID_ADDR, 4, 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 + return (None, 0) # 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: - return hexpansion_type + # we only use the LSByte of the PID to identify the type of HexDrive, as the MSByte is used for other things + if vid_and_pid_bytes[2] == hexpansion_type.pid: + return (hexpansion_type, 2 if vid_and_pid_bytes[:2] == b'\xCB\xCB' else 0) # Hardware Version 2 if VID is 0xCBCB, otherwise Hardware Version 1 # we are not interested in this type of hexpansion - return None + return (None, 0) def _parse_version(self, version): From 21e2a68d6c036624c2b695261e0db3d803138d35 Mon Sep 17 00:00:00 2001 From: robotmad Date: Thu, 7 May 2026 14:18:25 +0100 Subject: [PATCH 17/48] primitive board test featuresfull rpm vs power charting, start of analysis Co-authored-by: Copilot --- EEPROM/hexdrive.py | 35 ++-- app.py | 14 +- hexpansion_mgr.py | 16 +- sensor_manager.py | 6 +- sensor_test.py | 465 +++++++++++++++++++++++++++++++++-------- sensors/ina226.py | 54 ++--- sensors/opt4060.py | 19 +- sensors/sensor_base.py | 9 +- sensors/vl53l0x.py | 49 +---- 9 files changed, 465 insertions(+), 202 deletions(-) diff --git a/EEPROM/hexdrive.py b/EEPROM/hexdrive.py index 3a2faa1..d863c06 100644 --- a/EEPROM/hexdrive.py +++ b/EEPROM/hexdrive.py @@ -9,8 +9,8 @@ from system.eventbus import eventbus from system.hexpansion.config import HexpansionConfig from system.scheduler.events import RequestStopAppEvent -from tildagon import Pin as ePin import app +from tildagon import Pin as ePin # 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 @@ -150,10 +150,7 @@ def initialise(self) -> bool: return False # ensure SMPSU is turned off to start with - self.power = False - - # ensure distance sensor is enabled to start with (if we have a version of the hardware with a distance sensor) - self.set_dist_xshut(True) + self.set_power(False) # allocate PWM outputs according to the type of HexDrive #return self._pwm_init() @@ -173,17 +170,21 @@ def initialise(self) -> bool: print(f"D:{self.config.port}:Servo {channel} on Physical channel {physical_channel}") self._freq[physical_channel] = _DEFAULT_SERVO_FREQ self._pwm_setup = True + + # ensure distance sensor is enabled to start with (if we have a version of the hardware with a distance sensor) + self.set_dist_xshut(True) + return True def deinitialise(self) -> bool: """ De-initialise the app - return True if successful, False if failed.""" # Turn off all PWM outputs & release resources - self.power = False + self.set_power(False) self._pwm_deinit() for hs_pin in self.config.pin: hs_pin.init(mode=Pin.IN) - if self._hexdrive_type.hw_ver >= 1: + if self._hw_ver >= 1: self._led_control.deinit() self._dist_xshut.deinit() return True @@ -224,7 +225,7 @@ def background_update(self, delta: int): 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): @@ -322,14 +323,14 @@ def set_freq(self, freq: int, channel: int | None = None, servo: bool = False) - self.config.pin[this_channel].value(0) if self._logging: print(self._pwm_log_string(this_channel) + " disabled") - 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 + 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 @@ -550,7 +551,7 @@ def _set_pwmoutput(self, _channel: int, _duty_cycle: int) -> bool: def _check_port_for_hexdrive(self, port: int) -> tuple[HexDriveType | None, int]: #just read the part of the header which contains the VID & PID try: - vid_and_pid_bytes = self.config.i2c.readfrom_mem(_EEPROM_ADDR, _VID_ADDR, 4, addrsize = (8*_EEPROM_NUM_ADDRESS_BYTES)) + vid_and_pid_bytes = self.config.i2c.readfrom_mem(_EEPROM_ADDR, _VID_ADDR, 4, 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}") diff --git a/app.py b/app.py index 018d3b9..374e3be 100644 --- a/app.py +++ b/app.py @@ -185,6 +185,7 @@ 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.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. @@ -671,6 +672,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() @@ -686,6 +691,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 @@ -697,6 +703,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: @@ -836,8 +843,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 @@ -899,7 +906,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}") @@ -907,6 +914,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 diff --git a/hexpansion_mgr.py b/hexpansion_mgr.py index 58fbcd6..a4b4020 100644 --- a/hexpansion_mgr.py +++ b/hexpansion_mgr.py @@ -20,7 +20,7 @@ from events.input import BUTTON_TYPES from machine import I2C from system.eventbus import eventbus -from system.hexpansion.events import HexpansionInsertionEvent +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 @@ -151,14 +151,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) @@ -844,21 +842,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) 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) + 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 +867,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 diff --git a/sensor_manager.py b/sensor_manager.py index 795a139..5800037 100644 --- a/sensor_manager.py +++ b/sensor_manager.py @@ -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, 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: @@ -213,7 +217,7 @@ def last_data(self) -> dict: return self._last_data @property - def port(self) -> str | None: + def port(self) -> int | None: return self._port @property diff --git a/sensor_test.py b/sensor_test.py index 53cdbed..56dc719 100644 --- a/sensor_test.py +++ b/sensor_test.py @@ -14,14 +14,14 @@ from app_components.tokens import label_font_size, button_labels from app_components.notification import Notification from system.hexpansion.config import HexpansionConfig +from system.hexpansion.util import detect_eeprom_addr, get_hexpansion_block_devices, read_hexpansion_header import settings as platform_settings -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 SETTINGS_NAME_PREFIX, DEFAULT_BACKGROUND_UPDATE_PERIOD, MOTOR_PWM_FREQ +import vfs + +from egpio import ePin +from .sensor_manager import SensorManager + +from .app import SETTINGS_NAME_PREFIX, DEFAULT_BACKGROUND_UPDATE_PERIOD, MOTOR_PWM_FREQ, STATE_SENSOR try: from machine import Pin, mem32, disable_irq, enable_irq @@ -64,9 +64,9 @@ def enable_irq(_state: int) -> None: _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_EMITTER_PINS = [1, 2] # LS_B & LS_C 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) +_ROTATION_RATE_SENSOR_ENABLE_PINS = [3, 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 = 2 # Step size for adjusting IR emitter brightness in manual mode, 0-255 (0=off, 255=full on) @@ -76,9 +76,11 @@ def enable_irq(_state: int) -> None: _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 +_AUTO_SCAN_STEPS = 60 # Number of power levels to test during auto scan +_AUTO_SCAN_SETTLE_MS = 320 # ms to wait after setting power before starting actual measurement period +_AUTO_SCAN_MEASURE_MS = 5000 # ms measurement window per step (maximum) +_AUTO_RESULTS_FILENAME = "mtrtst.csv" +_AUTO_RESULTS_DEST_LABELS = ("badge fs", "hex fs") # Pages of information to show for each sensor (can be switched with up/down buttons) @@ -114,8 +116,8 @@ def enable_irq(_state: int) -> None: 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 + Currently only the motor-test CSV destination is exposed here.""" + s["path"] = MySetting(s, 0, 0, len(_AUTO_RESULTS_DEST_LABELS) - 1, labels=_AUTO_RESULTS_DEST_LABELS) # ---- Sensor Test manager --------------------------------------------------- @@ -132,7 +134,7 @@ class SensorTestMgr: def __init__(self, app, hextest_port: int | None = None, 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 = {} @@ -146,32 +148,34 @@ def __init__(self, app, hextest_port: int | None = None, logging: bool = False): self._new_sample: bool = False 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 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_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 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._rotation_detected: bool = False # True once motion has been observed during auto scan 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._motor_calibration_fit: list[tuple[float, float] | None] = [] # list of (slope, intercept) fits, indexed by motor number + 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 @@ -205,6 +209,10 @@ def sample_count(self) -> int: def sample_count(self, value: int): self._sample_count = value + @property + def _rotation_rate_rounding(self) -> int: + return (self._rotation_rate_measurement_period * self._rotation_rate_spokes) // 2 + def hextest_setup(self, port: int | None): """Use HS pins on a spare Hexpansion to make rotation rate measurements.""" @@ -215,7 +223,7 @@ def hextest_setup(self, port: int | None): 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._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() @@ -241,21 +249,25 @@ def start(self) -> bool: 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 - if app.hexdrive_ports is not None: + # If a HexTest is present then go straight to motor test mode. + if len(app.hexdrive_apps) > 0 and self._test_support_hexpansion_config is not None and self._start_motor_test_mode(): + self._port_selected = self._test_support_hexpansion_config.port + self._sub_state = _SUB_MOTOR_TEST + elif 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 @@ -265,10 +277,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() @@ -440,26 +452,50 @@ 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"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._app.hexdrive_ports[0]) + if sensor_mgr.type == "Colour": + if config.ls_pin[1].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[3].value(): + return None + 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 + if sensor_mgr.type == "Colour": + if config.ls_pin[1].value(): + self._test_results["colour int high"] = True + elif sensor_mgr.type == "Distance": + if config.ls_pin[3].value(): + self._test_results["distance int high"] = True + elif self._sub_state == _SUB_MOTOR_TEST: self._sample_ina226_in_background() return (self._rotation_rate_motor_power, self._rotation_rate_motor_power) @@ -472,13 +508,139 @@ def _auto_rotation_rate_step(self): if self._auto_step >= _AUTO_SCAN_STEPS: # Scan complete — stop motors self._auto_done = True + self._rotation_detected = False self._rotation_rate_motor_power = 0 self._auto_direction *= -1 # reverse direction for next scan + self._auto_fit_calculate() + self._save_auto_results_csv() 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 + self._rotation_rate_measurement_period_elapsed = 0 + self._auto_settling = True + + + def _auto_results_dest_mode(self) -> int: + setting = self._app.settings.get("path") + if setting is None: + return 0 + try: + return int(setting.v) + except Exception: # pylint: disable=broad-exception-caught + return 0 + + + def _mount_hexdrive_fs(self, port: int) -> tuple[str | None, bool]: + mountpoint = f"/hexpansion_{port}" + config = HexpansionConfig(port) + eeprom_addr, addr_len = detect_eeprom_addr(config.i2c) + if eeprom_addr is None or addr_len is None: + print(f"ST:No EEPROM found on hexdrive port {port}") + return None, False + header = read_hexpansion_header(config.i2c, eeprom_addr=eeprom_addr, addr_len=addr_len) + if header is None: + print(f"ST:Failed to read hexdrive header on port {port}") + return None, False + try: + _, partition = get_hexpansion_block_devices(config.i2c, header, eeprom_addr, addr_len=addr_len) + except RuntimeError as exc: + print(f"ST:Failed to get hexdrive 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"ST:Failed to mount {mountpoint}: {exc}") + return None, False + except Exception as exc: # pylint: disable=broad-exception-caught + print(f"ST:Failed to mount {mountpoint}: {exc}") + return None, False + return mountpoint, mounted_here + + + def _auto_results_path(self) -> tuple[str | None, str | None, bool]: + if self._auto_results_dest_mode() == 1: + if len(self._app.hexdrive_ports) == 0: + print("ST:No HexDrive present for hex fs CSV save") + return None, None, False + mountpoint, mounted_here = self._mount_hexdrive_fs(self._app.hexdrive_ports[0]) + if mountpoint is None: + return None, None, False + return f"{mountpoint}/{_AUTO_RESULTS_FILENAME}", mountpoint, mounted_here + return f"/{_AUTO_RESULTS_FILENAME}", None, False + + + def _save_auto_results_csv(self) -> bool: + if len(self._auto_results) == 0: + return False + output_path, mountpoint, mounted_here = self._auto_results_path() + if output_path is None: + return False + + rpm_count = len(self._rotation_rate_rpms) + header = ["pwr"] + [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._auto_results: + row = [str(power)] + row.extend(str(rpm) for rpm in rpms) + row.append(str(current_ma)) + csv_file.write(",".join(row).encode()) + except Exception as exc: # pylint: disable=broad-exception-caught + print(f"ST:Failed to save CSV {output_path}: {exc}") + 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"ST:Failed to unmount {mountpoint}: {exc}") + + print(f"ST:Saved auto motor test CSV to {output_path}") + return True + + + @staticmethod + def _linear_regression(points: list[tuple[int, int]]) -> tuple[float, float] | None: + if len(points) < 2: + return None + count = len(points) + sum_x = sum(point[0] for point in points) + sum_y = sum(point[1] for point in points) + sum_xx = sum(point[0] * point[0] for point in points) + sum_xy = sum(point[0] * point[1] for point in points) + denominator = (count * sum_xx) - (sum_x * sum_x) + if denominator == 0: + return None + slope = ((count * sum_xy) - (sum_x * sum_y)) / denominator + intercept = (sum_y - (slope * sum_x)) / count + return slope, intercept + + + def _auto_fit_calculate(self) -> None: + self._motor_calibration_fit = [] + for index in range(len(self._rotation_rate_rpms)): + points = [(power, rpms[index]) for power, rpms, _ in self._auto_results if index < len(rpms)] + self._motor_calibration_fit.append(self._linear_regression(points)) + + + def _show_auto_results_fit(self) -> None: + lines = ["Auto Scan Fit"] + colours: list[tuple[float, float, float]] = [(1, 1, 0)] + for index in range(len(self._rotation_rate_rpms)): + fit = self._motor_calibration_fit[index] if index < len(self._motor_calibration_fit) else None + if fit is None: + lines.append(f"M{index + 1}: n/a") + else: + slope, intercept = fit + lines.append(f"M{index + 1}: r={slope:.3f}p{intercept:+.1f}") + colours.append(self._colour_for_index(index)) + self._app.show_message(lines, colours, return_state=STATE_SENSOR) # ------------------------------------------------------------------ @@ -501,7 +663,7 @@ def _rotation_rate_enable(self, enable: bool = True) -> bool: try: if enable: if self._logging: - print("Enabling rotation rate emitter and sensors") + print("ST:Enabling rotation rate emitters 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) @@ -510,7 +672,7 @@ def _rotation_rate_enable(self, enable: bool = True) -> bool: 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") + print("ST:Disabling rotation rate emitters 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: @@ -529,7 +691,7 @@ def _init_ina226_for_motor_test(self) -> bool: self._ina226_reading = {} self._reset_ina226_accumulators() try: - from .sensor_manager import SensorManager + #from .sensor_manager import SensorManager 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, 7): @@ -557,8 +719,7 @@ def _init_ina226_for_motor_test(self) -> bool: 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 + self._ina226_sample_count = -1 def _sample_ina226_in_background(self) -> None: @@ -569,9 +730,10 @@ def _sample_ina226_in_background(self) -> None: if data is None: return try: - self._ina226_sum_current_ma += int(data.get("mA", 0)) - self._ina226_sum_bus_mv += int(data.get("mV", 0)) - #self._ina226_sum_power_mw += int(data.get("mW", 0)) + 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: @@ -603,6 +765,7 @@ def _update_motor_test_mode(self, delta: int): # pylint: disable=unused-argumen # CANCEL always exits motor test mode if app.button_states.get(BUTTON_TYPES["CANCEL"]): app.button_states.clear() + self._show_auto_results_fit() self._stop_motor_test_mode() return @@ -618,6 +781,8 @@ def _update_motor_test_mode(self, delta: int): # pylint: disable=unused-argumen counter.value(0) # reset counter if self._auto_mode: # Switch back to manual + self._show_auto_results_fit() + self._rotation_rate_measurement_period = _ROTATION_RATE_MEASUREMENT_PERIOD_MS self._auto_mode = False self._auto_done = False else: @@ -625,43 +790,63 @@ def _update_motor_test_mode(self, delta: int): # pylint: disable=unused-argumen self._auto_mode = True self._auto_done = False self._auto_step = 0 - self._auto_timer = 0 + self._rotation_rate_measurement_period = _AUTO_SCAN_MEASURE_MS self._auto_settling = True self._auto_results = [] - self._auto_max_rpm = 0 - self._auto_max_current_ma = 0 + self._auto_max_rpm = 10 + self._auto_max_current_ma = 50 + self._rotation_detected = False app.refresh = True return if self._auto_mode: if not self._auto_done: - self._auto_timer += delta + self._rotation_rate_measurement_period_elapsed += delta if self._auto_settling: - if self._auto_timer >= _AUTO_SCAN_SETTLE_MS: + 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: + 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._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._rotation_rate_rpms = [0] * len(self._rotation_rate_counters) + if self._logging: + print(f"ST:Auto Scan Step {self._auto_step}/{_AUTO_SCAN_STEPS} - Power: {power}, Rate: 0 rpm, Current: {current_ma}mA") + self._auto_results.append((power//66, [0] * len(self._rotation_rate_counters), current_ma)) self._auto_rotation_rate_step() + else: - self._auto_timer = 0 + 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._auto_settling = False self._reset_ina226_accumulators() else: - if self._auto_timer >= _AUTO_SCAN_MEASURE_MS: + if self._rotation_rate_measurement_period_elapsed >= self._rotation_rate_measurement_period: # 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) + 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) + rounding) // (_AUTO_SCAN_MEASURE_MS * self._rotation_rate_spokes) + rpm = ((60000 * count) + self._rotation_rate_rounding) // (self._rotation_rate_measurement_period_elapsed * self._rotation_rate_spokes) if rpm > self._auto_max_rpm: self._auto_max_rpm = rpm - rate[index] = 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) @@ -670,15 +855,16 @@ def _update_motor_test_mode(self, delta: int): # pylint: disable=unused-argumen self._auto_max_current_ma = current_abs power = self._rotation_rate_motor_power if self._logging: - print(f"ST:Auto Scan Step {self._auto_step}/{_AUTO_SCAN_STEPS} - Power: {power}, Rate: {rate} rpm, Current: {current_ma}mA") - self._auto_results.append((power, rate, current_ma)) + print(f"ST:Auto Scan Step {self._auto_step}/{_AUTO_SCAN_STEPS} - Power: {power}, Rates: {self._rotation_rate_rpms} rpm, Current: {current_ma}mA") + self._auto_results.append((power//66, self._rotation_rate_rpms, 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: + 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: @@ -686,8 +872,8 @@ def _update_motor_test_mode(self, delta: int): # pylint: disable=unused-argumen 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"ST:Rotation Rates: {self._rotation_rate_rpms}") + #if self.logging: + # print(f"ST:Rotation Rates: {self._rotation_rate_rpms}") # Manual mode button handling if app.button_states.get(BUTTON_TYPES["UP"]): @@ -695,21 +881,25 @@ def _update_motor_test_mode(self, delta: int): # pylint: disable=unused-argumen self.rotation_rate_emitter_duty = min(255, self.rotation_rate_emitter_duty + _IR_EMITTER_PWM_STEP_SIZE) if self.logging: print(f"ST:IR+Emitter Duty: {self.rotation_rate_emitter_duty}") + app.refresh = True 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"ST:IR-Emitter Duty: {self.rotation_rate_emitter_duty}") + app.refresh = True 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"ST:Motor+Power: {self._rotation_rate_motor_power}") + app.refresh = True 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"ST:Motor-Power: {self._rotation_rate_motor_power}") + app.refresh = True def _setup_for_sensor_type(self): @@ -854,7 +1044,7 @@ def _capture_white_reference(self) -> bool: 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) + self._app.notification = Notification("White Cal Saved", port=self._port_selected) return True @@ -1014,6 +1204,24 @@ def _start_motor_test_mode(self) -> bool: 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) + # Read INA226: + if self._init_ina226_for_motor_test(): + if self._ina226 is not None: + ina226 = self._ina226 + data = ina226.read(timeout=160) + try: + volts = int(data.get("mV", 0)) + amps = int(data.get("mA", 0)) + except Exception as e: # pylint: disable=broad-exception-caught + print(f"ST:Error reading INA226 data: {e}") + else: + if 3000 <= volts <= 3200 and amps < 5: + if self.logging: + print("ST:INA226 initial voltage & current reading OK") + self._test_results["Power Off"] = True + else: + self._test_results["Power Off"] = False + 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. @@ -1025,13 +1233,13 @@ def _start_motor_test_mode(self) -> bool: # 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 + 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"ST:Failed to allocate PCNT counter for pin {pin_num} (GPIO {gpio_num})") - app.notification = Notification("PCNT Init Failed") + 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: @@ -1042,18 +1250,66 @@ def _start_motor_test_mode(self) -> bool: print(f"ST: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 + + if self._ina226_sensor_mgr is not None: + app.update_period = self._ina226_sensor_mgr.read_interval # update at the sensor read interval + else: + app.update_period = _MOTOR_TEST_BACKGROUND_UPDATE_PERIOD + + # If we don't have a distance sensor then we can do a simple loopback test + sensor_mgr = self._sensor_mgr + if sensor_mgr is not None and sensor_mgr.get_sensor_by_name("VL53L0X") is None: + # Loop back test for XSHUT - DIST_INT + config = HexpansionConfig(self._app.hexdrive_ports[0]) + self._app.hexdrive_apps[0].set_dist_xshut(1) + if 1 == config.ls_pin[3].value(): + self._test_results["XSHUT high"] = True + self._test_results["dist int high"] = True + else: + self._test_results["XSHUT high"] = False + + self._app.hexdrive_apps[0].set_dist_xshut(0) + if 0 == config.ls_pin[3].value(): + self._test_results["XSHUT low"] = True + self._test_results["dist int low"] = True + else: + self._test_results["XSHUT low"] = False + app.update_period = _MOTOR_TEST_BACKGROUND_UPDATE_PERIOD return True if self.logging: - print("ST:Failed to initialise HexDrive for motor test mode") - app.notification = Notification("HexDrive Init Failed") + print("ST:Failed to initialise for motor test mode") + app.notification = Notification("Test Init Failed") return False def _stop_motor_test_mode(self): if self._logging: print("ST:Stopping Motor Test mode and cleaning up") + + # Take voltage reading before we power down + if self._ina226 is not None: + ina226 = self._ina226 + data = ina226.read(timeout=160) + try: + volts = int(data.get("mV", 0)) + except Exception as e: # pylint: disable=broad-exception-caught + print(f"ST:Error reading INA226 data: {e}") + else: + self._test_results["5V Voltage"] = volts + if 4900 <= volts <= 5300: + self._test_results["Power On"] = True + else: + self._test_results["Power On"] = False + + + # confirm all tests passed: + if all(self._test_results.get(test, False) for test in ("Power Off", "Power On","XSHUT high", "XSHUT low", "colour int high", "colour int low", "dist int high", "dist int low")): + if self.logging: + print("ST:***** Test PASSED *****") + self._app.notification = Notification(" Test PASSED", port=self._port_selected) + # Report test results + print(f"ST:Test results: {self._test_results}") + app = self._app self._auto_mode = False self._auto_done = False @@ -1109,6 +1365,7 @@ def _draw_motor_test_mode(self, ctx): if self._auto_mode: self._draw_auto_scan(ctx) return + #print("DRAWING") # 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)] @@ -1121,9 +1378,9 @@ def _draw_motor_test_mode(self, ctx): colours += [(1, 0, 1)] if self._ina226_reading: lines += [f"I:{self._ina226_reading.get('mA', 0)}mA"] - colours += [(1, 0.3, 0.3)] - lines += [f"V:{self._ina226_reading.get('mV', 0)}mV"] colours += [(0.3, 0.8, 1.0)] + #lines += [f"V:{self._ina226_reading.get('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") @@ -1159,16 +1416,12 @@ def _draw_auto_scan(self, ctx): 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 + x = chart_left + (abs(power) * chart_w) // 100 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 - 1, bar_w, 2).fill() + 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 @@ -1176,24 +1429,68 @@ def _draw_auto_scan(self, ctx): ctx.rectangle(x, marker_y - 1, bar_w, 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(-50, chart_top - 25).text("Complete") + + 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("0%") + 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._auto_results[0][0] + right_power = self._auto_results[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._auto_step * 100) // _AUTO_SCAN_STEPS - ctx.move_to(-50, chart_top - 25).text(f"Scan {progress}%") + 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 - 10 - ctx.rgb(1.0, 0.2, 0.2).move_to(-50, chart_bottom + label_font_size + 2).text(f"{self._auto_last_current_ma}mA") + ctx.font_size = label_font_size - 8 + for index, rpm in enumerate(self._rotation_rate_rpms): + ctx.rgb(*self._colour_for_index(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.2, 0.2).move_to(15, chart_bottom + 5 + ctx.font_size).text(f"{self._auto_last_current_ma}mA") # Y axis Maximum RPM and Current labels - ctx.font_size = label_font_size - 10 - ctx.rgb(0.0, 1.0, 1.0).move_to(chart_left, chart_top - 5).text(f"rpm:{max_rpm}") - ctx.rgb(1.0, 0.2, 0.2).move_to(15, chart_top - 5).text(f"mA:{max_current_ma}") + ctx.font_size = label_font_size - 8 + ctx.rgb(0.0, 1.0, 0.5).move_to(chart_left+20, chart_top - 5).text(f"rpm:{max_rpm}") + ctx.rgb(1.0, 0.2, 0.2).move_to(5, chart_top - 5).text(f"mA:{max_current_ma}") #button_labels(ctx, cancel_label="Back", confirm_label="Manual") + def _colour_for_index(self, index: int) -> tuple[float, float, float]: + if index == 0: + return (0.0, 1.0, 0.5) + elif index == 1: + return (1.0, 0.5, 0.0) + else: + return (1.0, 1.0, 1.0) def _draw_select_port(self, ctx): self._app.draw_message(ctx, @@ -1483,7 +1780,7 @@ def init(self, src: int, filter_ns: int | None = None) -> bool: return False if self.logging: - print(f"PCNT U{unit}: configured OK, CONF0=0x{mem32[conf0_addr]:08X}, CTRL=0x{mem32[_PCNT_CTRL_REG]:08X}, 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 diff --git a/sensors/ina226.py b/sensors/ina226.py index 009a05b..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 @@ -110,9 +91,9 @@ def _sleep_ms(delay_ms: int) -> None: # 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") + + #print(f"S:{self.NAME} {bus_mv}mV, {current_ma}mA") return { "mV": bus_mv, "mA": current_ma, - #"power_mW": power_mw, } def read_sample_if_ready(self) -> dict[str, int] | None: @@ -157,7 +135,10 @@ 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() @@ -173,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 { "mV": str(sample["mV"]), "mA": str(sample["mA"]), - #"power_mW": str(sample["power_mW"]), } - 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/opt4060.py b/sensors/opt4060.py index 883eb00..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,16 +273,16 @@ def _init(self) -> bool: return True - def _measure(self) -> dict: + def _measure(self, timeout: int = _READ_TIMEOUT_MS) -> dict: if self._i2c is None: return {"Error": "not initialized"} - - # Poll status for conversion-ready; timeout after READ_INTERVAL_MS - deadline = time.ticks_add(time.ticks_ms(), self.READ_INTERVAL_MS) + + # 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"} diff --git a/sensors/sensor_base.py b/sensors/sensor_base.py index 9435bbe..f8f04b1 100644 --- a/sensors/sensor_base.py +++ b/sensors/sensor_base.py @@ -14,6 +14,7 @@ class SensorBase: + """Abstract base class for BadgeBot I2C sensor drivers.""" # Sub-classes must override these I2C_ADDR = 0x00 NAME = "Unknown" @@ -45,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. @@ -53,7 +54,7 @@ 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)} @@ -81,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 # ------------------------------------------------------------------ @@ -95,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 diff --git a/sensors/vl53l0x.py b/sensors/vl53l0x.py index 9abeaac..cf9578e 100644 --- a/sensors/vl53l0x.py +++ b/sensors/vl53l0x.py @@ -69,35 +69,8 @@ (0xFF, 0x00), (0x80, 0x00), ) - -def _ticks_ms() -> int: - ticks_ms = getattr(time, "ticks_ms", None) - if ticks_ms is not None: - return ticks_ms() - return int(getattr(time, "monotonic")() * 1000) - - -def _ticks_add(base: int, delta: int) -> int: - if hasattr(time, "ticks_add"): - return time.ticks_add(base, delta) - return base + delta - - -def _ticks_diff(finish: int, now: int) -> int: - if hasattr(time, "ticks_diff"): - return time.ticks_diff(finish, now) - return finish - now - - -def _sleep_ms(delay_ms: int): - sleep_ms = getattr(time, "sleep_ms", None) - if sleep_ms is not None: - sleep_ms(delay_ms) - return - getattr(time, "sleep")(delay_ms / 1000) - - class VL53L0X(SensorBase): + """VL53L0X Time-of-Flight distance sensor driver.""" I2C_ADDR = 0x29 NAME = "VL53L0X" READ_INTERVAL_MS = 100 @@ -177,16 +150,16 @@ def _init(self) -> bool: return True - def _measure(self) -> dict: + def _measure(self, timeout: int = _RANGE_TIMEOUT_MS) -> dict: diagnostics_output(1,0) self._prepare_single_shot() self._write_u8(_SYSRANGE_START, 0x01) - deadline = _ticks_add(_ticks_ms(), _RANGE_TIMEOUT_MS) + deadline = time.ticks_add(time.ticks_ms(), timeout) while self._read_u8(_SYSRANGE_START) & 0x01: - if _ticks_diff(deadline, _ticks_ms()) <= 0: + if time.ticks_diff(deadline, time.ticks_ms()) <= 0: return {"dist_mm": "timeout"} - _sleep_ms(1) + time.sleep_ms(1) if not self._wait_for_interrupt_ready(): return {"dist_mm": "timeout"} @@ -219,11 +192,11 @@ def _prepare_single_shot(self): self._close_stop_variable_window() def _wait_for_interrupt_ready(self) -> bool: - deadline = _ticks_add(_ticks_ms(), _RANGE_TIMEOUT_MS) + deadline = time.ticks_add(time.ticks_ms(), _RANGE_TIMEOUT_MS) while (self._read_u8(_RESULT_INTERRUPT_STATUS) & _INTERRUPT_READY_MASK) == 0: - if _ticks_diff(deadline, _ticks_ms()) <= 0: + if time.ticks_diff(deadline, time.ticks_ms()) <= 0: return False - _sleep_ms(1) + time.sleep_ms(1) return True def _perform_single_ref_calibration(self, vhv_init_byte: int) -> bool: @@ -250,11 +223,11 @@ def _get_spad_info(self): self._write_u8(0x94, 0x6B) self._write_u8(_SPAD_POLL_REG, 0x00) - deadline = _ticks_add(_ticks_ms(), _RANGE_TIMEOUT_MS) + deadline = time.ticks_add(time.ticks_ms(), _RANGE_TIMEOUT_MS) while self._read_u8(_SPAD_POLL_REG) == 0x00: - if _ticks_diff(deadline, _ticks_ms()) <= 0: + if time.ticks_diff(deadline, time.ticks_ms()) <= 0: return None - _sleep_ms(1) + time.sleep_ms(1) self._write_u8(_SPAD_POLL_REG, 0x01) spad_info = self._read_u8(_SPAD_INFO_REG) From 891a36fa1b8232511a9439440d2d6ca97bc0e0d7 Mon Sep 17 00:00:00 2001 From: robotmad Date: Thu, 7 May 2026 22:53:17 +0100 Subject: [PATCH 18/48] initial draft of texhest app - with means to download via BadgeBot --- EEPROM/hextest.py | 2199 +++++++++++++++++++++++++++++++++++++ app.py | 3 +- dev/download_to_device.py | 1 + settings_mgr.py | 4 +- 4 files changed, 2204 insertions(+), 3 deletions(-) create mode 100644 EEPROM/hextest.py diff --git a/EEPROM/hextest.py b/EEPROM/hextest.py new file mode 100644 index 0000000..742b9c8 --- /dev/null +++ b/EEPROM/hextest.py @@ -0,0 +1,2199 @@ +"""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 + +import ota +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.config import HexpansionConfig +from system.scheduler.events import (RequestForegroundPopEvent, + RequestForegroundPushEvent, + RequestStopAppEvent) +from system.scheduler import scheduler + +import app +from tildagon import Pin as ePin +from micropython import const + + +# 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 + +SETTINGS_NAME_PREFIX = "hextest." # Prefix for settings keys in EEPROM + +# HexTest Hexpansion constants +# Hardware defintions: +_NUM_HEXPANSION_SLOTS = 6 + +# 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 = [1, 2] # LS_B & LS_C 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, 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 = 2 # Step size for adjusting IR emitter brightness in manual mode, 0-255 (0=off, 255=full on) + +# Rotation Rate Auto scan configuration +_AUTO_SCAN_STEPS = 60 # Number of power levels to test during auto scan +_AUTO_SCAN_SETTLE_MS = 320 # ms to wait after setting power before starting actual measurement period +_AUTO_SCAN_MEASURE_MS = 5000 # ms measurement window per step (maximum) +_AUTO_RESULTS_FILENAME = "mtrtst.csv" +_AUTO_RESULTS_DEST_LABELS = ("badge fs", "hex fs") + +# App states +STATE_MENU = 0 +STATE_MESSAGE = 1 # Message display +STATE_SETTINGS = 2 # Edit Settings +STATE_SENSOR = 3 # Sensor Test +STATE_MOTOR_TEST = 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 = 0 +MENU_ITEM_MOTOR_TEST = 1 +MENU_ITEM_SETTINGS = 2 +MENU_ITEM_ABOUT = 3 +MENU_ITEM_EXIT = 4 + +DEFAULT_BACKGROUND_UPDATE_PERIOD = 100 # mS when not moving +_LOGGING = True +_AUTO_REPEAT_MS = 200 # Time between auto-repeats, in ms +_AUTO_REPEAT_COUNT_THRES = 10 # Number of auto-repeats before increasing level +_AUTO_REPEAT_SPEED_LEVEL_MAX = 4 # Maximum level of auto-repeat speed increases +_AUTO_REPEAT_LEVEL_MAX = 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 = 0 +_PAGE_STATS = 1 +_PAGE_DATA = 2 +_PAGE_CAL = 3 +_PAGE_NAMES = { + 0: "Raw", + 1: "Stats", + 2: "Data", + 3: "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"D: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 = 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 - common settings first, then each module registers its own later + 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 = [] + self.hextest_port = self.config.port + + self.hexdrive_app = 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 = [] # 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 + self._rotation_rate_spokes: int = _DEFAULT_SPOKES_PER_ROTATION + + # 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_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._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._motor_calibration_fit: list[tuple[float, float] | None] = [] # list of (slope, intercept) fits, indexed by motor number + + 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(_AUTO_RESULTS_DEST_LABELS) - 1, labels=_AUTO_RESULTS_DEST_LABELS) + + 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"T: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) + + + # ------------------------------------------------------------------ + + @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.""" + 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.config is not None: + for pin_num in _ROTATION_RATE_EMITTER_PINS: + self.config.ls_pin[pin_num].duty(self._rotation_rate_emitter_duty) + + # ------------------------------------------------------------------ + + def update_settings(self): + """Update settings from EEPROM.""" + if self.logging: + print("T:Updating settings from EEPROM") + for s in self.settings: + self.settings[s].v = platform_settings.get(f"{SETTINGS_NAME_PREFIX}{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("T: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("T: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.""" + for hs_pin in self.config.pin: + hs_pin.init(mode=Pin.IN) + return True + + + ### ASYNC EVENT HANDLERS ### + + 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 = time.ticks_ms() + + while True: + cur_time = time.ticks_ms() + delta_ticks = 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() + # 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.button_states.clear() + self._sensor_data = {} + self._display_data = {} + self.refresh = True + + # Find HexDrive to test + self.hexdrive_app = self._find_hexpansion_app(4) # HARD CODED we look for HexDrive on slot 4 + + # Setup INA226: + if self._init_ina226_for_motor_test(): + if self._ina226 is not None and self.hexdrive_app is not None: + try: + if self.hexdrive_app.initialise() and self.hexdrive_app.set_power(True): + self.hexdrive_app.set_keep_alive(2000) # Updates can be quite slow as we are using the draw function + except AttributeError: + pass + + # 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.config.port][pin_num] + 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"T: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"T:Rate counter {self._rotation_rate_counters}") + self._rotation_rate_measurement_period_elapsed = 0 + self._rotation_rate_rpms = [0] * len(self._rotation_rate_counters) + + #self.update_period = self._ina226.read_interval # update at the sensor read interval + return True + if self.logging: + print("T:Failed to initialise for motor test mode") + self.notification = Notification("Test Init Failed") + return False + + + def _stop_motor_test_mode(self): + if self._logging: + print("T:Stopping Motor Test mode and cleaning up") + + # Take voltage reading before we power down + if self._ina226 is not None: + ina226 = self._ina226 + data = ina226.read(timeout=160) + try: + volts = int(data.get("mV", 0)) + except Exception as e: # pylint: disable=broad-exception-caught + print(f"T:Error reading INA226 data: {e}") + + 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("T: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_freq(0) + self.hexdrive_app.set_power(False) + except AttributeError: + pass + + 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.refresh = True + + + 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, 7): + if not mgr.open(port): + mgr.close() + if self._logging: + print(f"T: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"T: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"T: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) -> None: + 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"T: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): + self._auto_step += 1 + self.refresh = True + if self._auto_step >= _AUTO_SCAN_STEPS: + # Scan complete — stop motors + self._auto_done = True + self._rotation_detected = False + self._rotation_rate_motor_power = 0 + self._auto_direction *= -1 # reverse direction for next scan + #self._auto_fit_calculate() + #self._save_auto_results_csv() + else: + # Advance to next power level + self._rotation_rate_motor_power = self._auto_direction * (65535 * self._auto_step) // (_AUTO_SCAN_STEPS - 1) + self._rotation_rate_measurement_period_elapsed = 0 + self._auto_settling = True + + # ------------------------------------------------------------------ + # HEXPANSION operations + # ------------------------------------------------------------------ + + def _find_hexpansion_app(self, port: int) -> object | None: + """Find the app instance running from the hexpansion on the given port, if any. Returns the app instance if found, None otherwise.""" + expected_app_name = "HexDriveApp" + candidate_app = None + for an_app in scheduler.apps: + if type(an_app).__name__ == expected_app_name: + 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 + + ### 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"T: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("T: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"T: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.message_type == "reboop": + 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 + else: + # Message has been acknowledged by the user - allow access to the menu + self.button_states.clear() + # 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 + if self.config is None: + self._stop_motor_test_mode() + return + + # 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._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._show_auto_results_fit() + self._rotation_rate_measurement_period = _ROTATION_RATE_MEASUREMENT_PERIOD_MS + 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._rotation_rate_measurement_period = _AUTO_SCAN_MEASURE_MS + self._auto_settling = True + self._auto_results = [] + self._auto_max_rpm = 10 + self._auto_max_current_ma = 50 + self._rotation_detected = False + self.refresh = True + return + + if self._auto_mode: + if not self._auto_done: + self._rotation_rate_measurement_period_elapsed += delta + if self._auto_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._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._rotation_rate_rpms = [0] * len(self._rotation_rate_counters) + if self._logging: + print(f"T:Auto Scan Step {self._auto_step}/{_AUTO_SCAN_STEPS} - Power: {power}, Rate: 0 rpm, Current: {current_ma}mA") + self._auto_results.append((power//66, [0] * len(self._rotation_rate_counters), current_ma)) + self._auto_rotation_rate_step() + + 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._auto_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._auto_max_rpm: + self._auto_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._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 + if self._logging: + print(f"T:Auto Scan Step {self._auto_step}/{_AUTO_SCAN_STEPS} - Power: {power}, Rates: {self._rotation_rate_rpms} rpm, Current: {current_ma}mA") + self._auto_results.append((power//66, self._rotation_rate_rpms, 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 >= 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"T:Rotation Rates: {self._rotation_rate_rpms}") + + # Manual mode button handling + if self.button_states.get(BUTTON_TYPES["UP"]): + self.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"T:IR+Emitter Duty: {self.rotation_rate_emitter_duty}") + self.refresh = True + elif self.button_states.get(BUTTON_TYPES["DOWN"]): + self.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"T: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(65535, self._rotation_rate_motor_power + 1000) + if self.logging: + print(f"T: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(-65535, self._rotation_rate_motor_power - 1000) + if self.logging: + print(f"T: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" or self.message_type == "hexpansion": + 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._auto_mode: + self._draw_auto_scan(ctx) + return + #print("DRAWING") + # 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"] + colours += [(0.3, 0.8, 1.0)] + #lines += [f"V:{self._ina226_reading.get('mV', 0)}mV"] + #colours += [(0.3, 0.8, 1.0)] + self.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 = 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._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) // 100 + for index, rpm in enumerate(rpms): + h = (rpm * chart_h) // max_rpm + if h > 0: + # colour by index to differentiate multiple counters if present + 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._auto_done: + ctx.move_to(-50, chart_top - 25).text("Complete") + + 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("0%") + 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._auto_results[0][0] + right_power = self._auto_results[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._auto_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): + ctx.rgb(*self._colour_for_index(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.2, 0.2).move_to(15, chart_bottom + 5 + ctx.font_size).text(f"{self._auto_last_current_ma}mA") + + # Y axis Maximum RPM and Current labels + ctx.font_size = label_font_size - 8 + ctx.rgb(0.0, 1.0, 0.5).move_to(chart_left+20, chart_top - 5).text(f"rpm:{max_rpm}") + ctx.rgb(1.0, 0.2, 0.2).move_to(5, chart_top - 5).text(f"mA:{max_current_ma}") + + #button_labels(ctx, cancel_label="Back", confirm_label="Manual") + + def _colour_for_index(self, index: int) -> tuple[float, float, float]: + if index == 0: + return (0.0, 1.0, 0.5) + elif index == 1: + return (1.0, 0.5, 0.0) + else: + return (1.0, 1.0, 1.0) + + + + + + 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("T: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"T: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"T: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"T:Main Menu {item} at index {idx}") + if item == MAIN_MENU_ITEMS[MENU_ITEM_MOTOR_TEST]: # Motor Test + if self._motor_test_start(): + self.current_state = STATE_MOTOR_TEST + #elif item == MAIN_MENU_ITEMS[MENU_ITEM_SENSOR_TEST]: # Sensor Test + # 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.set_menu(None) + self.button_states.clear() + self.current_state = STATE_MESSAGE + self.message = ["HexTest", f"V{self.VERSION}", "By RobotMad"] + self.message_colours = [(1,1,1)]*len(self.message) + self.refresh = True + elif item == MAIN_MENU_ITEMS[MENU_ITEM_EXIT]: # Exit + eventbus.remove(RequestForegroundPushEvent, self._gain_focus, self) + eventbus.remove(RequestForegroundPopEvent, self._lose_focus, self) + eventbus.emit(RequestStopAppEvent(self)) + + + def _settings_menu_select_handler(self, item: str, idx: int): + if self.logging: + print(f"T:Setting {item} @ {idx}") + if idx == 0: #Save + if self.logging: + print("T:Settings Save All") + platform_settings.save() + self.notification = Notification(" Settings Saved") + self.set_menu() + elif idx == 1: #Default + if self.logging: + print("T: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. +# -------------------------------------------------- + + 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("T: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"T: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"T: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"T: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"T: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"T: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 + + + + + + +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.""" + try: + if self.v != self.d: + platform_settings.set(f"{SETTINGS_NAME_PREFIX}.{self._index()}", self.v) + else: + platform_settings.set(f"{SETTINGS_NAME_PREFIX}.{self._index()}", None) + except Exception as e: # pylint: disable=broad-except + print(f"H:Failed to persist setting {self._index()}: {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 = 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 = 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 = 0x00 # Configuration register +_REG_SHUNT_VOLTAGE = 0x01 # Shunt voltage result (signed) +_REG_BUS_VOLTAGE = 0x02 # Bus voltage result (unsigned) +_REG_POWER = 0x03 # Power result (unsigned) +_REG_CURRENT = 0x04 # Current result (signed) +_REG_CALIBRATION = 0x05 # Calibration register +_REG_MASK_ENABLE = 0x06 # Alert mask/enable register +_REG_ALERT_LIMIT = 0x07 # Alert threshold register +_REG_MANUFACTURER_ID = 0xFE # Manufacturer ID register +_REG_DIE_ID = 0xFF # Die ID register + + +# Configuration register bits (0x00) +_CFG_RESET_BIT = 0x8000 # Software reset bit +_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 11:9) +_CFG_AVG_1 = 0b000 # 1 sample average +_CFG_AVG_4 = 0b001 # 4 sample average +_CFG_AVG_16 = 0b010 # 16 sample average +_CFG_AVG_64 = 0b011 # 64 sample average +_CFG_AVG_128 = 0b100 # 128 sample average +_CFG_AVG_256 = 0b101 # 256 sample average +_CFG_AVG_512 = 0b110 # 512 sample average +_CFG_AVG_1024 = 0b111 # 1024 sample average + +# 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 +_CFG_CT_588US = 0b011 # 588 us conversion time +_CFG_CT_1100US = 0b100 # 1.1 ms conversion time +_CFG_CT_2116US = 0b101 # 2.116 ms conversion time +_CFG_CT_4156US = 0b110 # 4.156 ms conversion time +_CFG_CT_8244US = 0b111 # 8.244 ms conversion time + +# Operating mode field values (bits 2:0) +_CFG_MODE_POWER_DOWN = 0b000 # Power-down mode +_CFG_MODE_SHUNT_TRIG = 0b001 # Shunt voltage, triggered +_CFG_MODE_BUS_TRIG = 0b010 # Bus voltage, triggered +_CFG_MODE_SHUNT_BUS_TRIG = 0b011 # Shunt and bus, triggered +_CFG_MODE_ADC_OFF = 0b100 # ADC off (disabled) +_CFG_MODE_SHUNT_CONT = 0b101 # Shunt voltage, continuous +_CFG_MODE_BUS_CONT = 0b110 # Bus voltage, continuous +_CFG_MODE_SHUNT_BUS_CONT = 0b111 # Shunt and bus, continuous + + +# Mask/Enable register bits (0x06) +_MASK_SOL = 0x8000 # Shunt over-voltage alert flag +_MASK_SUL = 0x4000 # Shunt under-voltage alert flag +_MASK_BOL = 0x2000 # Bus over-voltage alert flag +_MASK_BUL = 0x1000 # Bus under-voltage alert flag +_MASK_POL = 0x0800 # Power over-limit alert flag +_MASK_CNVR = 0x0400 # Conversion ready alert flag +_MASK_AFF = 0x0010 # Alert function flag +_MASK_CVRF = 0x0008 # Conversion ready flag +_MASK_OVF = 0x0004 # Math overflow flag +_MASK_APOL = 0x0002 # Alert pin polarity select +_MASK_LEN = 0x0001 # Alert latch enable + + +# Device identification +_MANUFACTURER_ID_TI = 0x5449 # Texas Instruments manufacturer ID + + +# Driver configuration constants (100 mΩ shunt) +_SHUNT_RESISTOR_MILLIOHM = 100 +_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 = 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, 0x40)) # only allow the one address we actually expect + 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 = 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 + +ALL_SENSOR_CLASSES = [INA226] + +class SensorManager: + 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("T:SensorManager initialised") + + + # ------------------------------------------------------------------ + + @property + def logging(self) -> bool: + return self._logging + + @logging.setter + def logging(self, value: bool): + self._logging = value + + @property + def read_interval(self) -> int: + return self._read_interval_ms + + @property + def type(self) -> str: + 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"T: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"T:I2C scan failed on port {port}: {e}") + return False + + if self.logging: + print(f"T: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"T: + {cls.NAME} @ 0x{sensor.i2c_addr:02X} {cls.TYPE}") + elif self.logging: + print(f"T: - {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"T: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) + + return len(self._sensors) > 0 + + + 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[_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): + """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"T: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): + 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): + 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 len(self._sensors) + + @property + def current_sensor_name(self) -> str: + 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 self._index + + @property + def last_data(self) -> dict: + return self._last_data + + @property + 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[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 + + + + +__app_export__ = HexTestApp diff --git a/app.py b/app.py index 374e3be..e2f3d82 100644 --- a/app.py +++ b/app.py @@ -30,6 +30,7 @@ from .utils import draw_logo_animated, parse_version HEXDRIVE_APP_VERSION = 7 +HEXTEST_APP_VERSION = 1 SETTINGS_NAME_PREFIX = "badgebot." # Prefix for settings keys in EEPROM APP_VERSION = "1.5" # BadgeBot App Version Number @@ -257,7 +258,7 @@ def __init__(self): HexpansionType(0x10CF, "HexDrive2", vid=0xCBCB, eeprom_total_size=32768, eeprom_page_size= 64, app_mpy_name="hexdrive", app_mpy_version=HEXDRIVE_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), + 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="Rotation" ), 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 diff --git a/dev/download_to_device.py b/dev/download_to_device.py index dc792cd..25a34a8 100644 --- a/dev/download_to_device.py +++ b/dev/download_to_device.py @@ -38,6 +38,7 @@ class ModuleSpec: # Add new runtime modules here as the project grows. MODULES: tuple[ModuleSpec, ...] = ( ModuleSpec(Path("EEPROM/hexdrive.py"), Path("EEPROM/hexdrive.mpy")), + ModuleSpec(Path("EEPROM/hextest.py"), Path("EEPROM/hextest.mpy")), ModuleSpec(Path("app.py"), Path("app.mpy")), ModuleSpec(Path("autotune.py"), Path("autotune.mpy")), ModuleSpec(Path("autotune_mgr.py"), Path("autotune_mgr.mpy")), diff --git a/settings_mgr.py b/settings_mgr.py index 7fab96b..0dea589 100644 --- a/settings_mgr.py +++ b/settings_mgr.py @@ -35,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)] @@ -123,7 +123,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") From baf708fae6ee4874e9ef58a5aa37c75d26b0bbcf Mon Sep 17 00:00:00 2001 From: robotmad Date: Fri, 8 May 2026 08:01:04 +0100 Subject: [PATCH 19/48] hextest work Co-authored-by: Copilot --- EEPROM/hextest.py | 241 +++++++++++++++++++++++++--------------------- app.py | 2 +- 2 files changed, 131 insertions(+), 112 deletions(-) diff --git a/EEPROM/hextest.py b/EEPROM/hextest.py index 742b9c8..0fb8adf 100644 --- a/EEPROM/hextest.py +++ b/EEPROM/hextest.py @@ -7,6 +7,11 @@ import asyncio import time +try: + from typing import TYPE_CHECKING +except ImportError: + TYPE_CHECKING = False + import ota from machine import I2C, Pin, mem32, disable_irq, enable_irq @@ -26,6 +31,18 @@ from tildagon import Pin as ePin from micropython import const +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 @@ -133,7 +150,7 @@ def __init__(self, config: HexpansionConfig | None = None): # 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 = DEFAULT_BACKGROUND_UPDATE_PERIOD # mS + 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 @@ -154,13 +171,13 @@ def __init__(self, config: HexpansionConfig | None = None): self._auto_repeat_count: int = 0 self.auto_repeat_level: int = 0 - self.hexdrive_ports = [] - self.hextest_port = self.config.port + self.hexdrive_ports: list[int] = [4] + self.hextest_port: int = self.config.port - self.hexdrive_app = 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 = [] # hardware counters used to count photodiode pulses for rate testing + 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 @@ -225,6 +242,10 @@ def __init__(self, config: HexpansionConfig | None = None): eventbus.on_async(RequestForegroundPopEvent, self._lose_focus, self) eventbus.on_async(RequestStopAppEvent, self._handle_stop_app, 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) + + # ------------------------------------------------------------------ @@ -296,11 +317,21 @@ def _rotation_rate_enable(self, enable: bool = True) -> bool: def deinitialise(self) -> bool: """ De-initialise the app - return True if successful, False if failed.""" + # 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.remove(RequestForegroundPushEvent, self._gain_focus, self) + eventbus.remove(RequestForegroundPopEvent, self._lose_focus, self) + eventbus.emit(RequestStopAppEvent(self)) + + ### ASYNC EVENT HANDLERS ### async def _gain_focus(self, event: RequestForegroundPushEvent): @@ -332,11 +363,11 @@ async def _handle_stop_app(self, event): 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 = time.ticks_ms() + last_time: int = time.ticks_ms() while True: - cur_time = time.ticks_ms() - delta_ticks = time.ticks_diff(cur_time, last_time) + 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 @@ -346,7 +377,7 @@ async def background_task(self): def _background_update(self, delta: int): """Perform background updates based on the current sub-state.""" - self._sample_ina226_in_background() + 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: @@ -401,70 +432,67 @@ def auto_repeat_clear(self): def _motor_test_start(self) -> bool: """Enter the Sensor Test flow from the main menu.""" - self.button_states.clear() self._sensor_data = {} self._display_data = {} self.refresh = True # Find HexDrive to test - self.hexdrive_app = self._find_hexpansion_app(4) # HARD CODED we look for HexDrive on slot 4 + for port in self.hexdrive_ports: + app = self._find_hexpansion_app(port) + if app is not None: + if self.logging: + print(f"T:Found HexDrive app to test on port {port}") + self.hexdrive_app = _as_hexdrive_app(app) + break - # Setup INA226: - if self._init_ina226_for_motor_test(): - if self._ina226 is not None and self.hexdrive_app is not None: - try: - if self.hexdrive_app.initialise() and self.hexdrive_app.set_power(True): - self.hexdrive_app.set_keep_alive(2000) # Updates can be quite slow as we are using the draw function - except AttributeError: - pass + if self.hexdrive_app is not None: + #Setup UUT = HexDrive + try: + if self.hexdrive_app.initialise() and self.hexdrive_app.set_power(True): + self.hexdrive_app.set_keep_alive(2000) # Updates can be quite slow as we are using the draw function + except AttributeError: + pass - # 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.config.port][pin_num] - 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"T: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"T:Rate counter {self._rotation_rate_counters}") - self._rotation_rate_measurement_period_elapsed = 0 - self._rotation_rate_rpms = [0] * len(self._rotation_rate_counters) + # 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 - #self.update_period = self._ina226.read_interval # update at the sensor read interval - return True + # 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] + 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"T: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"T: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("T:Failed to initialise for motor test mode") - self.notification = Notification("Test Init Failed") + print("T:Failed to initialise for motor test mode - no hexdrive to test") + self.notification = Notification("No HexDrive to Test") return False def _stop_motor_test_mode(self): if self._logging: print("T:Stopping Motor Test mode and cleaning up") - - # Take voltage reading before we power down - if self._ina226 is not None: - ina226 = self._ina226 - data = ina226.read(timeout=160) - try: - volts = int(data.get("mV", 0)) - except Exception as e: # pylint: disable=broad-exception-caught - print(f"T:Error reading INA226 data: {e}") - self._auto_mode = False self._auto_done = False self._rotation_rate_motor_power = 0 @@ -532,7 +560,7 @@ def _reset_ina226_accumulators(self) -> None: self._ina226_sample_count = -1 - def _sample_ina226_in_background(self) -> None: + def _sample_ina226_in_background(self, delta: int) -> None: # pylint: disable=unused-argument sensor = self._ina226 if sensor is None: return @@ -587,10 +615,10 @@ def _auto_rotation_rate_step(self): # HEXPANSION operations # ------------------------------------------------------------------ - def _find_hexpansion_app(self, port: int) -> object | None: + def _find_hexpansion_app(self, port: int) -> HexDriveLike | None: """Find the app instance running from the hexpansion on the given port, if any. Returns the app instance if found, None otherwise.""" expected_app_name = "HexDriveApp" - candidate_app = None + candidate_app: HexDriveLike | None = None for an_app in scheduler.apps: if type(an_app).__name__ == expected_app_name: if hasattr(an_app, "config"): @@ -599,12 +627,12 @@ def _find_hexpansion_app(self, port: int) -> object | None: 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 + return _as_hexdrive_app(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 + candidate_app = _as_hexdrive_app(an_app) return candidate_app ### MAIN APP CONTROL FUNCTIONS ### @@ -673,16 +701,16 @@ def update(self, delta: int): def _update_state_message(self, delta: int): # pylint: disable=unused-argument if self.button_states.get(BUTTON_TYPES["CONFIRM"]): + if self.logging: + print("T:Message acknowledged by user") + self.button_states.clear() if self.message_type == "reboop": - 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 else: # Message has been acknowledged by the user - allow access to the menu - self.button_states.clear() # refresh the menu in case available options have changed self.set_menu() self.refresh = True @@ -696,10 +724,6 @@ def _update_state_message(self, delta: int): # pylint: disable=unused-argum def _motor_test_update(self, delta: int): # pylint: disable=unused-argument - if self.config is None: - self._stop_motor_test_mode() - return - # CANCEL always exits motor test mode if self.button_states.get(BUTTON_TYPES["CANCEL"]): self.button_states.clear() @@ -866,7 +890,7 @@ def draw(self, ctx): 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" or self.message_type == "hexpansion": + 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) @@ -1114,24 +1138,24 @@ def _main_menu_select_handler(self, item: str, idx: int): if self.logging: print(f"T: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 - # if self._sensor_test_start(): - # self.current_state = STATE_SENSOR + 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.set_menu(None) self.button_states.clear() - self.current_state = STATE_MESSAGE - self.message = ["HexTest", f"V{self.VERSION}", "By RobotMad"] - self.message_colours = [(1,1,1)]*len(self.message) - self.refresh = True + 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 - eventbus.remove(RequestForegroundPushEvent, self._gain_focus, self) - eventbus.remove(RequestForegroundPopEvent, self._lose_focus, self) - eventbus.emit(RequestStopAppEvent(self)) + self._exit_app() def _settings_menu_select_handler(self, item: str, idx: int): @@ -1902,7 +1926,7 @@ class INA226(SensorBase): """INA226 sensor driver with integer fixed-point outputs.""" I2C_ADDR = 0x40 - I2C_ADDRS = tuple(range(0x40, 0x40)) # only allow the one address we actually expect + 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" @@ -1980,6 +2004,7 @@ def _shutdown(self) -> None: 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 @@ -1989,26 +2014,20 @@ def __init__(self, logging: bool = False): self._last_data = {} self._read_interval_ms = 10 self._type = "Generic" - if self.logging: + if self._logging: print("T:SensorManager initialised") # ------------------------------------------------------------------ - @property - def logging(self) -> bool: - return self._logging - - @logging.setter - def logging(self, value: bool): - self._logging = value - @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 @@ -2025,18 +2044,18 @@ def open(self, port: int) -> bool: try: self._i2c = I2C(port) except Exception as e: # pylint: disable=broad-exception-caught - if self.logging: + if self._logging: print(f"T: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: + if self._logging: print(f"T:I2C scan failed on port {port}: {e}") return False - if self.logging: + if self._logging: print(f"T:Port {port} scan: {[hex(a) for a in found_addrs]}") used_addrs = set() @@ -2048,15 +2067,15 @@ def open(self, port: int) -> bool: if address in used_addrs: continue try: - sensor = cls(i2c_addr=address, logging=self.logging) + 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: + if self._logging: print(f"T: + {cls.NAME} @ 0x{sensor.i2c_addr:02X} {cls.TYPE}") - elif self.logging: + elif self._logging: print(f"T: - {cls.NAME} @ 0x{address:02X} begin() failed") self._index = 0 @@ -2074,27 +2093,19 @@ def open(self, port: int) -> bool: # (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: + if self._logging: print(f"T: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 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[_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): """Shutdown all sensors and release the I2C bus.""" for s in self._sensors: @@ -2104,7 +2115,7 @@ def close(self): 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: + if self._logging: print(f"T:LED Off port {self._port}") config = HexpansionConfig(self._port) if config is not None: @@ -2122,6 +2133,7 @@ def close(self): # ------------------------------------------------------------------ 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 = {} @@ -2130,6 +2142,7 @@ def next_sensor(self): 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 = {} @@ -2156,10 +2169,12 @@ def read_current(self) -> dict: @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] @@ -2168,18 +2183,22 @@ def current_sensor_name(self) -> str: @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]]: diff --git a/app.py b/app.py index e2f3d82..0e89091 100644 --- a/app.py +++ b/app.py @@ -258,7 +258,7 @@ def __init__(self): HexpansionType(0x10CF, "HexDrive2", vid=0xCBCB, eeprom_total_size=32768, eeprom_page_size= 64, app_mpy_name="hexdrive", app_mpy_version=HEXDRIVE_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="Rotation" ), + 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 From ca570e5ea5a7a22b80492a570c9f032d924c96d7 Mon Sep 17 00:00:00 2001 From: robotmad Date: Fri, 8 May 2026 13:45:46 +0100 Subject: [PATCH 20/48] fix a warning --- EEPROM/hextest.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/EEPROM/hextest.py b/EEPROM/hextest.py index 0fb8adf..a9f15c1 100644 --- a/EEPROM/hextest.py +++ b/EEPROM/hextest.py @@ -438,11 +438,11 @@ def _motor_test_start(self) -> bool: # Find HexDrive to test for port in self.hexdrive_ports: - app = self._find_hexpansion_app(port) - if app is not None: + hexpansion_app = self._find_hexpansion_app(port) + if hexpansion_app is not None: if self.logging: print(f"T:Found HexDrive app to test on port {port}") - self.hexdrive_app = _as_hexdrive_app(app) + self.hexdrive_app = _as_hexdrive_app(hexpansion_app) break if self.hexdrive_app is not None: From 9cb3a7468bcad40f02539ab6a007b5e5a88915d4 Mon Sep 17 00:00:00 2001 From: robotmad Date: Fri, 8 May 2026 13:55:20 +0100 Subject: [PATCH 21/48] fix plot x scaling issue --- EEPROM/hextest.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/EEPROM/hextest.py b/EEPROM/hextest.py index a9f15c1..7e27a24 100644 --- a/EEPROM/hextest.py +++ b/EEPROM/hextest.py @@ -62,6 +62,8 @@ def _as_hexdrive_app(value): _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, 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 = 2 # Step size for adjusting IR emitter brightness in manual mode, 0-255 (0=off, 255=full on) +_POWER_SCALE_FACTOR = 66 + # Rotation Rate Auto scan configuration _AUTO_SCAN_STEPS = 60 # Number of power levels to test during auto scan @@ -784,7 +786,7 @@ def _motor_test_update(self, delta: int): # pylint: disable=unused-argument self._rotation_rate_rpms = [0] * len(self._rotation_rate_counters) if self._logging: print(f"T:Auto Scan Step {self._auto_step}/{_AUTO_SCAN_STEPS} - Power: {power}, Rate: 0 rpm, Current: {current_ma}mA") - self._auto_results.append((power//66, [0] * len(self._rotation_rate_counters), current_ma)) + self._auto_results.append((power//_POWER_SCALE_FACTOR, [0] * len(self._rotation_rate_counters), current_ma)) self._auto_rotation_rate_step() else: @@ -818,7 +820,7 @@ def _motor_test_update(self, delta: int): # pylint: disable=unused-argument power = self._rotation_rate_motor_power if self._logging: print(f"T:Auto Scan Step {self._auto_step}/{_AUTO_SCAN_STEPS} - Power: {power}, Rates: {self._rotation_rate_rpms} rpm, Current: {current_ma}mA") - self._auto_results.append((power//66, self._rotation_rate_rpms, current_ma)) + self._auto_results.append((power//_POWER_SCALE_FACTOR, self._rotation_rate_rpms, current_ma)) self._auto_rotation_rate_step() # In auto mode, no manual button control for power/IR @@ -987,7 +989,7 @@ def _draw_auto_scan(self, ctx): 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) // 100 + x = chart_left + (abs(power) * chart_w) // (65536//_POWER_SCALE_FACTOR) for index, rpm in enumerate(rpms): h = (rpm * chart_h) // max_rpm if h > 0: From 477f4751a751233345a046a8e48d0939d1f72697 Mon Sep 17 00:00:00 2001 From: lincoltd7 <117526452+lincoltd7@users.noreply.github.com> Date: Sat, 9 May 2026 01:46:30 +0100 Subject: [PATCH 22/48] use to be released hexpansion utils Co-authored-by: Copilot --- EEPROM/hextest.py | 72 ++++++++++++++++++++++++---- typings/system/hexpansion/events.pyi | 14 ++++++ typings/system/hexpansion/util.pyi | 2 + 3 files changed, 79 insertions(+), 9 deletions(-) diff --git a/EEPROM/hextest.py b/EEPROM/hextest.py index 7e27a24..2be6411 100644 --- a/EEPROM/hextest.py +++ b/EEPROM/hextest.py @@ -22,9 +22,12 @@ from system.eventbus import eventbus from system.hexpansion.config import HexpansionConfig +from system.hexpansion.events import HexpansionMountedEvent, HexpansionRemovalEvent from system.scheduler.events import (RequestForegroundPopEvent, RequestForegroundPushEvent, RequestStopAppEvent) +from system.hexpansion.util import get_app_by_slot, get_slots_by_vid_pid +from system.hexpansion.header import HexpansionHeader from system.scheduler import scheduler import app @@ -141,7 +144,7 @@ def __init__(self, config: HexpansionConfig | None = None): else: raise RuntimeError("HexTestApp requires BadgeOS Upgrade") except Exception as e: # pylint: disable=broad-except - print(f"D:Ver check failed {e}!") + print(f"T:Ver check failed {e}!") self.config: HexpansionConfig = config self._logging: bool = True @@ -173,8 +176,8 @@ def __init__(self, config: HexpansionConfig | None = None): self._auto_repeat_count: int = 0 self.auto_repeat_level: int = 0 - self.hexdrive_ports: list[int] = [4] - self.hextest_port: int = self.config.port + self._hexdrive_ports: list[int] = [] + self._hexdrive_in_use_port: int | None = None self.hexdrive_app: HexDriveLike | None = None @@ -243,6 +246,8 @@ def __init__(self, config: HexpansionConfig | None = None): 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) @@ -319,6 +324,10 @@ def _rotation_rate_enable(self, enable: bool = True) -> bool: 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() @@ -329,12 +338,45 @@ def deinitialise(self) -> bool: def _exit_app(self): """ Clean up and exit the app, returning to the main menu.""" - eventbus.remove(RequestForegroundPushEvent, self._gain_focus, self) - eventbus.remove(RequestForegroundPopEvent, self._lose_focus, self) + eventbus.emit(RequestStopAppEvent(self)) - ### ASYNC EVENT HANDLERS ### + # ------------------------------------------------------------------ + # Async event handlers (registered directly on eventbus) + # ------------------------------------------------------------------ + + async def _handle_removal(self, event: HexpansionRemovalEvent): + if self._foreground: + if self._logging: + print(f"H:Hexpansion removed from port {event.port}") + if event.port == self._hexdrive_in_use_port: + if self.logging: + print(f"H:HexDrive removed from port {event.port} during test - stopping test and returning to menu") + self._hexdrive_app = None + self._hexdrive_in_use_port = None + self._hexdrive_ports.remove(event.port) + self.notification = Notification("HexDrive Removed") + self._stop_motor_test_mode() + + + async def _handle_mounted(self, event: HexpansionMountedEvent): + if self._foreground: + 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] + if (event.header.vid, event.header.pid) in vid_pid_pairs: + if self.logging: + print(f"H:Attempting to use newly mounted HexDrive on port {event.port} for motor testing") + if self._motor_test_start(): + if self.logging: + print(f"H:Successfully started motor test with newly mounted HexDrive on port {event.port}") + else: + if self.logging: + print(f"H:Failed to start motor test with newly mounted HexDrive on port {event.port}") + async def _gain_focus(self, event: RequestForegroundPushEvent): if event.app is self: @@ -343,7 +385,6 @@ async def _gain_focus(self, event: RequestForegroundPushEvent): self._foreground = True - async def _lose_focus(self, event: RequestForegroundPopEvent): if event.app is self: if self.logging: @@ -439,9 +480,22 @@ def _motor_test_start(self) -> bool: self.refresh = True # Find HexDrive to test - for port in self.hexdrive_ports: - hexpansion_app = self._find_hexpansion_app(port) + # look for any type of hexdrive (including HexDrive2 variants) in any port by their VID/PID + for type in self.HEXPANSION_TYPES: + if self.logging: + print(f"T:Looking for {type.name} (VID:PID {type.vid:04X}:{type.pid:04X}, Motors: {type.motors}, Servos: {type.servos})") + ports = get_slots_by_vid_pid(type.vid, type.pid) + if ports: + if self.logging: + print(f"T:Found {type.name} on port(s): {ports}") + self._hexdrive_ports.extend(ports) + break + + for port in self._hexdrive_ports: + #hexpansion_app = self._find_hexpansion_app(port) + hexpansion_app = get_app_by_slot(port) if hexpansion_app is not None: + self._hexdrive_in_use_port = port if self.logging: print(f"T:Found HexDrive app to test on port {port}") self.hexdrive_app = _as_hexdrive_app(hexpansion_app) 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 From efdff972dc4ba091c104d455d91fa95c779e63a9 Mon Sep 17 00:00:00 2001 From: lincoltd7 <117526452+lincoltd7@users.noreply.github.com> Date: Sat, 9 May 2026 09:09:51 +0100 Subject: [PATCH 23/48] get hexdrive to fit in 8kEEPROM Co-authored-by: Copilot --- EEPROM/hexdrive.py | 75 +++++++++++++++++++++++----------------------- EEPROM/hextest.py | 3 +- 2 files changed, 39 insertions(+), 39 deletions(-) diff --git a/EEPROM/hexdrive.py b/EEPROM/hexdrive.py index d863c06..830a2a8 100644 --- a/EEPROM/hexdrive.py +++ b/EEPROM/hexdrive.py @@ -83,8 +83,6 @@ def __init__(self, config: HexpansionConfig | None = None): # 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: @@ -95,7 +93,7 @@ def __init__(self, config: HexpansionConfig | None = None): # read hexpansion header from EEPROM to find out which sub-type we are _hexdrive_type, hw_ver = self._check_port_for_hexdrive(self.config.port) if _hexdrive_type is None: - print(f"D:{self.config.port}:Unknown HexDrive type - initialisation failed") + #print(f"D:{self.config.port}:Unknown HexDrive type - initialisation failed") raise RuntimeError("Unknown HexDrive type") self._hw_ver = hw_ver @@ -127,7 +125,7 @@ def __init__(self, config: HexpansionConfig | None = None): eventbus.on_async(RequestStopAppEvent, self._handle_stop_app, self) if not self.initialise(): - raise RuntimeError("HexDriveApp initialisation failed") + raise RuntimeError("HexDriveApp init failed") def initialise(self) -> bool: @@ -159,7 +157,7 @@ def initialise(self) -> bool: # 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}") + #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 @@ -167,7 +165,7 @@ def initialise(self) -> bool: 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}") + #print(f"D:{self.config.port}:Servo {channel} on Physical channel {physical_channel}") self._freq[physical_channel] = _DEFAULT_SERVO_FREQ self._pwm_setup = True @@ -219,7 +217,7 @@ def background_update(self, delta: int): try: pwm.duty_u16(0) except Exception as e: # pylint: disable=broad-except - print(self._pwm_log_string(channel) + f"Off failed {e}") + #print(self._pwm_log_string(channel) + f"Off failed {e}") self.PWMOutput[channel] = None # Tidy Up @@ -243,7 +241,7 @@ def set_power(self, state: bool) -> bool: 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 True @@ -260,7 +258,7 @@ def set_dist_xshut(self, state: bool) -> bool: print(f"D:{self.config.port}:Distance Sensor XSHUT={'On' if state else 'Off'}") return True except Exception as e: # pylint: disable=broad-except - print(f"D:{self.config.port}:Distance Sensor XSHUT control failed {e}") + #print(f"D:{self.config.port}:Distance Sensor XSHUT control failed {e}") return False @@ -275,7 +273,7 @@ def set_sensor_led(self, state: bool) -> bool: print(f"D:{self.config.port}:Colour Sensor LED={'On' if state else 'Off'}") return True except Exception as e: # pylint: disable=broad-except - print(f"D:{self.config.port}:Colour Sensor LED control failed {e}") + #print(f"D:{self.config.port}:Colour Sensor LED control failed {e}") return False @@ -321,15 +319,15 @@ def set_freq(self, freq: int, channel: int | None = None, servo: bool = False) - 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") + #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") + #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(self._pwm_log_string(this_channel) + f"set freq {freq} failed {e}") return False return True @@ -357,9 +355,10 @@ def set_servoposition(self, channel: int | None = None, position: int | None = N 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: @@ -371,10 +370,10 @@ def set_servoposition(self, channel: int | None = None, position: int | None = N return False try: pwm.duty_ns(0) - if self._logging: - print(self._pwm_log_string(physical_channel) + "Off") + #if self._logging: + # print(self._pwm_log_string(physical_channel) + "Off") except Exception as e: # pylint: disable=broad-except - print(self._pwm_log_string(physical_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() @@ -390,8 +389,8 @@ def set_servoposition(self, channel: int | None = None, position: int | None = N 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[physical_channel] = PWM(self.config.pin[physical_channel], freq = self._freq[channel], duty_ns = pulse_width_in_ns) - if self._logging: - print(self._pwm_log_string(physical_channel) + f"{self.PWMOutput[physical_channel]} init") + #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(physical_channel) + f"PWM(init) failed {e}") @@ -410,18 +409,18 @@ def set_servoposition(self, channel: int | None = None, position: int | None = N if self._logging: 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(physical_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 - 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") + #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") + #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(physical_channel) + f"set duty failed {e}") + #print(self._pwm_log_string(physical_channel) + f"set duty failed {e}") return False self._outputs_energised = True @@ -471,15 +470,15 @@ def set_motors(self, outputs: tuple[int, ...]) -> bool: pwm_to_disable.deinit() self.PWMOutput[output_to_disable] = None 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") + #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 if output != 0: self._outputs_energised = True - #self._check_outputs_energised() self._time_since_last_update = 0 return True @@ -533,17 +532,17 @@ def _set_pwmoutput(self, _channel: int, _duty_cycle: int) -> bool: # Channel hasn't been setup yet so we need to initialise it from scratch pin = self.config.pin[_channel] self.PWMOutput[_channel] = PWM(pin, freq = self._freq[_channel], duty_u16 = _duty_cycle) - if self._logging: - print(self._pwm_log_string(_channel) + f"{self.PWMOutput[_channel]} init") + #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}") + #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 diff --git a/EEPROM/hextest.py b/EEPROM/hextest.py index 2be6411..af9452d 100644 --- a/EEPROM/hextest.py +++ b/EEPROM/hextest.py @@ -542,7 +542,7 @@ def _motor_test_start(self) -> bool: return True if self.logging: print("T:Failed to initialise for motor test mode - no hexdrive to test") - self.notification = Notification("No HexDrive to Test") + self.notification = Notification("HexDrive not Found") return False @@ -801,6 +801,7 @@ def _motor_test_update(self, delta: int): # pylint: disable=unused-argument # Switch back to manual #self._show_auto_results_fit() self._rotation_rate_measurement_period = _ROTATION_RATE_MEASUREMENT_PERIOD_MS + self._rotation_rate_rpms = [0] * len(self._rotation_rate_counters) self._auto_mode = False self._auto_done = False else: From 431e0a0d1a7c947a70e0a958dbbd6219938ab32e Mon Sep 17 00:00:00 2001 From: lincoltd7 <117526452+lincoltd7@users.noreply.github.com> Date: Sat, 9 May 2026 10:17:14 +0100 Subject: [PATCH 24/48] Vendor HexDrive2 for EEPROM deployment --- .gitmodules | 3 +++ app.py | 13 +++++++------ dev/build_release.py | 35 +++++++++++++++++++++++++++++++++-- dev/download_to_device.py | 1 + tests/test_smoke.py | 28 ++++++++++++++++++++++++++++ vendor/HexDrive2 | 1 + 6 files changed, 73 insertions(+), 8 deletions(-) create mode 100644 .gitmodules create mode 160000 vendor/HexDrive2 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/app.py b/app.py index 0e89091..8d9a86a 100644 --- a/app.py +++ b/app.py @@ -30,6 +30,7 @@ from .utils import draw_logo_animated, parse_version HEXDRIVE_APP_VERSION = 7 +HEXDRIVE2_APP_VERSION = 7 HEXTEST_APP_VERSION = 1 SETTINGS_NAME_PREFIX = "badgebot." # Prefix for settings keys in EEPROM @@ -250,12 +251,12 @@ def __init__(self): 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="hexdrive", app_mpy_version=HEXDRIVE_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="hexdrive", app_mpy_version=HEXDRIVE_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="hexdrive", app_mpy_version=HEXDRIVE_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="hexdrive", app_mpy_version=HEXDRIVE_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="hexdrive", app_mpy_version=HEXDRIVE_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="hexdrive", app_mpy_version=HEXDRIVE_APP_VERSION, app_name="HexDriveApp", motors=1, servos=1, sub_type="1 Mot 1 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" ), diff --git a/dev/build_release.py b/dev/build_release.py index 9b75e04..636af39 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", @@ -39,6 +46,10 @@ 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"), @@ -46,10 +57,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) @@ -57,8 +84,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 @@ -99,6 +125,11 @@ 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.") diff --git a/dev/download_to_device.py b/dev/download_to_device.py index 67a8100..c9fbd6b 100644 --- a/dev/download_to_device.py +++ b/dev/download_to_device.py @@ -40,6 +40,7 @@ class ModuleSpec: # Add new runtime modules here as the project grows. MODULES: tuple[ModuleSpec, ...] = ( ModuleSpec(Path("EEPROM/hexdrive.py"), Path("EEPROM/hexdrive.mpy")), + ModuleSpec(Path("vendor/HexDrive2/hexdrive2.py"), Path("EEPROM/hexdrive2.mpy")), ModuleSpec(Path("EEPROM/hextest.py"), Path("EEPROM/hextest.mpy")), ModuleSpec(Path("app.py"), Path("app.mpy")), ModuleSpec(Path("autotune.py"), Path("autotune.mpy")), diff --git a/tests/test_smoke.py b/tests/test_smoke.py index 7e0b274..6ddc6ef 100644 --- a/tests/test_smoke.py +++ b/tests/test_smoke.py @@ -1,4 +1,6 @@ +import re import sys +from pathlib import Path # Add badge software to pythonpath sys.path.append("../../../") @@ -7,6 +9,13 @@ 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 @@ -39,6 +48,25 @@ def test_app_versions_match(): 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. diff --git a/vendor/HexDrive2 b/vendor/HexDrive2 new file mode 160000 index 0000000..2aaca58 --- /dev/null +++ b/vendor/HexDrive2 @@ -0,0 +1 @@ +Subproject commit 2aaca588d3863b114b90c4938c7b35918c90ed08 From e80dbff050e75af23362ea313521a42e7ddcc187 Mon Sep 17 00:00:00 2001 From: Christopher Barnes Date: Sat, 9 May 2026 10:30:09 +0100 Subject: [PATCH 25/48] Clarify HexDrive support and PWM resource details Updated README to clarify supported configurations and PWM resource usage for HexDrive. --- README.md | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/README.md b/README.md index e863449..4951379 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,7 +16,6 @@ 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. @@ -71,7 +70,7 @@ 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. @@ -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 ### From ab8f249f0fc0445dd2574c6451bedc5831b7b760 Mon Sep 17 00:00:00 2001 From: lincoltd7 <117526452+lincoltd7@users.noreply.github.com> Date: Sun, 10 May 2026 07:46:19 +0100 Subject: [PATCH 26/48] Refactor HexDrive app to support optional hexpansion manager and clean up unused hardware version checks Co-authored-by: Copilot --- EEPROM/hexdrive.py | 139 +++++++++++++-------------------------------- 1 file changed, 40 insertions(+), 99 deletions(-) diff --git a/EEPROM/hexdrive.py b/EEPROM/hexdrive.py index 830a2a8..37c1a06 100644 --- a/EEPROM/hexdrive.py +++ b/EEPROM/hexdrive.py @@ -1,13 +1,18 @@ """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. 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 @@ -19,12 +24,6 @@ # Hardware defintions: _ENABLE_PIN = 0 # First LS pin used to enable the SMPSU -# Hardware Version 2 -_COLOUR_INT_PIN = 1 # Second LS pin used to detect interrupts from the colour sensor to trigger readings without polling -_LED_PIN = 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 = 3 # Fourth LS pin used to detect interrupts from the distance sensor to trigger readings without polling -_DIST_XSHUT_PIN = 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 - # 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 @@ -50,21 +49,17 @@ class HexDriveType: __slots__ = ("pid", "name", "motors", "servos", "servo_pin_map") 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.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(0xC8, motors=2, servos=2, servo_pins=(3, 1, -1, -1)), # uncommitted version (2) can be used for anything - HexDriveType(0xC9, servos=2, name="2 Servo", servo_pins=(3, 1, -1, -1)), HexDriveType(0xCA, motors=2, name="2 Motor"), 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)), - HexDriveType(0xCE, motors=1, name="1 Motor"), - HexDriveType(0xCF, motors=1, servos=1, name="1 Mot 1 Srvo", servo_pins=(1, -1, -1, -1)), ) @@ -90,16 +85,14 @@ def __init__(self, config: HexpansionConfig | None = None): except Exception as e: # pylint: disable=broad-except print(f"D:Ver check failed {e}!") - # read hexpansion header from EEPROM to find out which sub-type we are - _hexdrive_type, hw_ver = self._check_port_for_hexdrive(self.config.port) + # 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") - self._hw_ver = hw_ver - # report app starting and which port it is running on - print(f"D:HexDrive{'2' if 2 == self._hw_ver else ''} Type:'{_hexdrive_type.name}' App V{self.VERSION} by RobotMad on port {self.config.port}") + 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 @@ -114,9 +107,6 @@ def __init__(self, config: HexpansionConfig | None = None): # LS Pins self._power_control: ePin = self.config.ls_pin[_ENABLE_PIN] - if self._hw_ver > 1: - self._led_control: ePin = self.config.ls_pin[_LED_PIN] - self._dist_xshut: ePin = self.config.ls_pin[_DIST_XSHUT_PIN] # Servo related self._servo_pin_map: tuple[int, int, int, int] = self._hexdrive_type.servo_pin_map @@ -140,9 +130,6 @@ def initialise(self) -> bool: # Initialise LS Pins try: self._power_control.init(mode=Pin.OUT) - if self._hw_ver > 1: - self._led_control.init(mode=Pin.OUT) - self._dist_xshut.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 @@ -150,8 +137,6 @@ def initialise(self) -> bool: # ensure SMPSU is turned off to start with self.set_power(False) - # allocate PWM outputs according to the type of HexDrive - #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. @@ -169,22 +154,6 @@ def initialise(self) -> bool: self._freq[physical_channel] = _DEFAULT_SERVO_FREQ self._pwm_setup = True - # ensure distance sensor is enabled to start with (if we have a version of the hardware with a distance sensor) - self.set_dist_xshut(True) - - return 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) - if self._hw_ver >= 1: - self._led_control.deinit() - self._dist_xshut.deinit() return True @@ -194,7 +163,8 @@ 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 @@ -247,36 +217,6 @@ def set_power(self, state: bool) -> bool: return True - def set_dist_xshut(self, state: bool) -> bool: - """ Set the state of the distance sensor XSHUT pin to power cycle it for reset or power saving. Returns success or failure. """ - if self._hw_ver <= 1: - return False - try: - self._dist_xshut.init(mode=Pin.OUT) - self._dist_xshut.value(state) - if self._logging: - print(f"D:{self.config.port}:Distance Sensor XSHUT={'On' if state else 'Off'}") - return True - except Exception as e: # pylint: disable=broad-except - #print(f"D:{self.config.port}:Distance Sensor XSHUT control failed {e}") - return False - - - def set_sensor_led(self, state: bool) -> bool: - """ Set the state of the colour sensor LED pin to turn on or off the LED to illuminate the area under the colour sensor. Returns success or failure. """ - if self._hw_ver <= 1: - return False - try: - self._led_control.init(mode=Pin.OUT) - self._led_control.value(state) - if self._logging: - print(f"D:{self.config.port}:Colour Sensor LED={'On' if state else 'Off'}") - return True - except Exception as e: # pylint: disable=broad-except - #print(f"D:{self.config.port}:Colour Sensor LED control failed {e}") - return False - - def set_keep_alive(self, period: int): """ Set the keep alive period in milliseconds: This is the period of time that can elapse without any commands being received before the app automatically @@ -505,22 +445,6 @@ def _pwm_deinit(self): # 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. - # are any of the PWM outputs energised? - #def _check_outputs_energised(self): - # energised_output = False - # for channel, pwm in enumerate(self.PWMOutput): - # if pwm is not None: - # try: - # if 0 < pwm.duty_ns(): - # energised_output = True - # break - # except Exception as e: # pylint: disable=broad-except - # print(self._pwm_log_string(channel) + f"Check failed {e}") - # if self._outputs_energised != energised_output: - # if self._logging: - # print(f"D:{self.config.port}:Outputs {'Energised' if energised_output else 'De-energised'}") - # self._outputs_energised = energised_output - # Set a single PWM duty cycle (0-65535) for a specific MOTOR output # if the channel has not been setup yet then we initialise it from scratch, otherwise we just change the duty cycle @@ -547,21 +471,38 @@ def _set_pwmoutput(self, _channel: int, _duty_cycle: int) -> bool: return True - def _check_port_for_hexdrive(self, port: int) -> tuple[HexDriveType | None, int]: - #just read the part of the header which contains the VID & PID + def _check_port_for_hexdrive(self, port: int) -> HexDriveType | None: + + pid: int = 0 try: - vid_and_pid_bytes = self.config.i2c.readfrom_mem(_EEPROM_ADDR, _VID_ADDR, 4, 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, 0) + 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 None # check which type of HexDrive this is by scanning the HEXDRIVE_TYPES list for _, hexpansion_type in enumerate(_HEXDRIVE_TYPES): # we only use the LSByte of the PID to identify the type of HexDrive, as the MSByte is used for other things - if vid_and_pid_bytes[2] == hexpansion_type.pid: - return (hexpansion_type, 2 if vid_and_pid_bytes[:2] == b'\xCB\xCB' else 0) # Hardware Version 2 if VID is 0xCBCB, otherwise Hardware Version 1 - # we are not interested in this type of hexpansion - return (None, 0) + if pid & 0xFF == hexpansion_type.pid: + return hexpansion_type + # we are not interested in this type of hexpansion as it was not recognised + return None def _parse_version(self, version): From 61ef43c9e4dcea02333d4e3a6686b62d8b740d76 Mon Sep 17 00:00:00 2001 From: lincoltd7 <117526452+lincoltd7@users.noreply.github.com> Date: Sun, 10 May 2026 07:47:58 +0100 Subject: [PATCH 27/48] Update HexDrive2 subproject commit reference --- vendor/HexDrive2 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/vendor/HexDrive2 b/vendor/HexDrive2 index 2aaca58..080b31a 160000 --- a/vendor/HexDrive2 +++ b/vendor/HexDrive2 @@ -1 +1 @@ -Subproject commit 2aaca588d3863b114b90c4938c7b35918c90ed08 +Subproject commit 080b31afa10eae7551300fecf19bbc6fafdadd45 From 771c59ab6d70505d2d5a63403a946a0f8db2593f Mon Sep 17 00:00:00 2001 From: robotmad Date: Tue, 12 May 2026 22:17:49 +0100 Subject: [PATCH 28/48] more efficient persist method --- EEPROM/hextest.py | 17 +++++++++-------- app.py | 2 +- settings_mgr.py | 11 ++++++----- 3 files changed, 16 insertions(+), 14 deletions(-) diff --git a/EEPROM/hextest.py b/EEPROM/hextest.py index af9452d..40abcd7 100644 --- a/EEPROM/hextest.py +++ b/EEPROM/hextest.py @@ -327,7 +327,7 @@ def deinitialise(self) -> bool: 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) + eventbus.remove(RequestForegroundPopEvent, self._lose_focus, self) # deinit any allocated Counters for counter in self._rotation_rate_counters: counter.deinit() @@ -355,7 +355,7 @@ async def _handle_removal(self, event: HexpansionRemovalEvent): print(f"H:HexDrive removed from port {event.port} during test - stopping test and returning to menu") self._hexdrive_app = None self._hexdrive_in_use_port = None - self._hexdrive_ports.remove(event.port) + self._hexdrive_ports.remove(event.port) self.notification = Notification("HexDrive Removed") self._stop_motor_test_mode() @@ -489,7 +489,7 @@ def _motor_test_start(self) -> bool: if self.logging: print(f"T:Found {type.name} on port(s): {ports}") self._hexdrive_ports.extend(ports) - break + break for port in self._hexdrive_ports: #hexpansion_app = self._find_hexpansion_app(port) @@ -1461,13 +1461,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"{SETTINGS_NAME_PREFIX}.{self._index()}", self.v) - else: - platform_settings.set(f"{SETTINGS_NAME_PREFIX}.{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}") # ------------------------------------------------------------------ diff --git a/app.py b/app.py index 8d9a86a..31e674e 100644 --- a/app.py +++ b/app.py @@ -234,7 +234,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 diff --git a/settings_mgr.py b/settings_mgr.py index 0dea589..8a9fd1b 100644 --- a/settings_mgr.py +++ b/settings_mgr.py @@ -102,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"{SETTINGS_NAME_PREFIX}.{self._index()}", self.v) - else: - platform_settings.set(f"{SETTINGS_NAME_PREFIX}.{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: From 42ba9b1cf843e077b8ca3523e9e3f5b5aeb28ea4 Mon Sep 17 00:00:00 2001 From: robotmad Date: Wed, 13 May 2026 01:36:02 +0100 Subject: [PATCH 29/48] minification --- .gitignore | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index 67f618e..e7fe9b8 100644 --- a/.gitignore +++ b/.gitignore @@ -6,4 +6,6 @@ BadgeBot.code-workspace .editorconfig .venv/ .venv-wsl*/ - +# minify.py build artefacts +vendor/**/*.min.py +vendor/**/*.renamed.py From 7e394062a7560d9483dd445c0a7e77dc0fd9f93a Mon Sep 17 00:00:00 2001 From: robotmad Date: Wed, 13 May 2026 17:37:21 +0100 Subject: [PATCH 30/48] mpy minification --- README.md | 41 +++++- dev/dev_requirements.txt | 3 +- dev/download_to_device.py | 26 ++-- dev/minify.py | 283 ++++++++++++++++++++++++++++++++++++++ 4 files changed, 340 insertions(+), 13 deletions(-) create mode 100644 dev/minify.py diff --git a/README.md b/README.md index 4951379..a7b6aba 100644 --- a/README.md +++ b/README.md @@ -17,8 +17,8 @@ If your HexDrive software (stored on the EEPROM on the hexpansion) is not the la - Unknown 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 ### @@ -72,7 +72,7 @@ The maximum allowed servo range is VERY WIDE - most Servos will not be able to c 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 @@ -108,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 @@ -211,6 +211,39 @@ 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` | +| `EEPROM/hextest.py` | `EEPROM/hextest.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/dev/dev_requirements.txt b/dev/dev_requirements.txt index 7870b62..10a8504 100644 --- a/dev/dev_requirements.txt +++ b/dev/dev_requirements.txt @@ -3,4 +3,5 @@ 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 c9fbd6b..3ee979c 100644 --- a/dev/download_to_device.py +++ b/dev/download_to_device.py @@ -35,13 +35,14 @@ class ModuleSpec: source: Path artifact: Path + minify: bool = False # Add new runtime modules here as the project grows. MODULES: tuple[ModuleSpec, ...] = ( - ModuleSpec(Path("EEPROM/hexdrive.py"), Path("EEPROM/hexdrive.mpy")), - ModuleSpec(Path("vendor/HexDrive2/hexdrive2.py"), Path("EEPROM/hexdrive2.mpy")), - ModuleSpec(Path("EEPROM/hextest.py"), Path("EEPROM/hextest.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")), @@ -431,11 +432,20 @@ def _compile_changed_modules( _log("SKP", f"compile {spec.source} (source unchanged)") continue - _log("INFO", f"compile {spec.source} -> {spec.artifact}") - _run_command( - [_tool("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}") diff --git a/dev/minify.py b/dev/minify.py new file mode 100644 index 0000000..c1b1e97 --- /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", + # Property with @rotation_rate_emitter_duty.setter – must NOT be renamed + "rotation_rate_emitter_duty", + # 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)] + 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()) From 8e77c999daa8bf32f4bbf754d6689a3e5142be79 Mon Sep 17 00:00:00 2001 From: robotmad Date: Thu, 14 May 2026 17:46:00 +0100 Subject: [PATCH 31/48] hextest compatibility with older badgeOS --- EEPROM/hextest.py | 101 +++++++++++++++++++++++++++++----------------- 1 file changed, 64 insertions(+), 37 deletions(-) diff --git a/EEPROM/hextest.py b/EEPROM/hextest.py index 40abcd7..c96d6c2 100644 --- a/EEPROM/hextest.py +++ b/EEPROM/hextest.py @@ -7,6 +7,7 @@ import asyncio import time + try: from typing import TYPE_CHECKING except ImportError: @@ -26,9 +27,50 @@ from system.scheduler.events import (RequestForegroundPopEvent, RequestForegroundPushEvent, RequestStopAppEvent) -from system.hexpansion.util import get_app_by_slot, get_slots_by_vid_pid from system.hexpansion.header import HexpansionHeader from system.scheduler import scheduler +try: + from system.hexpansion.util import get_app_by_slot, get_slots_by_vid_pid +except ImportError: + # In case we are running on old version of BadgeOS, where these functions are not available, define stubs that return None or empty lists. + from system.hexpansion.util import detect_eeprom_addr + + 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, _NUM_HEXPANSION_SLOTS + 1): + try: + i2c = I2C(port) + # 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 @@ -179,7 +221,7 @@ def __init__(self, config: HexpansionConfig | None = None): self._hexdrive_ports: list[int] = [] self._hexdrive_in_use_port: int | None = None - self.hexdrive_app: HexDriveLike | 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 @@ -367,7 +409,7 @@ async def _handle_mounted(self, event: HexpansionMountedEvent): # 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] - if (event.header.vid, event.header.pid) in vid_pid_pairs: + if hasattr(event, "header") and (event.header.vid, event.header.pid) in vid_pid_pairs: if self.logging: print(f"H:Attempting to use newly mounted HexDrive on port {event.port} for motor testing") if self._motor_test_start(): @@ -376,6 +418,11 @@ async def _handle_mounted(self, event: HexpansionMountedEvent): 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)") async def _gain_focus(self, event: RequestForegroundPushEvent): @@ -423,9 +470,9 @@ def _background_update(self, delta: int): 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: + 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 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: @@ -481,31 +528,30 @@ def _motor_test_start(self) -> bool: # Find HexDrive to test # look for any type of hexdrive (including HexDrive2 variants) in any port by their VID/PID - for type in self.HEXPANSION_TYPES: + for hexpansion_type in self.HEXPANSION_TYPES: if self.logging: - print(f"T:Looking for {type.name} (VID:PID {type.vid:04X}:{type.pid:04X}, Motors: {type.motors}, Servos: {type.servos})") - ports = get_slots_by_vid_pid(type.vid, type.pid) + print(f"T: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"T:Found {type.name} on port(s): {ports}") + print(f"T:Found {hexpansion_type.name} on port(s): {ports}") self._hexdrive_ports.extend(ports) break for port in self._hexdrive_ports: - #hexpansion_app = self._find_hexpansion_app(port) - hexpansion_app = get_app_by_slot(port) + 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"T:Found HexDrive app to test on port {port}") - self.hexdrive_app = _as_hexdrive_app(hexpansion_app) + self._hexdrive_app = hexpansion_app break - if self.hexdrive_app is not None: + if self._hexdrive_app is not None: #Setup UUT = HexDrive try: - if self.hexdrive_app.initialise() and self.hexdrive_app.set_power(True): - self.hexdrive_app.set_keep_alive(2000) # Updates can be quite slow as we are using the draw function + if self._hexdrive_app.initialise() and self._hexdrive_app.set_power(True): + self._hexdrive_app.set_keep_alive(2000) # Updates can be quite slow as we are using the draw function except AttributeError: pass @@ -564,10 +610,10 @@ def _stop_motor_test_mode(self): self._ina226_sensor_mgr = None self._ina226 = None - if self.hexdrive_app is not None: + if self._hexdrive_app is not None: try: - self.hexdrive_app.set_freq(0) - self.hexdrive_app.set_power(False) + self._hexdrive_app.set_freq(0) + self._hexdrive_app.set_power(False) except AttributeError: pass @@ -671,25 +717,6 @@ def _auto_rotation_rate_step(self): # HEXPANSION operations # ------------------------------------------------------------------ - def _find_hexpansion_app(self, port: int) -> HexDriveLike | None: - """Find the app instance running from the hexpansion on the given port, if any. Returns the app instance if found, None otherwise.""" - expected_app_name = "HexDriveApp" - candidate_app: HexDriveLike | None = None - for an_app in scheduler.apps: - if type(an_app).__name__ == expected_app_name: - 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 _as_hexdrive_app(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 = _as_hexdrive_app(an_app) - return candidate_app ### MAIN APP CONTROL FUNCTIONS ### From cf26dfb7dbff1342fa3e89e2e8209bb4d37ce5f4 Mon Sep 17 00:00:00 2001 From: robotmad Date: Fri, 15 May 2026 01:21:33 +0100 Subject: [PATCH 32/48] wip --- EEPROM/hextest.py | 105 +++++++++++++++++++++++++++++++++------------- app.py | 2 +- sensor_test.py | 3 +- 3 files changed, 78 insertions(+), 32 deletions(-) diff --git a/EEPROM/hextest.py b/EEPROM/hextest.py index c96d6c2..32a47ed 100644 --- a/EEPROM/hextest.py +++ b/EEPROM/hextest.py @@ -108,11 +108,11 @@ def _as_hexdrive_app(value): _ROTATION_RATE_SENSOR_ENABLE_PINS = [3, 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 = 2 # Step size for adjusting IR emitter brightness in manual mode, 0-255 (0=off, 255=full on) _POWER_SCALE_FACTOR = 66 - +_MOTOR_PWM_FREQUENCY = 20000 # Default PWM frequency to set on the HexDrive for testing, in Hz. # Rotation Rate Auto scan configuration _AUTO_SCAN_STEPS = 60 # Number of power levels to test during auto scan -_AUTO_SCAN_SETTLE_MS = 320 # ms to wait after setting power before starting actual measurement period +_AUTO_SCAN_SETTLE_MS = 500 # ms to wait after setting power before starting actual measurement period _AUTO_SCAN_MEASURE_MS = 5000 # ms measurement window per step (maximum) _AUTO_RESULTS_FILENAME = "mtrtst.csv" _AUTO_RESULTS_DEST_LABELS = ("badge fs", "hex fs") @@ -389,21 +389,21 @@ def _exit_app(self): # ------------------------------------------------------------------ async def _handle_removal(self, event: HexpansionRemovalEvent): - if self._foreground: + if event.port == self._hexdrive_in_use_port: if self._logging: print(f"H:Hexpansion removed from port {event.port}") - if event.port == self._hexdrive_in_use_port: - if self.logging: - print(f"H:HexDrive removed from port {event.port} during test - stopping test and returning to menu") - self._hexdrive_app = None - self._hexdrive_in_use_port = None - self._hexdrive_ports.remove(event.port) - self.notification = Notification("HexDrive Removed") + 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: + 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 @@ -411,10 +411,13 @@ async def _handle_mounted(self, event: HexpansionMountedEvent): vid_pid_pairs = [(type.vid, type.pid) for type in self.HEXPANSION_TYPES] if hasattr(event, "header") and (event.header.vid, event.header.pid) in vid_pid_pairs: if self.logging: - print(f"H:Attempting to use newly mounted HexDrive on port {event.port} for motor testing") + 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}") @@ -423,6 +426,8 @@ async def _handle_mounted(self, event: HexpansionMountedEvent): 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): @@ -525,7 +530,7 @@ def _motor_test_start(self) -> bool: 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: @@ -550,7 +555,7 @@ def _motor_test_start(self) -> bool: if self._hexdrive_app is not None: #Setup UUT = HexDrive try: - if self._hexdrive_app.initialise() and self._hexdrive_app.set_power(True): + 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 @@ -568,6 +573,14 @@ def _motor_test_start(self) -> bool: # 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"T: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) @@ -592,6 +605,7 @@ def _motor_test_start(self) -> bool: return False + def _stop_motor_test_mode(self): if self._logging: print("T:Stopping Motor Test mode and cleaning up") @@ -605,17 +619,17 @@ def _stop_motor_test_mode(self): try: self._ina226_sensor_mgr.close() except Exception as exc: # pylint: disable=broad-exception-caught - if self._logging: - print("T:INA226 sensor manager close failed:", exc) + print("T: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_freq(0) + self._hexdrive_app.set_motors((0, 0)) self._hexdrive_app.set_power(False) - except AttributeError: - pass + except AttributeError as e: + print(f"T: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: @@ -623,7 +637,16 @@ def _stop_motor_test_mode(self): self._rotation_rate_counters = [] self.update_period = DEFAULT_BACKGROUND_UPDATE_PERIOD self._rotation_rate_enable(False) - self.refresh = True + self.return_to_menu() + + + def _stop_sensor_test_mode(self): + if self._logging: + print("T: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: @@ -725,7 +748,7 @@ def update(self, delta: int): if not self._foreground: # This triggers the automatic foreground display eventbus.emit(RequestForegroundPushEvent(self)) - self._foreground = True + #self._foreground = True if self.notification: self.notification.update(delta) @@ -839,8 +862,8 @@ def _motor_test_update(self, delta: int): # pylint: disable=unused-argument self._rotation_rate_measurement_period = _AUTO_SCAN_MEASURE_MS self._auto_settling = True self._auto_results = [] - self._auto_max_rpm = 10 - self._auto_max_current_ma = 50 + self._auto_max_rpm = 0 + self._auto_max_current_ma = 0 self._rotation_detected = False self.refresh = True return @@ -1036,6 +1059,9 @@ def _motor_test_draw(self, ctx): colours += [(0.3, 0.8, 1.0)] #lines += [f"V:{self._ina226_reading.get('mV', 0)}mV"] #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="IR+", down_label="IR-", cancel_label="Back", left_label="Pwr-", right_label="Pwr+", confirm_label="Auto") @@ -1134,8 +1160,9 @@ def _draw_auto_scan(self, ctx): # Y axis Maximum RPM and Current labels ctx.font_size = label_font_size - 8 - ctx.rgb(0.0, 1.0, 0.5).move_to(chart_left+20, chart_top - 5).text(f"rpm:{max_rpm}") - ctx.rgb(1.0, 0.2, 0.2).move_to(5, chart_top - 5).text(f"mA:{max_current_ma}") + ctx.rgb(1.0, 1.0, 0.0).move_to(-15, chart_top - 5).text("Max") + ctx.rgb(0.0, 1.0, 0.5).move_to(chart_left+10, chart_top - 5).text(f"rpm:{self._auto_max_rpm}") + ctx.rgb(1.0, 0.2, 0.2).move_to(20, chart_top - 5).text(f"mA:{self._auto_max_current_ma}") #button_labels(ctx, cancel_label="Back", confirm_label="Manual") @@ -1390,13 +1417,31 @@ class HexpansionType: 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 + 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 diff --git a/app.py b/app.py index 31e674e..a99b018 100644 --- a/app.py +++ b/app.py @@ -30,7 +30,7 @@ from .utils import draw_logo_animated, parse_version HEXDRIVE_APP_VERSION = 7 -HEXDRIVE2_APP_VERSION = 7 +HEXDRIVE2_APP_VERSION = 1 HEXTEST_APP_VERSION = 1 SETTINGS_NAME_PREFIX = "badgebot." # Prefix for settings keys in EEPROM diff --git a/sensor_test.py b/sensor_test.py index b8245ce..731991c 100644 --- a/sensor_test.py +++ b/sensor_test.py @@ -1432,7 +1432,8 @@ def _stop_motor_test_mode(self): self._ina226 = None if len(app.hexdrive_apps) > 0: - app.hexdrive_apps[0].set_freq(0) + #app.hexdrive_apps[0].set_freq(0) + app.hexdrive_apps[0].set_motors((0,0)) app.hexdrive_apps[0].set_power(False) for c in self._rotation_rate_counters: From 7c5673b3a0c92c158cc48eae2fb45204a9201d22 Mon Sep 17 00:00:00 2001 From: robotmad Date: Mon, 18 May 2026 17:13:24 +0100 Subject: [PATCH 33/48] remember position in menus --- .gitignore | 1 + app.py | 12 +++++++++++- 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index e7fe9b8..ab4f7c5 100644 --- a/.gitignore +++ b/.gitignore @@ -9,3 +9,4 @@ BadgeBot.code-workspace # minify.py build artefacts vendor/**/*.min.py vendor/**/*.renamed.py +EEPROM/*.renamed.py diff --git a/app.py b/app.py index a99b018..b146524 100644 --- a/app.py +++ b/app.py @@ -190,6 +190,8 @@ def __init__(self): 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 @@ -514,6 +516,8 @@ def update_settings(self): 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 @@ -997,6 +1001,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 @@ -1008,13 +1013,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: @@ -1111,8 +1118,11 @@ 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): From b091d5c6093e969bcc723f41a2121549b1d716f5 Mon Sep 17 00:00:00 2001 From: robotmad Date: Mon, 18 May 2026 17:14:25 +0100 Subject: [PATCH 34/48] consistent use of _SLOTS across apps --- hexpansion_mgr.py | 32 ++++++++++++++++---------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/hexpansion_mgr.py b/hexpansion_mgr.py index a4b4020..dc1322f 100644 --- a/hexpansion_mgr.py +++ b/hexpansion_mgr.py @@ -25,7 +25,7 @@ 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 @@ -128,10 +128,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 @@ -307,7 +307,7 @@ 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.""" - state_idx = self._hexpansion_state_by_slot[self._port_selected - 1] if 1 <= self._port_selected <= _NUM_HEXPANSION_SLOTS else None + 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 @@ -601,7 +601,7 @@ def _update_state_upgrade(self, delta: int): # pylint: disable=unused-argume 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 @@ -613,7 +613,7 @@ def _report_hexpansion_states(self): return app = self._app print("H:Current Hexpansion States:") - for port in range(0, _NUM_HEXPANSION_SLOTS): + 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]] @@ -681,7 +681,7 @@ def _update_state_check(self, delta): # pylint: disable=unused-argument # 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: @@ -769,12 +769,12 @@ def _update_state_port_select(self, delta: int): # pylint: disable=unused-argu 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"]): @@ -812,7 +812,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 @@ -964,12 +964,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 +1005,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: From 2eb9cb4473db8b9f5cbd8549f4fb6eba544907de Mon Sep 17 00:00:00 2001 From: robotmad Date: Mon, 18 May 2026 17:15:10 +0100 Subject: [PATCH 35/48] improved user experience for power and acceleration values --- motor_moves.py | 31 +++++++++++++++++++------------ 1 file changed, 19 insertions(+), 12 deletions(-) diff --git a/motor_moves.py b/motor_moves.py index b9a6595..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() @@ -548,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) From fe73e547b8ea41ff39428813e467132d930ab396 Mon Sep 17 00:00:00 2001 From: robotmad Date: Mon, 18 May 2026 17:16:55 +0100 Subject: [PATCH 36/48] renaming to make use of variables etc clearer --- sensor_test.py | 125 ++++++++++++++++++++++++------------------------- 1 file changed, 62 insertions(+), 63 deletions(-) diff --git a/sensor_test.py b/sensor_test.py index 731991c..1333216 100644 --- a/sensor_test.py +++ b/sensor_test.py @@ -91,7 +91,7 @@ def _sleep_ms(delay_ms: int) -> None: _AUTO_SCAN_SETTLE_MS = 320 # ms to wait after setting power before starting actual measurement period _AUTO_SCAN_MEASURE_MS = 5000 # ms measurement window per step (maximum) _AUTO_RESULTS_FILENAME = "mtrtst.csv" -_AUTO_RESULTS_DEST_LABELS = ("badge fs", "hex fs") +_FILE_DEST_LABELS = ("Badge FS", "Hex FS") # Pages of information to show for each sensor (can be switched with up/down buttons) @@ -128,7 +128,7 @@ def _sleep_ms(delay_ms: int) -> None: def init_settings(s, MySetting: type): # pylint: disable=unused-argument, invalid-name """Register sensor-test-specific settings in the shared settings dict. Currently only the motor-test CSV destination is exposed here.""" - s["path"] = MySetting(s, 0, 0, len(_AUTO_RESULTS_DEST_LABELS) - 1, labels=_AUTO_RESULTS_DEST_LABELS) + s["path"] = MySetting(s, 0, 0, len(_FILE_DEST_LABELS) - 1, labels=_FILE_DEST_LABELS) # ---- Sensor Test manager --------------------------------------------------- @@ -170,16 +170,16 @@ def __init__(self, app, hextest_port: int | None = None, logging: bool = False): self._rotation_rate_spokes: int = _DEFAULT_SPOKES_PER_ROTATION # 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_settling: bool = True # True = in settle phase, False = in measure phase + self._scan_mode: bool = False # True = auto scanning, False = manual + 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._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._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_ms: 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._ina226 = None @@ -608,21 +608,21 @@ def background_update(self, delta) -> tuple[int, int] | None: # pylint: disable def _auto_rotation_rate_step(self): - self._auto_step += 1 + self._scan_step += 1 self._app.refresh = True - if self._auto_step >= _AUTO_SCAN_STEPS: + if self._scan_step >= _AUTO_SCAN_STEPS: # Scan complete — stop motors - self._auto_done = True + self._scan_done = True self._rotation_detected = False self._rotation_rate_motor_power = 0 - self._auto_direction *= -1 # reverse direction for next scan + self._scan_direction *= -1 # reverse direction for next scan self._auto_fit_calculate() - self._save_auto_results_csv() + self._save_capture_data_csv() else: # Advance to next power level - self._rotation_rate_motor_power = self._auto_direction * (65535 * self._auto_step) // (_AUTO_SCAN_STEPS - 1) + self._rotation_rate_motor_power = self._scan_direction * (65535 * self._scan_step) // (_AUTO_SCAN_STEPS - 1) self._rotation_rate_measurement_period_elapsed = 0 - self._auto_settling = True + self._capture_settling = True def _auto_results_dest_mode(self) -> int: @@ -678,8 +678,8 @@ def _auto_results_path(self) -> tuple[str | None, str | None, bool]: return f"/{_AUTO_RESULTS_FILENAME}", None, False - def _save_auto_results_csv(self) -> bool: - if len(self._auto_results) == 0: + def _save_capture_data_csv(self) -> bool: + if len(self._capture_data) == 0: return False output_path, mountpoint, mounted_here = self._auto_results_path() if output_path is None: @@ -687,15 +687,14 @@ def _save_auto_results_csv(self) -> bool: rpm_count = len(self._rotation_rate_rpms) header = ["pwr"] + [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._auto_results: + 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).encode()) + csv_file.write((",".join(row) + "\n").encode()) except Exception as exc: # pylint: disable=broad-exception-caught print(f"ST:Failed to save CSV {output_path}: {exc}") return False @@ -730,7 +729,7 @@ def _linear_regression(points: list[tuple[int, int]]) -> tuple[float, float] | N def _auto_fit_calculate(self) -> None: self._motor_calibration_fit = [] for index in range(len(self._rotation_rate_rpms)): - points = [(power, rpms[index]) for power, rpms, _ in self._auto_results if index < len(rpms)] + points = [(power, rpms[index]) for power, rpms, _ in self._capture_data if index < len(rpms)] self._motor_calibration_fit.append(self._linear_regression(points)) @@ -878,36 +877,36 @@ def _update_motor_test_mode(self, delta: int): # pylint: disable=unused-argumen 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._last_current_ms = 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: + if self._scan_mode: # Switch back to manual self._show_auto_results_fit() self._rotation_rate_measurement_period = _ROTATION_RATE_MEASUREMENT_PERIOD_MS - self._auto_mode = False - self._auto_done = False + self._scan_mode = False + self._scan_done = False else: # Start auto scan - self._auto_mode = True - self._auto_done = False - self._auto_step = 0 + self._scan_mode = True + self._scan_done = False + self._scan_step = 0 self._rotation_rate_measurement_period = _AUTO_SCAN_MEASURE_MS - self._auto_settling = True - self._auto_results = [] - self._auto_max_rpm = 10 - self._auto_max_current_ma = 50 + self._capture_settling = True + self._capture_data = [] + self._max_rpm = 10 + self._max_current_ma = 50 self._rotation_detected = False app.refresh = True return - if self._auto_mode: - if not self._auto_done: + if self._scan_mode: + if not self._scan_done: self._rotation_rate_measurement_period_elapsed += delta - if self._auto_settling: + 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 @@ -920,14 +919,14 @@ def _update_motor_test_mode(self, delta: int): # pylint: disable=unused-argumen 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 + self._last_current_ms = 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"ST:Auto Scan Step {self._auto_step}/{_AUTO_SCAN_STEPS} - Power: {power}, Rate: 0 rpm, Current: {current_ma}mA") - self._auto_results.append((power//66, [0] * len(self._rotation_rate_counters), current_ma)) + print(f"ST:Auto Scan Step {self._scan_step}/{_AUTO_SCAN_STEPS} - Power: {power}, Rate: 0 rpm, Current: {current_ma}mA") + self._capture_data.append((power//66, [0] * len(self._rotation_rate_counters), current_ma)) self._auto_rotation_rate_step() else: @@ -937,7 +936,7 @@ def _update_motor_test_mode(self, delta: int): # pylint: disable=unused-argumen 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._auto_settling = False + self._capture_settling = False self._reset_ina226_accumulators() else: if self._rotation_rate_measurement_period_elapsed >= self._rotation_rate_measurement_period: @@ -947,21 +946,21 @@ def _update_motor_test_mode(self, delta: int): # pylint: disable=unused-argumen 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._auto_max_rpm: - self._auto_max_rpm = rpm + 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._auto_last_current_ma = current_ma - if current_abs > self._auto_max_current_ma: - self._auto_max_current_ma = current_abs + self._last_current_ms = 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"ST:Auto Scan Step {self._auto_step}/{_AUTO_SCAN_STEPS} - Power: {power}, Rates: {self._rotation_rate_rpms} rpm, Current: {current_ma}mA") - self._auto_results.append((power//66, self._rotation_rate_rpms, current_ma)) + print(f"ST:Auto Scan Step {self._scan_step}/{_AUTO_SCAN_STEPS} - Power: {power}, Rates: {self._rotation_rate_rpms} rpm, Current: {current_ma}mA") + self._capture_data.append((power//66, self._rotation_rate_rpms, current_ma)) self._auto_rotation_rate_step() # In auto mode, no manual button control for power/IR @@ -1416,8 +1415,8 @@ def _stop_motor_test_mode(self): print(f"ST:Test results: {self._test_results}") app = self._app - self._auto_mode = False - self._auto_done = False + self._scan_mode = False + self._scan_done = False self._rotation_rate_motor_power = 0 self._ina226_reading = {} self._reset_ina226_accumulators() @@ -1468,7 +1467,7 @@ def draw(self, ctx): def _draw_motor_test_mode(self, ctx): if self._test_support_hexpansion_config is None: return - if self._auto_mode: + if self._scan_mode: self._draw_auto_scan(ctx) return #print("DRAWING") @@ -1510,9 +1509,9 @@ def _draw_auto_scan(self, ctx): 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 + 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 small bars. @@ -1521,7 +1520,7 @@ def _draw_auto_scan(self, ctx): # 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] + power, rpms, current_ma = self._capture_data[i] x = chart_left + (abs(power) * chart_w) // 100 for index, rpm in enumerate(rpms): h = (rpm * chart_h) // max_rpm @@ -1536,7 +1535,7 @@ def _draw_auto_scan(self, ctx): # Title and max RPM label ctx.font_size = label_font_size - if self._auto_done: + if self._scan_done: ctx.move_to(-50, chart_top - 25).text("Complete") ctx.font_size = label_font_size - 8 @@ -1554,8 +1553,8 @@ def _draw_auto_scan(self, ctx): continue slope, intercept = fit # get min and max power values from the scan range - left_power = self._auto_results[0][0] - right_power = self._auto_results[n-1][0] + 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) @@ -1574,14 +1573,14 @@ def _draw_auto_scan(self, ctx): ctx.rgb(*self._colour_for_index(index)).move_to(x1, y1).line_to(x2, y2).stroke() else: - progress = (self._auto_step * 100) // _AUTO_SCAN_STEPS + 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): ctx.rgb(*self._colour_for_index(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.2, 0.2).move_to(15, chart_bottom + 5 + ctx.font_size).text(f"{self._auto_last_current_ma}mA") + ctx.rgb(1.0, 0.2, 0.2).move_to(15, chart_bottom + 5 + ctx.font_size).text(f"{self._last_current_ms}mA") # Y axis Maximum RPM and Current labels ctx.font_size = label_font_size - 8 From d6d54bd8570ef261b743897f1d1f75875bea1f1f Mon Sep 17 00:00:00 2001 From: robotmad Date: Mon, 18 May 2026 17:17:33 +0100 Subject: [PATCH 37/48] remember settings menu position --- settings_mgr.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/settings_mgr.py b/settings_mgr.py index 8a9fd1b..47d18ef 100644 --- a/settings_mgr.py +++ b/settings_mgr.py @@ -142,8 +142,9 @@ def logging(self, value: bool): 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 From dc4f38cabd732b925a3a7d0c77d4a1d2fc2841c4 Mon Sep 17 00:00:00 2001 From: robotmad Date: Mon, 18 May 2026 17:18:56 +0100 Subject: [PATCH 38/48] use of const & set a default hexdrive type (uncommitted) in case of badge version issues to work it out --- EEPROM/hexdrive.py | 42 ++++++++++++++++++++++++------------------ 1 file changed, 24 insertions(+), 18 deletions(-) diff --git a/EEPROM/hexdrive.py b/EEPROM/hexdrive.py index 37c1a06..5dab02d 100644 --- a/EEPROM/hexdrive.py +++ b/EEPROM/hexdrive.py @@ -3,6 +3,11 @@ # This is the app to be installed from the HexDrive Hexpansion EEPROM. # 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 @@ -22,26 +27,26 @@ # HexDrive Hexpansion constants # Hardware defintions: -_ENABLE_PIN = 0 # First LS pin used to enable the SMPSU +_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) -_VID_ADDR = 0x10 # Address in the EEPROM where the Vendor ID (VID) byte is stored - used to identify the hardware version of the HexDrive -_PID_ADDR = 0x12 # Address in the EEPROM where the Product ID (PID) byte is stored - used to identify the type of Hexpansion +_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: @@ -62,6 +67,7 @@ def __init__(self, pid_byte: int, motors: int = 0, servos: int = 0, name: str = 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.""" @@ -472,7 +478,7 @@ def _set_pwmoutput(self, _channel: int, _duty_cycle: int) -> bool: def _check_port_for_hexdrive(self, port: int) -> HexDriveType | None: - + pid: int = 0 try: if hexpansion_app is not None: @@ -488,21 +494,21 @@ def _check_port_for_hexdrive(self, port: int) -> HexDriveType | None: 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}") + 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 None + return _DEFAULT_HEXDRIVE_TYPE # check which type of HexDrive this is by scanning the HEXDRIVE_TYPES list for _, hexpansion_type in enumerate(_HEXDRIVE_TYPES): # 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 as it was not recognised - return None + return _DEFAULT_HEXDRIVE_TYPE def _parse_version(self, version): From de26df923558bca772525f1884b53000a16b5f40 Mon Sep 17 00:00:00 2001 From: robotmad Date: Mon, 18 May 2026 17:20:09 +0100 Subject: [PATCH 39/48] use of const and some renaming to clarify use of variables etc --- EEPROM/hextest.py | 498 ++++++++++++++++++++++++++++------------------ 1 file changed, 306 insertions(+), 192 deletions(-) diff --git a/EEPROM/hextest.py b/EEPROM/hextest.py index 32a47ed..355975c 100644 --- a/EEPROM/hextest.py +++ b/EEPROM/hextest.py @@ -7,13 +7,13 @@ 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 @@ -29,12 +29,14 @@ RequestStopAppEvent) from system.hexpansion.header import HexpansionHeader from system.scheduler import scheduler +from system.hexpansion.util import ( + detect_eeprom_addr, + get_hexpansion_block_devices, + read_hexpansion_header, +) try: from system.hexpansion.util import get_app_by_slot, get_slots_by_vid_pid except ImportError: - # In case we are running on old version of BadgeOS, where these functions are not available, define stubs that return None or empty lists. - from system.hexpansion.util import detect_eeprom_addr - 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: @@ -46,7 +48,7 @@ def get_app_by_slot(slot): 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, _NUM_HEXPANSION_SLOTS + 1): + for port in range(1, 7): try: i2c = I2C(port) # Autodetect eeprom addr @@ -74,7 +76,11 @@ def get_slots_by_vid_pid(vid, pid): import app from tildagon import Pin as ePin -from micropython import const +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 @@ -92,67 +98,69 @@ def _as_hexdrive_app(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 -SETTINGS_NAME_PREFIX = "hextest." # Prefix for settings keys in EEPROM +_PRE = "hextest." # Prefix for settings keys in EEPROM # HexTest Hexpansion constants # Hardware defintions: -_NUM_HEXPANSION_SLOTS = 6 +_SLOTS = const(6) # 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 = [1, 2] # LS_B & LS_C 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, 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 = 2 # Step size for adjusting IR emitter brightness in manual mode, 0-255 (0=off, 255=full on) -_POWER_SCALE_FACTOR = 66 -_MOTOR_PWM_FREQUENCY = 20000 # Default PWM frequency to set on the HexDrive for testing, in Hz. +_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(25) # default duty cycle for the IR emitter when doing rate testing, 0-255 (0=off, 255=full on) +_DEFAULT_SPOKES_PER_ROTATION = const(3) # number of times the photodiode will be triggered per full rotation of the wheel +_MOTOR_TEST_BACKGROUND_UPDATE_PERIOD = const(1000) # background update period in ms to use during motor test mode (tradeoff between display responsiveness and CPU load) +_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 = 60 # Number of power levels to test during auto scan -_AUTO_SCAN_SETTLE_MS = 500 # ms to wait after setting power before starting actual measurement period -_AUTO_SCAN_MEASURE_MS = 5000 # ms measurement window per step (maximum) -_AUTO_RESULTS_FILENAME = "mtrtst.csv" -_AUTO_RESULTS_DEST_LABELS = ("badge fs", "hex fs") +_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(25535) # 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 = 0 -STATE_MESSAGE = 1 # Message display -STATE_SETTINGS = 2 # Edit Settings -STATE_SENSOR = 3 # Sensor Test -STATE_MOTOR_TEST = 4 # Motor Test +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 = 0 -MENU_ITEM_MOTOR_TEST = 1 -MENU_ITEM_SETTINGS = 2 -MENU_ITEM_ABOUT = 3 -MENU_ITEM_EXIT = 4 +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 = 100 # mS when not moving +DEFAULT_BACKGROUND_UPDATE_PERIOD = const(100) # mS when not moving _LOGGING = True -_AUTO_REPEAT_MS = 200 # Time between auto-repeats, in ms -_AUTO_REPEAT_COUNT_THRES = 10 # Number of auto-repeats before increasing level -_AUTO_REPEAT_SPEED_LEVEL_MAX = 4 # Maximum level of auto-repeat speed increases -_AUTO_REPEAT_LEVEL_MAX = 3 # Maximum level of auto-repeat digit increases +_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 = 0 -_PAGE_STATS = 1 -_PAGE_DATA = 2 -_PAGE_CAL = 3 +_PAGE_RAW = const(0) +_PAGE_STATS = const(1) +_PAGE_DATA = const(2) +_PAGE_CAL = const(3) _PAGE_NAMES = { - 0: "Raw", - 1: "Stats", - 2: "Data", - 3: "Cal", + _PAGE_RAW: "Raw", + _PAGE_STATS: "Stats", + _PAGE_DATA: "Data", + _PAGE_CAL: "Cal", } # Badge hexpansion HS pin to ESP32-S3 GPIO number mapping @@ -220,7 +228,6 @@ def __init__(self, config: HexpansionConfig | None = None): 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) @@ -231,18 +238,20 @@ def __init__(self, config: HexpansionConfig | None = None): self._rotation_rate_motor_power: int = 0 # Power applied to motors in TEST mode self._rotation_rate_spokes: int = _DEFAULT_SPOKES_PER_ROTATION - # 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_settling: bool = True # True = in settle phase, False = in measure phase + # 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._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._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_ms: 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._ina226 = None self._ina226_sensor_mgr = None # SensorManager used exclusively for motor-test INA226 discovery @@ -264,7 +273,7 @@ def __init__(self, config: HexpansionConfig | None = None): 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(_AUTO_RESULTS_DEST_LABELS) - 1, labels=_AUTO_RESULTS_DEST_LABELS) + self.settings["path"] = MySetting(self.settings, 0, 0, len(_FILE_DEST_LABELS) - 1, labels=_FILE_DEST_LABELS) self.update_settings() @@ -330,7 +339,7 @@ def update_settings(self): if self.logging: print("T:Updating settings from EEPROM") for s in self.settings: - self.settings[s].v = platform_settings.get(f"{SETTINGS_NAME_PREFIX}{s}", self.settings[s].d) + 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}") @@ -609,8 +618,8 @@ def _motor_test_start(self) -> bool: def _stop_motor_test_mode(self): if self._logging: print("T:Stopping Motor Test mode and cleaning up") - self._auto_mode = False - self._auto_done = False + self._scan_mode = False + self._scan_done = False self._rotation_rate_motor_power = 0 self._ina226_reading = {} self._reset_ina226_accumulators() @@ -720,21 +729,109 @@ def _consume_ina226_average(self) -> int | None: def _auto_rotation_rate_step(self): - self._auto_step += 1 + self._scan_step += 1 self.refresh = True - if self._auto_step >= _AUTO_SCAN_STEPS: + if self._scan_step >= _AUTO_SCAN_STEPS: # Scan complete — stop motors - self._auto_done = True + self._scan_done = True self._rotation_detected = False self._rotation_rate_motor_power = 0 - self._auto_direction *= -1 # reverse direction for next scan + self._scan_direction *= -1 # reverse direction for next scan #self._auto_fit_calculate() - #self._save_auto_results_csv() + self._save_capture_data_csv() else: # Advance to next power level - self._rotation_rate_motor_power = self._auto_direction * (65535 * self._auto_step) // (_AUTO_SCAN_STEPS - 1) + 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._auto_settling = True + 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 = "/hexcurrent" + eeprom_addr, addr_len = detect_eeprom_addr(self.config.i2c) + if eeprom_addr is None or addr_len is None: + print("HT:No EEPROM found on HexCurrent port") + return None, False + header = read_hexpansion_header(self.config.i2c, eeprom_addr=eeprom_addr, addr_len=addr_len) + if header is None: + print("HT:Failed to read HexCurrent 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 @@ -840,38 +937,42 @@ def _motor_test_update(self, delta: int): # pylint: disable=unused-argument # CONFIRM toggles between manual and auto mode elif self.button_states.get(BUTTON_TYPES["CONFIRM"]): self.button_states.clear() - self._rotation_rate_motor_power = 0 - self._auto_last_current_ma = 0 + self._last_current_ms = 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: + 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._auto_mode = False - self._auto_done = False + self._ina226_reading = {} + self._scan_mode = False + self._scan_done = False else: # Start auto scan - self._auto_mode = True - self._auto_done = False - self._auto_step = 0 + self._scan_mode = True + self._scan_done = False + self._scan_step = 0 + self._rotation_rate_motor_power = _AUTO_SCAN_MIN_POWER self._rotation_rate_measurement_period = _AUTO_SCAN_MEASURE_MS - self._auto_settling = True - self._auto_results = [] - self._auto_max_rpm = 0 - self._auto_max_current_ma = 0 + 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._auto_mode: - if not self._auto_done: + if self._scan_mode: + if not self._scan_done: self._rotation_rate_measurement_period_elapsed += delta - if self._auto_settling: + 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 @@ -884,15 +985,17 @@ def _motor_test_update(self, delta: int): # pylint: disable=unused-argument 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 + self._last_current_ms = 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"T:Auto Scan Step {self._auto_step}/{_AUTO_SCAN_STEPS} - Power: {power}, Rate: 0 rpm, Current: {current_ma}mA") - self._auto_results.append((power//_POWER_SCALE_FACTOR, [0] * len(self._rotation_rate_counters), current_ma)) + print(f"T:Auto Scan Step {self._scan_step}/{_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 self._unsaved_data: + self._unsaved_data = True else: self._rotation_detected = True @@ -901,7 +1004,7 @@ def _motor_test_update(self, delta: int): # pylint: disable=unused-argument 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._auto_settling = False + self._capture_settling = False self._reset_ina226_accumulators() else: if self._rotation_rate_measurement_period_elapsed >= self._rotation_rate_measurement_period: @@ -911,22 +1014,24 @@ def _motor_test_update(self, delta: int): # pylint: disable=unused-argument 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._auto_max_rpm: - self._auto_max_rpm = rpm + 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._auto_last_current_ma = current_ma - if current_abs > self._auto_max_current_ma: - self._auto_max_current_ma = current_abs + self._last_current_ms = 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"T:Auto Scan Step {self._auto_step}/{_AUTO_SCAN_STEPS} - Power: {power}, Rates: {self._rotation_rate_rpms} rpm, Current: {current_ma}mA") - self._auto_results.append((power//_POWER_SCALE_FACTOR, self._rotation_rate_rpms, current_ma)) + print(f"T:Auto Scan Step {self._scan_step}/{_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 @@ -947,25 +1052,27 @@ def _motor_test_update(self, delta: int): # pylint: disable=unused-argument # Manual mode button handling if self.button_states.get(BUTTON_TYPES["UP"]): self.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"T:IR+Emitter Duty: {self.rotation_rate_emitter_duty}") + self._hut_id += 1 + #self.rotation_rate_emitter_duty = min(255, self.rotation_rate_emitter_duty + _IR_EMITTER_PWM_STEP_SIZE) + #if self.logging: + # print(f"T:IR+Emitter Duty: {self.rotation_rate_emitter_duty}") self.refresh = True elif self.button_states.get(BUTTON_TYPES["DOWN"]): self.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"T:IR-Emitter Duty: {self.rotation_rate_emitter_duty}") + self._hut_id = max(0, self._hut_id - 1) + #self.rotation_rate_emitter_duty = max(0, self.rotation_rate_emitter_duty - _IR_EMITTER_PWM_STEP_SIZE) + #if self.logging: + # print(f"T: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(65535, self._rotation_rate_motor_power + 1000) + self._rotation_rate_motor_power = min(_MAX_POWER, self._rotation_rate_motor_power + 1000) if self.logging: print(f"T: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(-65535, self._rotation_rate_motor_power - 1000) + self._rotation_rate_motor_power = max(-_MAX_POWER, self._rotation_rate_motor_power - 1000) if self.logging: print(f"T:Motor-Power: {self._rotation_rate_motor_power}") self.refresh = True @@ -1040,12 +1147,13 @@ def draw_message(ctx, message, colours, size=label_font_size): def _motor_test_draw(self, ctx): if self.config is None: return - if self._auto_mode: + if self._scan_mode: self._draw_auto_scan(ctx) return #print("DRAWING") # 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)}%"] + #lines = [f"IR:{int(self.rotation_rate_emitter_duty * 100 // 255)}%"] + lines = [f"ID:{self._hut_id}"] # show the current HUT ID for data logging purposes colours = [(1, 1, 0)] # Show power lines += [f"Pwr:{self._rotation_rate_motor_power}"] @@ -1055,16 +1163,14 @@ def _motor_test_draw(self, ctx): lines += [f"{index}: {rpm}rpm"] colours += [(1, 0, 1)] if self._ina226_reading: - lines += [f"I:{self._ina226_reading.get('mA', 0)}mA"] + 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)] - #lines += [f"V:{self._ina226_reading.get('mV', 0)}mV"] - #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="IR+", down_label="IR-", cancel_label="Back", - left_label="Pwr-", right_label="Pwr+", confirm_label="Auto") + button_labels(ctx, up_label="ID+", down_label="ID-", cancel_label="Back", + left_label="Pwr-", right_label="Pwr+", confirm_label="Scan") def _draw_auto_scan(self, ctx): @@ -1085,19 +1191,18 @@ def _draw_auto_scan(self, ctx): 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 + 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 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. + # 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._auto_results[i] - x = chart_left + (abs(power) * chart_w) // (65536//_POWER_SCALE_FACTOR) + 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: @@ -1111,11 +1216,11 @@ def _draw_auto_scan(self, ctx): # Title and max RPM label ctx.font_size = label_font_size - if self._auto_done: + if self._scan_done: ctx.move_to(-50, chart_top - 25).text("Complete") 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("0%") + 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%") @@ -1129,8 +1234,8 @@ def _draw_auto_scan(self, ctx): continue slope, intercept = fit # get min and max power values from the scan range - left_power = self._auto_results[0][0] - right_power = self._auto_results[n-1][0] + 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) @@ -1149,22 +1254,23 @@ def _draw_auto_scan(self, ctx): ctx.rgb(*self._colour_for_index(index)).move_to(x1, y1).line_to(x2, y2).stroke() else: - progress = (self._auto_step * 100) // _AUTO_SCAN_STEPS + 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): ctx.rgb(*self._colour_for_index(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.2, 0.2).move_to(15, chart_bottom + 5 + ctx.font_size).text(f"{self._auto_last_current_ma}mA") + ctx.rgb(1.0, 0.2, 0.2).move_to(15, chart_bottom + 5 + ctx.font_size).text(f"{self._last_current_ms}mA") # Y axis Maximum RPM and Current labels ctx.font_size = label_font_size - 8 ctx.rgb(1.0, 1.0, 0.0).move_to(-15, chart_top - 5).text("Max") - ctx.rgb(0.0, 1.0, 0.5).move_to(chart_left+10, chart_top - 5).text(f"rpm:{self._auto_max_rpm}") - ctx.rgb(1.0, 0.2, 0.2).move_to(20, chart_top - 5).text(f"mA:{self._auto_max_current_ma}") + ctx.rgb(0.0, 1.0, 0.5).move_to(chart_left+10, chart_top - 5).text(f"rpm:{self._max_rpm}") + ctx.rgb(1.0, 0.2, 0.2).move_to(20, chart_top - 5).text(f"mA:{self._max_current_ma}") + + button_labels(ctx, confirm_label="OK" if self._scan_done else "Quit") - #button_labels(ctx, cancel_label="Back", confirm_label="Manual") def _colour_for_index(self, index: int) -> tuple[float, float, float]: if index == 0: @@ -1303,6 +1409,7 @@ def _menu_back_handler(self): # 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 '+'.""" @@ -1536,7 +1643,7 @@ def persist(self): index = self._index() if index is None: return - key = f"{SETTINGS_NAME_PREFIX}.{index}" + 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 @@ -1553,7 +1660,7 @@ def persist(self): _GPIO_BASE = const(0x60004000) _PCNT_BASE = const(0x60017000) -_PCNT_NUM_UNITS = 4 # ESP32-S3 has 4 PCNT units +_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) @@ -1582,7 +1689,7 @@ def persist(self): _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 = 33 +_PCNT_SIG_BASE = const(33) # APB clock frequency for filter calculation (Hz) _APB_CLK_HZ = const(80_000_000) @@ -1965,80 +2072,80 @@ def _write_u16_be(self, reg: int, value: int): # Register map -_REG_CONFIGURATION = 0x00 # Configuration register -_REG_SHUNT_VOLTAGE = 0x01 # Shunt voltage result (signed) -_REG_BUS_VOLTAGE = 0x02 # Bus voltage result (unsigned) -_REG_POWER = 0x03 # Power result (unsigned) -_REG_CURRENT = 0x04 # Current result (signed) -_REG_CALIBRATION = 0x05 # Calibration register -_REG_MASK_ENABLE = 0x06 # Alert mask/enable register -_REG_ALERT_LIMIT = 0x07 # Alert threshold register -_REG_MANUFACTURER_ID = 0xFE # Manufacturer ID register -_REG_DIE_ID = 0xFF # Die ID register +_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 = 0x8000 # Software reset bit -_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) +_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 = 0b000 # 1 sample average -_CFG_AVG_4 = 0b001 # 4 sample average -_CFG_AVG_16 = 0b010 # 16 sample average -_CFG_AVG_64 = 0b011 # 64 sample average -_CFG_AVG_128 = 0b100 # 128 sample average -_CFG_AVG_256 = 0b101 # 256 sample average -_CFG_AVG_512 = 0b110 # 512 sample average -_CFG_AVG_1024 = 0b111 # 1024 sample average +_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 = 0b000 # 140 us conversion time -_CFG_CT_204US = 0b001 # 204 us conversion time -_CFG_CT_332US = 0b010 # 332 us conversion time -_CFG_CT_588US = 0b011 # 588 us conversion time -_CFG_CT_1100US = 0b100 # 1.1 ms conversion time -_CFG_CT_2116US = 0b101 # 2.116 ms conversion time -_CFG_CT_4156US = 0b110 # 4.156 ms conversion time -_CFG_CT_8244US = 0b111 # 8.244 ms conversion time +_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 = 0b000 # Power-down mode -_CFG_MODE_SHUNT_TRIG = 0b001 # Shunt voltage, triggered -_CFG_MODE_BUS_TRIG = 0b010 # Bus voltage, triggered -_CFG_MODE_SHUNT_BUS_TRIG = 0b011 # Shunt and bus, triggered -_CFG_MODE_ADC_OFF = 0b100 # ADC off (disabled) -_CFG_MODE_SHUNT_CONT = 0b101 # Shunt voltage, continuous -_CFG_MODE_BUS_CONT = 0b110 # Bus voltage, continuous -_CFG_MODE_SHUNT_BUS_CONT = 0b111 # Shunt and bus, continuous +_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 = 0x8000 # Shunt over-voltage alert flag -_MASK_SUL = 0x4000 # Shunt under-voltage alert flag -_MASK_BOL = 0x2000 # Bus over-voltage alert flag -_MASK_BUL = 0x1000 # Bus under-voltage alert flag -_MASK_POL = 0x0800 # Power over-limit alert flag -_MASK_CNVR = 0x0400 # Conversion ready alert flag -_MASK_AFF = 0x0010 # Alert function flag -_MASK_CVRF = 0x0008 # Conversion ready flag -_MASK_OVF = 0x0004 # Math overflow flag -_MASK_APOL = 0x0002 # Alert pin polarity select -_MASK_LEN = 0x0001 # Alert latch enable +_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 = 0x5449 # Texas Instruments manufacturer ID +_MANUFACTURER_ID_TI = const(0x5449) # Texas Instruments manufacturer ID # Driver configuration constants (100 mΩ shunt) -_SHUNT_RESISTOR_MILLIOHM = 100 -_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 = 50 +_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 @@ -2127,9 +2234,9 @@ def _shutdown(self) -> None: -_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 +_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] @@ -2343,6 +2450,13 @@ def get_sensor_by_name(self, name: str) -> SensorBase | None: 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 From ffc3ef4e53f443a7b515aa254ec95d522b82db38 Mon Sep 17 00:00:00 2001 From: robotmad Date: Tue, 19 May 2026 18:07:03 +0100 Subject: [PATCH 40/48] Update vendor HexDrive2 submodule to latest --- vendor/HexDrive2 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/vendor/HexDrive2 b/vendor/HexDrive2 index 080b31a..2def42b 160000 --- a/vendor/HexDrive2 +++ b/vendor/HexDrive2 @@ -1 +1 @@ -Subproject commit 080b31afa10eae7551300fecf19bbc6fafdadd45 +Subproject commit 2def42b0d749c669721cc07ad0a09ed1a6406ff2 From 217f4c62dddd54f1afa1e22df5b0ea760c26c900 Mon Sep 17 00:00:00 2001 From: robotmad Date: Sun, 31 May 2026 15:07:07 +0100 Subject: [PATCH 41/48] remove hextest from BadgeBot - now runs as its own app --- README.md | 1 - hexpansion_mgr.py | 3 --- 2 files changed, 4 deletions(-) diff --git a/README.md b/README.md index a7b6aba..ca98a97 100644 --- a/README.md +++ b/README.md @@ -219,7 +219,6 @@ Hexpansion apps stored on EEPROM are minified before being compiled to `.mpy` to |--------|----------| | `vendor/HexDrive2/hexdrive2.py` | `EEPROM/hexdrive2.mpy` | | `EEPROM/hexdrive.py` | `EEPROM/hexdrive.mpy` | -| `EEPROM/hextest.py` | `EEPROM/hextest.mpy` | The pipeline uses `dev/minify.py` which: 1. Renames internal `self.*` attributes to short names via an AST transform (source stays readable) diff --git a/hexpansion_mgr.py b/hexpansion_mgr.py index dc1322f..5469eaf 100644 --- a/hexpansion_mgr.py +++ b/hexpansion_mgr.py @@ -213,9 +213,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.""" From d44c7ac9c16927a0e125369e5dff855e8955982c Mon Sep 17 00:00:00 2001 From: robotmad Date: Sun, 31 May 2026 15:09:22 +0100 Subject: [PATCH 42/48] use new hexpansion support and make robust against micropython v1.28 PWM changes --- EEPROM/hexdrive.py | 9 +- sensor_test.py | 921 ++------------------------------------------- 2 files changed, 34 insertions(+), 896 deletions(-) diff --git a/EEPROM/hexdrive.py b/EEPROM/hexdrive.py index 5dab02d..befa401 100644 --- a/EEPROM/hexdrive.py +++ b/EEPROM/hexdrive.py @@ -334,7 +334,9 @@ def set_servoposition(self, channel: int | None = None, position: int | None = N # 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[physical_channel] = PWM(self.config.pin[physical_channel], freq = self._freq[channel], duty_ns = pulse_width_in_ns) + 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 @@ -461,7 +463,8 @@ def _set_pwmoutput(self, _channel: int, _duty_cycle: int) -> bool: if self.PWMOutput[_channel] is None: # Channel hasn't been setup yet so we need to initialise it from scratch pin = self.config.pin[_channel] - self.PWMOutput[_channel] = PWM(pin, freq = self._freq[_channel], duty_u16 = _duty_cycle) + 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] @@ -472,7 +475,7 @@ def _set_pwmoutput(self, _channel: int, _duty_cycle: int) -> bool: #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 diff --git a/sensor_test.py b/sensor_test.py index 1333216..1ac6252 100644 --- a/sensor_test.py +++ b/sensor_test.py @@ -16,20 +16,15 @@ from app_components.tokens import label_font_size, button_labels from app_components.notification import Notification from system.hexpansion.config import HexpansionConfig -from system.hexpansion.util import detect_eeprom_addr, get_hexpansion_block_devices, read_hexpansion_header import settings as platform_settings -import vfs -from egpio import ePin from .sensor_manager import SensorManager -from .app import SETTINGS_NAME_PREFIX, DEFAULT_BACKGROUND_UPDATE_PERIOD, MOTOR_PWM_FREQ, STATE_SENSOR +from .app import SETTINGS_NAME_PREFIX, DEFAULT_BACKGROUND_UPDATE_PERIOD 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 @@ -70,28 +65,9 @@ def _sleep_ms(delay_ms: int) -> None: 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 = [1, 2] # LS_B & LS_C 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, 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 = 2 # Step size for adjusting IR emitter brightness in manual mode, 0-255 (0=off, 255=full on) - - # 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 = 60 # Number of power levels to test during auto scan -_AUTO_SCAN_SETTLE_MS = 320 # ms to wait after setting power before starting actual measurement period -_AUTO_SCAN_MEASURE_MS = 5000 # ms measurement window per step (maximum) -_AUTO_RESULTS_FILENAME = "mtrtst.csv" -_FILE_DEST_LABELS = ("Badge FS", "Hex FS") # Pages of information to show for each sensor (can be switched with up/down buttons) @@ -123,13 +99,18 @@ def _sleep_ms(delay_ms: 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 only the motor-test CSV destination is exposed here.""" - s["path"] = MySetting(s, 0, 0, len(_FILE_DEST_LABELS) - 1, labels=_FILE_DEST_LABELS) - + """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 --------------------------------------------------- @@ -142,7 +123,7 @@ class SensorTestMgr: Reference to the main application instance. """ - def __init__(self, app, hextest_port: int | None = None, logging: bool = False): + def __init__(self, app, logging: bool = False): self._app = app self._sub_state = _SUB_SELECT_PORT self._sensor_mgr: SensorManager | None = None @@ -161,38 +142,6 @@ def __init__(self, app, hextest_port: int | None = None, logging: bool = False): 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 - 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] = [] # 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 - self._rotation_rate_spokes: int = _DEFAULT_SPOKES_PER_ROTATION - - # Auto scan state - self._scan_mode: bool = False # True = auto scanning, False = manual - 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_ms: 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._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 - - # Use HS pins on a spare Hexpansion to measure rotation rate - self._test_support_hexpansion_config: HexpansionConfig | None = None - self.hextest_setup(hextest_port) - if self._logging: print("SensorTestMgr initialised") @@ -220,125 +169,6 @@ def sample_count(self) -> int: def sample_count(self, value: int): self._sample_count = value - @property - def _rotation_rate_rounding(self) -> int: - return (self._rotation_rate_measurement_period * self._rotation_rate_spokes) // 2 - - - 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 - - - def _rotation_rate_sensor_pair(self, pair_index: int = 0) -> tuple[int, int] | None: - """Return the requested HS sensor pin pair from `_ROTATION_RATE_SENSOR_PINS`.""" - start = pair_index * 2 - if start < 0 or start + 1 >= len(_ROTATION_RATE_SENSOR_PINS): - return None - return _ROTATION_RATE_SENSOR_PINS[start], _ROTATION_RATE_SENSOR_PINS[start + 1] - - - def encoder_smoke_test( - self, - samples: int = 12, - interval_ms: int = 250, - filter_ns: int = 1_000_000, - max: int | None = 3, - min: int = 0, - ) -> bool: - """Run a short console-based encoder smoke test on the first HexTest sensor pair.""" - if samples <= 0: - print("S:Encoder smoke test requires at least one sample") - return False - if interval_ms < 0: - print("S:Encoder smoke test requires interval_ms >= 0") - return False - if self._sub_state == _SUB_MOTOR_TEST: - print("S:Encoder smoke test unavailable while motor test mode is active") - return False - - config = self._test_support_hexpansion_config - if config is None: - print("S:Encoder smoke test requires a HexTest Hexpansion") - return False - - hs_pair = self._rotation_rate_sensor_pair(0) - if hs_pair is None: - print("S:Encoder smoke test requires at least one HS sensor pin pair") - return False - - gpios = _HS_PIN_TO_GPIO.get(config.port) - if gpios is None: - print(f"S:Encoder smoke test does not know the GPIO mapping for port {config.port}") - return False - - phase_a_pin, phase_b_pin = hs_pair - phase_a_gpio = gpios[phase_a_pin] - phase_b_gpio = gpios[phase_b_pin] - range_desc = "hardware range" if max is None else f"min={min}, max={max}" - - self._rotation_rate_enable(True) - encoder = Encoder( - None, - phase_a_gpio, - phase_b_gpio, - filter_ns=filter_ns, - max=max, - min=min, - logging=True, - ) - if encoder.unit is None: - print( - f"S:Encoder smoke test failed on HexTest port {config.port} " - f"HS pins {phase_a_pin}/{phase_b_pin}" - ) - self._rotation_rate_enable(False) - return False - - print( - f"S:Encoder smoke test on HexTest port {config.port}, " - f"HS pins {phase_a_pin}/{phase_b_pin}, GPIOs {phase_a_gpio}/{phase_b_gpio}, {range_desc}" - ) - print("S:Rotate the wheel by hand and watch position/cycles for direction and wrap behaviour") - - try: - print(f"S:Encoder initial: position={encoder.value()}, cycles={encoder.cycles()}") - for sample_index in range(samples): - _sleep_ms(interval_ms) - print( - f"S:Encoder sample {sample_index + 1}/{samples}: " - f"position={encoder.value()}, cycles={encoder.cycles()}" - ) - - final_position = encoder.value() - final_cycles = encoder.cycles() - encoder.value(0) - print( - f"S:Encoder reset after position={final_position}, cycles={final_cycles}; " - f"now position={encoder.value()}, cycles={encoder.cycles()}" - ) - print("S:Encoder smoke test complete") - return True - finally: - encoder.deinit() - self._rotation_rate_enable(False) - # ------------------------------------------------------------------ # Entry point from menu @@ -354,11 +184,7 @@ def start(self) -> bool: 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 HexTest is present then go straight to motor test mode. - if len(app.hexdrive_apps) > 0 and self._test_support_hexpansion_config is not None and self._start_motor_test_mode(): - self._port_selected = self._test_support_hexpansion_config.port - self._sub_state = _SUB_MOTOR_TEST - elif app.hexdrive_ports is not None: + 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): @@ -426,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 """ @@ -577,16 +390,18 @@ def background_update(self, delta) -> tuple[int, int] | None: # pylint: disable # 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._app.hexdrive_ports[0]) + config = HexpansionConfig(self._port_selected) if sensor_mgr.type == "Colour": - if config.ls_pin[1].value(): + 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[3].value(): - return None - self._test_results["distance int low"] = True + 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() @@ -595,158 +410,15 @@ def background_update(self, delta) -> tuple[int, int] | None: # pylint: disable self._sensor_data = {"Error": str(e)} if sensor_mgr.type == "Colour": - if config.ls_pin[1].value(): + if config.ls_pin[_COLOUR_INT_PIN].value(): self._test_results["colour int high"] = True elif sensor_mgr.type == "Distance": - if config.ls_pin[3].value(): + if config.ls_pin[_DIST_INT_PIN].value(): self._test_results["distance int high"] = 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 - def _auto_rotation_rate_step(self): - self._scan_step += 1 - self._app.refresh = True - if self._scan_step >= _AUTO_SCAN_STEPS: - # Scan complete — stop motors - self._scan_done = True - self._rotation_detected = False - self._rotation_rate_motor_power = 0 - self._scan_direction *= -1 # reverse direction for next scan - self._auto_fit_calculate() - self._save_capture_data_csv() - else: - # Advance to next power level - self._rotation_rate_motor_power = self._scan_direction * (65535 * self._scan_step) // (_AUTO_SCAN_STEPS - 1) - self._rotation_rate_measurement_period_elapsed = 0 - self._capture_settling = True - - - def _auto_results_dest_mode(self) -> int: - setting = self._app.settings.get("path") - if setting is None: - return 0 - try: - return int(setting.v) - except Exception: # pylint: disable=broad-exception-caught - return 0 - - - def _mount_hexdrive_fs(self, port: int) -> tuple[str | None, bool]: - mountpoint = f"/hexpansion_{port}" - config = HexpansionConfig(port) - eeprom_addr, addr_len = detect_eeprom_addr(config.i2c) - if eeprom_addr is None or addr_len is None: - print(f"ST:No EEPROM found on hexdrive port {port}") - return None, False - header = read_hexpansion_header(config.i2c, eeprom_addr=eeprom_addr, addr_len=addr_len) - if header is None: - print(f"ST:Failed to read hexdrive header on port {port}") - return None, False - try: - _, partition = get_hexpansion_block_devices(config.i2c, header, eeprom_addr, addr_len=addr_len) - except RuntimeError as exc: - print(f"ST:Failed to get hexdrive 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"ST:Failed to mount {mountpoint}: {exc}") - return None, False - except Exception as exc: # pylint: disable=broad-exception-caught - print(f"ST:Failed to mount {mountpoint}: {exc}") - return None, False - return mountpoint, mounted_here - - - def _auto_results_path(self) -> tuple[str | None, str | None, bool]: - if self._auto_results_dest_mode() == 1: - if len(self._app.hexdrive_ports) == 0: - print("ST:No HexDrive present for hex fs CSV save") - return None, None, False - mountpoint, mounted_here = self._mount_hexdrive_fs(self._app.hexdrive_ports[0]) - if mountpoint is None: - return None, None, False - return f"{mountpoint}/{_AUTO_RESULTS_FILENAME}", mountpoint, mounted_here - return f"/{_AUTO_RESULTS_FILENAME}", None, False - - - def _save_capture_data_csv(self) -> bool: - if len(self._capture_data) == 0: - return False - output_path, mountpoint, mounted_here = self._auto_results_path() - if output_path is None: - return False - - rpm_count = len(self._rotation_rate_rpms) - header = ["pwr"] + [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"ST:Failed to save CSV {output_path}: {exc}") - 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"ST:Failed to unmount {mountpoint}: {exc}") - - print(f"ST:Saved auto motor test CSV to {output_path}") - return True - - - @staticmethod - def _linear_regression(points: list[tuple[int, int]]) -> tuple[float, float] | None: - if len(points) < 2: - return None - count = len(points) - sum_x = sum(point[0] for point in points) - sum_y = sum(point[1] for point in points) - sum_xx = sum(point[0] * point[0] for point in points) - sum_xy = sum(point[0] * point[1] for point in points) - denominator = (count * sum_xx) - (sum_x * sum_x) - if denominator == 0: - return None - slope = ((count * sum_xy) - (sum_x * sum_y)) / denominator - intercept = (sum_y - (slope * sum_x)) / count - return slope, intercept - - - def _auto_fit_calculate(self) -> None: - self._motor_calibration_fit = [] - for index in range(len(self._rotation_rate_rpms)): - points = [(power, rpms[index]) for power, rpms, _ in self._capture_data if index < len(rpms)] - self._motor_calibration_fit.append(self._linear_regression(points)) - - - def _show_auto_results_fit(self) -> None: - lines = ["Auto Scan Fit"] - colours: list[tuple[float, float, float]] = [(1, 1, 0)] - for index in range(len(self._rotation_rate_rpms)): - fit = self._motor_calibration_fit[index] if index < len(self._motor_calibration_fit) else None - if fit is None: - lines.append(f"M{index + 1}: n/a") - else: - slope, intercept = fit - lines.append(f"M{index + 1}: r={slope:.3f}p{intercept:+.1f}") - colours.append(self._colour_for_index(index)) - self._app.show_message(lines, colours, return_state=STATE_SENSOR) - - # ------------------------------------------------------------------ # Per-tick update # ------------------------------------------------------------------ @@ -757,253 +429,6 @@ 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("ST:Enabling rotation rate emitters 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("ST:Disabling rotation rate emitters 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() - try: - #from .sensor_manager import SensorManager - 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, 7): - if not mgr.open(port): - mgr.close() - if self._logging: - print(f"ST: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"ST: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"ST: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) -> None: - 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"ST: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 _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._show_auto_results_fit() - 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._last_current_ms = 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_measurement_period = _ROTATION_RATE_MEASUREMENT_PERIOD_MS - 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._rotation_rate_measurement_period = _AUTO_SCAN_MEASURE_MS - self._capture_settling = True - self._capture_data = [] - self._max_rpm = 10 - self._max_current_ma = 50 - self._rotation_detected = False - app.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_ms = 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"ST:Auto Scan Step {self._scan_step}/{_AUTO_SCAN_STEPS} - Power: {power}, Rate: 0 rpm, Current: {current_ma}mA") - self._capture_data.append((power//66, [0] * len(self._rotation_rate_counters), current_ma)) - self._auto_rotation_rate_step() - - 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_ms = 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"ST:Auto Scan Step {self._scan_step}/{_AUTO_SCAN_STEPS} - Power: {power}, Rates: {self._rotation_rate_rpms} rpm, Current: {current_ma}mA") - self._capture_data.append((power//66, self._rotation_rate_rpms, 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 >= 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"ST: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"ST:IR+Emitter Duty: {self.rotation_rate_emitter_duty}") - app.refresh = True - 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"ST:IR-Emitter Duty: {self.rotation_rate_emitter_duty}") - app.refresh = True - 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"ST:Motor+Power: {self._rotation_rate_motor_power}") - app.refresh = True - 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"ST:Motor-Power: {self._rotation_rate_motor_power}") - app.refresh = True def _setup_for_sensor_type(self): @@ -1044,30 +469,19 @@ 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: - if self._start_motor_test_mode(): - app.notification = Notification("Motor Test", port=self._port_selected) - if self.logging: - print(f"ST: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() - app.refresh = True - if sensor_mgr.open(self._port_selected): - - self._setup_for_sensor_type() - 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() @@ -1303,149 +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) - # Read INA226: - if self._init_ina226_for_motor_test(): - if self._ina226 is not None: - ina226 = self._ina226 - data = ina226.read(timeout=160) - try: - volts = int(data.get("mV", 0)) - amps = int(data.get("mA", 0)) - except Exception as e: # pylint: disable=broad-exception-caught - print(f"ST:Error reading INA226 data: {e}") - else: - if 3000 <= volts <= 3200 and amps < 5: - if self.logging: - print("ST:INA226 initial voltage & current reading OK") - self._test_results["Power Off"] = True - else: - self._test_results["Power Off"] = False - - 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=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"ST: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"ST:Rate counter {self._rotation_rate_counters}") - self._rotation_rate_measurement_period_elapsed = 0 - self._rotation_rate_rpms = [0] * len(self._rotation_rate_counters) - - if self._ina226_sensor_mgr is not None: - app.update_period = self._ina226_sensor_mgr.read_interval # update at the sensor read interval - else: - app.update_period = _MOTOR_TEST_BACKGROUND_UPDATE_PERIOD - - # If we don't have a distance sensor then we can do a simple loopback test - sensor_mgr = self._sensor_mgr - if sensor_mgr is not None and sensor_mgr.get_sensor_by_name("VL53L0X") is None: - # Loop back test for XSHUT - DIST_INT - config = HexpansionConfig(self._app.hexdrive_ports[0]) - self._app.hexdrive_apps[0].set_dist_xshut(1) - if 1 == config.ls_pin[3].value(): - self._test_results["XSHUT high"] = True - self._test_results["dist int high"] = True - else: - self._test_results["XSHUT high"] = False - - self._app.hexdrive_apps[0].set_dist_xshut(0) - if 0 == config.ls_pin[3].value(): - self._test_results["XSHUT low"] = True - self._test_results["dist int low"] = True - else: - self._test_results["XSHUT low"] = False - app.update_period = _MOTOR_TEST_BACKGROUND_UPDATE_PERIOD - return True - if self.logging: - print("ST:Failed to initialise for motor test mode") - app.notification = Notification("Test Init Failed") - return False - - - def _stop_motor_test_mode(self): - if self._logging: - print("ST:Stopping Motor Test mode and cleaning up") - - # Take voltage reading before we power down - if self._ina226 is not None: - ina226 = self._ina226 - data = ina226.read(timeout=160) - try: - volts = int(data.get("mV", 0)) - except Exception as e: # pylint: disable=broad-exception-caught - print(f"ST:Error reading INA226 data: {e}") - else: - self._test_results["5V Voltage"] = volts - if 4900 <= volts <= 5300: - self._test_results["Power On"] = True - else: - self._test_results["Power On"] = False - - - # confirm all tests passed: - if all(self._test_results.get(test, False) for test in ("Power Off", "Power On","XSHUT high", "XSHUT low", "colour int high", "colour int low", "dist int high", "dist int low")): - if self.logging: - print("ST:***** Test PASSED *****") - self._app.notification = Notification(" Test PASSED", port=self._port_selected) - # Report test results - print(f"ST:Test results: {self._test_results}") - - app = self._app - 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 - 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_freq(0) - app.hexdrive_apps[0].set_motors((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 # ------------------------------------------------------------------ @@ -1458,145 +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._scan_mode: - self._draw_auto_scan(ctx) - return - #print("DRAWING") - # 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"] - colours += [(0.3, 0.8, 1.0)] - #lines += [f"V:{self._ina226_reading.get('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 = 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 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._capture_data[i] - x = chart_left + (abs(power) * chart_w) // 100 - for index, rpm in enumerate(rpms): - h = (rpm * chart_h) // max_rpm - if h > 0: - # colour by index to differentiate multiple counters if present - 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(-50, chart_top - 25).text("Complete") - - 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("0%") - 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): - ctx.rgb(*self._colour_for_index(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.2, 0.2).move_to(15, chart_bottom + 5 + ctx.font_size).text(f"{self._last_current_ms}mA") - - # Y axis Maximum RPM and Current labels - ctx.font_size = label_font_size - 8 - ctx.rgb(0.0, 1.0, 0.5).move_to(chart_left+20, chart_top - 5).text(f"rpm:{max_rpm}") - ctx.rgb(1.0, 0.2, 0.2).move_to(5, chart_top - 5).text(f"mA:{max_current_ma}") - - #button_labels(ctx, cancel_label="Back", confirm_label="Manual") - - def _colour_for_index(self, index: int) -> tuple[float, float, float]: - if index == 0: - return (0.0, 1.0, 0.5) - elif index == 1: - return (1.0, 0.5, 0.0) - else: - return (1.0, 1.0, 1.0) - def _draw_select_port(self, ctx): self._app.draw_message(ctx, ["Sensor Test", f"Port: {self._port_selected}"], From 671110c27e25984d06cb05bc8a6d534e6a3302e4 Mon Sep 17 00:00:00 2001 From: robotmad Date: Sun, 31 May 2026 15:12:14 +0100 Subject: [PATCH 43/48] hextest - file id from hexpansion id when non-zero. settings for ir_pwm and whether to allow control of IR or "Serialise" i.e. ID. --- EEPROM/hextest.py | 149 ++++++++++++++++++++++++++++++---------------- dev/minify.py | 10 ++-- vendor/HexDrive2 | 2 +- 3 files changed, 105 insertions(+), 56 deletions(-) diff --git a/EEPROM/hextest.py b/EEPROM/hextest.py index 355975c..0c27b05 100644 --- a/EEPROM/hextest.py +++ b/EEPROM/hextest.py @@ -22,6 +22,7 @@ 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, @@ -32,7 +33,6 @@ from system.hexpansion.util import ( detect_eeprom_addr, get_hexpansion_block_devices, - read_hexpansion_header, ) try: from system.hexpansion.util import get_app_by_slot, get_slots_by_vid_pid @@ -48,9 +48,9 @@ def get_app_by_slot(slot): 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, 7): + for port in range(1, 1+_SLOTS): try: - i2c = I2C(port) + 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: @@ -120,7 +120,7 @@ def _as_hexdrive_app(value): _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(25535) # Minimum power level to test during auto scan (0-65535) +_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") @@ -217,7 +217,7 @@ def __init__(self, config: HexpansionConfig | None = None): self.current_menu: str | None = None self.menu: Menu | None = None - # Settings - common settings first, then each module registers its own later + # Settings self.settings: dict = {} self.edit_setting = None self.edit_setting_value = None @@ -248,10 +248,11 @@ def __init__(self, config: HexpansionConfig | None = None): 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_ms: int = 0 # latest current sampled in auto mode + 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 @@ -273,7 +274,9 @@ def __init__(self, config: HexpansionConfig | None = None): 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['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.update_settings() @@ -323,14 +326,16 @@ def _rotation_rate_rounding(self) -> int: @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 + 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._rotation_rate_emitter_duty = value + 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(self._rotation_rate_emitter_duty) + self.config.ls_pin[pin_num].duty(value) # ------------------------------------------------------------------ @@ -349,7 +354,7 @@ def _rotation_rate_enable(self, enable: bool = True) -> bool: return False try: if enable: - if self._logging: + if self.logging: print("T: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 @@ -358,7 +363,7 @@ def _rotation_rate_enable(self, enable: bool = True) -> bool: 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: + if self.logging: print("T: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 @@ -389,7 +394,6 @@ def deinitialise(self) -> bool: def _exit_app(self): """ Clean up and exit the app, returning to the main menu.""" - eventbus.emit(RequestStopAppEvent(self)) @@ -398,8 +402,9 @@ def _exit_app(self): # ------------------------------------------------------------------ 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: + if self.logging: print(f"H:Hexpansion removed from port {event.port}") self._hexdrive_app = None self._hexdrive_in_use_port = None @@ -413,12 +418,14 @@ async def _handle_removal(self, event: HexpansionRemovalEvent): async def _handle_mounted(self, event: HexpansionMountedEvent): if self._foreground and self.current_state in [STATE_MESSAGE, STATE_MENU]: - if self._logging: + 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] - if hasattr(event, "header") and (event.header.vid, event.header.pid) in vid_pid_pairs: + 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}") @@ -562,6 +569,9 @@ def _motor_test_start(self) -> bool: 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): @@ -613,6 +623,36 @@ def _motor_test_start(self) -> bool: 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"H: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): @@ -666,7 +706,7 @@ def _init_ina226_for_motor_test(self) -> bool: 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, 7): + for port in range(1, 1+_SLOTS): if not mgr.open(port): mgr.close() if self._logging: @@ -732,13 +772,16 @@ def _auto_rotation_rate_step(self): self._scan_step += 1 self.refresh = True if self._scan_step >= _AUTO_SCAN_STEPS: - # Scan complete — stop motors - self._scan_done = True + if self._scan_direction == -1: + # Scan complete + self._scan_done = True + self._scan_direction = 1 + #self._auto_fit_calculate() + self._save_capture_data_csv() + else: + self._scan_direction = -1 # reverse direction for second pass self._rotation_detected = False self._rotation_rate_motor_power = 0 - self._scan_direction *= -1 # reverse direction for next scan - #self._auto_fit_calculate() - self._save_capture_data_csv() else: # Advance to next 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))) @@ -757,14 +800,10 @@ 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 = "/hexcurrent" - eeprom_addr, addr_len = detect_eeprom_addr(self.config.i2c) - if eeprom_addr is None or addr_len is None: - print("HT:No EEPROM found on HexCurrent port") - return None, False - header = read_hexpansion_header(self.config.i2c, eeprom_addr=eeprom_addr, addr_len=addr_len) + mountpoint = "/hextest" + header = self._get_header_for_port(self.config.port) if header is None: - print("HT:Failed to read HexCurrent EEPROM header") + 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) @@ -937,7 +976,7 @@ def _motor_test_update(self, delta: int): # pylint: disable=unused-argument # CONFIRM toggles between manual and auto mode elif self.button_states.get(BUTTON_TYPES["CONFIRM"]): self.button_states.clear() - self._last_current_ms = 0 + self._last_current_ma = 0 self._rotation_rate_measurement_period_elapsed = 0 self._reset_ina226_accumulators() for counter in self._rotation_rate_counters: @@ -957,7 +996,8 @@ def _motor_test_update(self, delta: int): # pylint: disable=unused-argument self._scan_mode = True self._scan_done = False self._scan_step = 0 - self._rotation_rate_motor_power = _AUTO_SCAN_MIN_POWER + 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 = [] @@ -985,7 +1025,7 @@ def _motor_test_update(self, delta: int): # pylint: disable=unused-argument current_ma = self._consume_ina226_average() if current_ma is not None: current_abs = abs(current_ma) - self._last_current_ms = 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 @@ -994,7 +1034,7 @@ def _motor_test_update(self, delta: int): # pylint: disable=unused-argument print(f"T:Auto Scan Step {self._scan_step}/{_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 self._unsaved_data: + if not self._unsaved_data: self._unsaved_data = True else: @@ -1022,7 +1062,7 @@ def _motor_test_update(self, delta: int): # pylint: disable=unused-argument current_ma = self._consume_ina226_average() if current_ma is not None: current_abs = abs(current_ma) - self._last_current_ms = 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 @@ -1052,17 +1092,21 @@ def _motor_test_update(self, delta: int): # pylint: disable=unused-argument # Manual mode button handling if self.button_states.get(BUTTON_TYPES["UP"]): self.button_states.clear() - self._hut_id += 1 - #self.rotation_rate_emitter_duty = min(255, self.rotation_rate_emitter_duty + _IR_EMITTER_PWM_STEP_SIZE) - #if self.logging: - # print(f"T:IR+Emitter Duty: {self.rotation_rate_emitter_duty}") + 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"T:IR+Emitter Duty: {self.rotation_rate_emitter_duty}") self.refresh = True elif self.button_states.get(BUTTON_TYPES["DOWN"]): self.button_states.clear() - self._hut_id = max(0, self._hut_id - 1) - #self.rotation_rate_emitter_duty = max(0, self.rotation_rate_emitter_duty - _IR_EMITTER_PWM_STEP_SIZE) - #if self.logging: - # print(f"T:IR-Emitter Duty: {self.rotation_rate_emitter_duty}") + 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"T:IR-Emitter Duty: {self.rotation_rate_emitter_duty}") self.refresh = True elif self.button_states.get(BUTTON_TYPES["RIGHT"]): self.button_states.clear() @@ -1151,9 +1195,11 @@ def _motor_test_draw(self, ctx): self._draw_auto_scan(ctx) return #print("DRAWING") - # 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)}%"] - lines = [f"ID:{self._hut_id}"] # show the current HUT ID for data logging purposes + 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}"] @@ -1169,8 +1215,10 @@ def _motor_test_draw(self, ctx): lines += [""] colours += [(0.3, 0.8, 1.0)] self.draw_message(ctx, lines, colours, label_font_size) - button_labels(ctx, up_label="ID+", down_label="ID-", cancel_label="Back", - left_label="Pwr-", right_label="Pwr+", confirm_label="Scan") + 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): @@ -1217,7 +1265,7 @@ def _draw_auto_scan(self, ctx): # Title and max RPM label ctx.font_size = label_font_size if self._scan_done: - ctx.move_to(-50, chart_top - 25).text("Complete") + 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}%") @@ -1261,13 +1309,14 @@ def _draw_auto_scan(self, ctx): ctx.font_size = label_font_size - 8 for index, rpm in enumerate(self._rotation_rate_rpms): ctx.rgb(*self._colour_for_index(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.2, 0.2).move_to(15, chart_bottom + 5 + ctx.font_size).text(f"{self._last_current_ms}mA") + 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.0).move_to(-15, chart_top - 5).text("Max") ctx.rgb(0.0, 1.0, 0.5).move_to(chart_left+10, chart_top - 5).text(f"rpm:{self._max_rpm}") - ctx.rgb(1.0, 0.2, 0.2).move_to(20, chart_top - 5).text(f"mA:{self._max_current_ma}") + 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") diff --git a/dev/minify.py b/dev/minify.py index c1b1e97..c33e359 100644 --- a/dev/minify.py +++ b/dev/minify.py @@ -67,8 +67,6 @@ # Public state accessed by BadgeBot "config", "VERSION", "settings", "hexdrive_app", "logging", "auto_repeat_level", "refresh", "current_state", - # Property with @rotation_rate_emitter_duty.setter – must NOT be renamed - "rotation_rate_emitter_duty", # Public methods called by BadgeBot "update_settings", "set_logging", "deinitialise", "show_message", "return_to_menu", "set_menu", "auto_repeat_check", "auto_repeat_clear", @@ -206,15 +204,17 @@ def minify_file( "--output", str(temp_min), str(temp_renamed), ] r = subprocess.run(cmd, capture_output=True, text=True) - temp_renamed.unlink() + #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(temp_min)] + cmd = [str(MPY_CROSS), "-O2", "-o", str(artifact), str(source)] + r = subprocess.run(cmd, capture_output=True, text=True) - temp_min.unlink() + #temp_min.unlink() if r.returncode != 0: print(f"[FAIL] mpy-cross on {source.name}: {r.stderr}", file=sys.stderr) return -1 diff --git a/vendor/HexDrive2 b/vendor/HexDrive2 index 2def42b..a3345b7 160000 --- a/vendor/HexDrive2 +++ b/vendor/HexDrive2 @@ -1 +1 @@ -Subproject commit 2def42b0d749c669721cc07ad0a09ed1a6406ff2 +Subproject commit a3345b7ff6bc5bb9dbe887b96cbd8e79eda9350b From 6738aae600c85d80ca6355519a6dacfed19dc836 Mon Sep 17 00:00:00 2001 From: robotmad Date: Sun, 31 May 2026 15:16:09 +0100 Subject: [PATCH 44/48] Update HexDrive2 submodule to latest commit --- vendor/HexDrive2 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/vendor/HexDrive2 b/vendor/HexDrive2 index a3345b7..4176da3 160000 --- a/vendor/HexDrive2 +++ b/vendor/HexDrive2 @@ -1 +1 @@ -Subproject commit a3345b7ff6bc5bb9dbe887b96cbd8e79eda9350b +Subproject commit 4176da3e1bc56f9879ed38fc7908453b9a3d8e81 From 46955f7f2f968c9a82beb1b89576c3247a8182c2 Mon Sep 17 00:00:00 2001 From: robotmad Date: Thu, 4 Jun 2026 23:51:11 +0100 Subject: [PATCH 45/48] BLE --- app.py | 8 +++ bluetooth_mgr.py | 117 ++++++++++++++++++++++++++++++++++++++ dev/build_release.py | 1 + dev/download_to_device.py | 1 + 4 files changed, 127 insertions(+) create mode 100644 bluetooth_mgr.py diff --git a/app.py b/app.py index b146524..3738d12 100644 --- a/app.py +++ b/app.py @@ -21,6 +21,8 @@ from machine import Pin import app +from .bluetooth_mgr import bluetooth, RobotBLE, ble_process_command + # 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. @@ -347,6 +349,12 @@ 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) + if self.logging: print(f"BadgeBot App V{self.app_version} Initialised") diff --git a/bluetooth_mgr.py b/bluetooth_mgr.py new file mode 100644 index 0000000..6c64016 --- /dev/null +++ b/bluetooth_mgr.py @@ -0,0 +1,117 @@ +# MicroPython BLE Robot Control + +import bluetooth +import struct +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 _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 --- +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 + """ + 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 action == '1': # Only act on button press + if button == '5': + print("ACTION: Moving Forward") + elif button == '6': + print("ACTION: Moving Backward") + elif button == '7': + print("ACTION: Turning Left") + elif button == '8': + print("ACTION: Turning Right") + else: + print("ACTION: Stopping Motors") diff --git a/dev/build_release.py b/dev/build_release.py index 636af39..2720bab 100644 --- a/dev/build_release.py +++ b/dev/build_release.py @@ -20,6 +20,7 @@ class ModuleSpec: "autotune_mgr", "settings_mgr", "hexpansion_mgr", + "bluetooth_mgr", "line_follow", "motor_moves", "servo_test", diff --git a/dev/download_to_device.py b/dev/download_to_device.py index 3ee979c..7b8b563 100644 --- a/dev/download_to_device.py +++ b/dev/download_to_device.py @@ -50,6 +50,7 @@ class ModuleSpec: 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")), From be7ecb8fa4cb03adabcc3669931bf062e531dc2d Mon Sep 17 00:00:00 2001 From: robotmad Date: Fri, 5 Jun 2026 00:24:26 +0100 Subject: [PATCH 46/48] Add BLE logging support and motor override functionality - Introduced BLE logging capabilities to redirect print statements over BLE. - Added methods to enable and disable BLE logging. - Implemented motor override logic based on BLE drive button inputs. - Updated motor power calculations to use a scale factor for consistency. --- app.py | 35 ++++++++++-- bluetooth_mgr.py | 126 ++++++++++++++++++++++++++++++++++++++++---- motor_controller.py | 16 +++--- 3 files changed, 156 insertions(+), 21 deletions(-) diff --git a/app.py b/app.py index 3738d12..885429f 100644 --- a/app.py +++ b/app.py @@ -21,7 +21,7 @@ from machine import Pin import app -from .bluetooth_mgr import bluetooth, RobotBLE, ble_process_command +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 @@ -75,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 @@ -105,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 @@ -212,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) @@ -354,6 +357,9 @@ def __init__(self): 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) if self.logging: print(f"BadgeBot App V{self.app_version} Initialised") @@ -382,6 +388,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.""" @@ -448,9 +462,16 @@ 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: + 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") @@ -528,6 +549,12 @@ def fast_settings_update(self): 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): diff --git a/bluetooth_mgr.py b/bluetooth_mgr.py index 6c64016..0282cac 100644 --- a/bluetooth_mgr.py +++ b/bluetooth_mgr.py @@ -2,6 +2,7 @@ import bluetooth import struct +import sys import time from micropython import const @@ -63,10 +64,14 @@ def send_telemetry(self, text): 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() @@ -90,12 +95,23 @@ def _append(adv_type, value): # --- 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 @@ -104,14 +120,106 @@ def ble_process_command(data): button = command[2] action = command[3] # '1' for press, '0' for release - if action == '1': # Only act on button press - if button == '5': - print("ACTION: Moving Forward") + 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("ACTION: Moving Backward") + print("BLE: Backward") elif button == '7': - print("ACTION: Turning Left") + print("BLE: Left") elif button == '8': - print("ACTION: Turning Right") - else: - print("ACTION: Stopping Motors") + 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/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: From d6c23614112fe68f425bb1045927f17046560153 Mon Sep 17 00:00:00 2001 From: robotmad Date: Thu, 11 Jun 2026 01:19:30 +0100 Subject: [PATCH 47/48] Refactor HexTestApp logging and settings; update rotation rate parameters and improve I2S testing code --- EEPROM/hextest.py | 164 +++++++++++++++++++++++++--------------------- app.py | 50 +++++++++++--- hexpansion_mgr.py | 23 +++++++ 3 files changed, 152 insertions(+), 85 deletions(-) diff --git a/EEPROM/hextest.py b/EEPROM/hextest.py index 0c27b05..34a4c10 100644 --- a/EEPROM/hextest.py +++ b/EEPROM/hextest.py @@ -106,9 +106,8 @@ def _as_hexdrive_app(value): # 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(25) # default duty cycle for the IR emitter when doing rate testing, 0-255 (0=off, 255=full on) -_DEFAULT_SPOKES_PER_ROTATION = const(3) # number of times the photodiode will be triggered per full rotation of the wheel -_MOTOR_TEST_BACKGROUND_UPDATE_PERIOD = const(1000) # background update period in ms to use during motor test mode (tradeoff between display responsiveness and CPU load) +_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) @@ -194,7 +193,7 @@ def __init__(self, config: HexpansionConfig | None = None): else: raise RuntimeError("HexTestApp requires BadgeOS Upgrade") except Exception as e: # pylint: disable=broad-except - print(f"T:Ver check failed {e}!") + print(f"HT:Ver check failed {e}!") self.config: HexpansionConfig = config self._logging: bool = True @@ -236,7 +235,6 @@ def __init__(self, config: HexpansionConfig | None = None): 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 - self._rotation_rate_spokes: int = _DEFAULT_SPOKES_PER_ROTATION # Auto scan test self._scan_mode: bool = False # True = auto scanning, False = manual @@ -277,6 +275,7 @@ def __init__(self, config: HexpansionConfig | None = None): 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() @@ -292,7 +291,7 @@ def __init__(self, config: HexpansionConfig | None = None): 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"T:HexTest App V{self.VERSION} by RobotMad on port {self.config.port}") + 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 @@ -320,8 +319,8 @@ def logging(self): # ------------------------------------------------------------------ @property - def _rotation_rate_rounding(self) -> int: - return (self._rotation_rate_measurement_period * self._rotation_rate_spokes) // 2 + 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: @@ -337,12 +336,19 @@ def rotation_rate_emitter_duty(self, value: int): 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("T:Updating settings from EEPROM") + 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: @@ -355,7 +361,7 @@ def _rotation_rate_enable(self, enable: bool = True) -> bool: try: if enable: if self.logging: - print("T:Enabling rotation rate emitters and sensors") + 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) @@ -364,7 +370,7 @@ def _rotation_rate_enable(self, enable: bool = True) -> bool: 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("T:Disabling rotation rate emitters and sensors") + 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: @@ -551,11 +557,11 @@ def _motor_test_start(self) -> bool: # 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"T:Looking for {hexpansion_type.name} (VID:PID {hexpansion_type.vid:04X}:{hexpansion_type.pid:04X}, Motors: {hexpansion_type.motors}, Servos: {hexpansion_type.servos})") + 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"T:Found {hexpansion_type.name} on port(s): {ports}") + print(f"HT:Found {hexpansion_type.name} on port(s): {ports}") self._hexdrive_ports.extend(ports) break @@ -564,7 +570,7 @@ def _motor_test_start(self) -> bool: if hexpansion_app is not None: self._hexdrive_in_use_port = port if self.logging: - print(f"T:Found HexDrive app to test on port {port}") + print(f"HT:Found HexDrive app to test on port {port}") self._hexdrive_app = hexpansion_app break @@ -598,14 +604,14 @@ def _motor_test_start(self) -> bool: # 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"T:Not setting up rotation rate counter on pin {pin_num} (GPIO {gpio_num}) as this HexDrive type only has {hexpansion_type.motors} motors") + 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"T:Failed to allocate PCNT counter for pin {pin_num} (GPIO {gpio_num})") + 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: @@ -614,12 +620,12 @@ def _motor_test_start(self) -> bool: self._rotation_rate_counters = [] return False if self.logging: - print(f"T:Rate counter {counter}") + 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("T:Failed to initialise for motor test mode - no hexdrive to test") + print("HT:Failed to initialise for motor test mode - no hexdrive to test") self.notification = Notification("HexDrive not Found") return False @@ -648,7 +654,7 @@ def _seed_hut_id_from_detected_hexdrive(self, port: int | None, header: Hexpansi if unique_id != 0: self._hut_id = unique_id if self._logging: - print(f"H:Initialised HUT ID from UID {unique_id} on port {port}") + 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) @@ -657,7 +663,7 @@ def _seed_hut_id_from_detected_hexdrive(self, port: int | None, header: Hexpansi def _stop_motor_test_mode(self): if self._logging: - print("T:Stopping Motor Test mode and cleaning up") + print("HT:Stopping Motor Test mode and cleaning up") self._scan_mode = False self._scan_done = False self._rotation_rate_motor_power = 0 @@ -668,7 +674,7 @@ def _stop_motor_test_mode(self): try: self._ina226_sensor_mgr.close() except Exception as exc: # pylint: disable=broad-exception-caught - print("T:INA226 sensor manager close failed:", exc) + print("HT:INA226 sensor manager close failed:", exc) self._ina226_sensor_mgr = None self._ina226 = None @@ -677,7 +683,7 @@ def _stop_motor_test_mode(self): self._hexdrive_app.set_motors((0, 0)) self._hexdrive_app.set_power(False) except AttributeError as e: - print(f"T:Failed to set motor outputs off {e}") + print(f"HT:Failed to set motor outputs off {e}") self._hexdrive_in_use_port = None for c in self._rotation_rate_counters: @@ -691,7 +697,7 @@ def _stop_motor_test_mode(self): def _stop_sensor_test_mode(self): if self._logging: - print("T:Stopping Sensor Test mode and cleaning up") + print("HT:Stopping Sensor Test mode and cleaning up") self._sensor_data = {} self._display_data = {} self._hexdrive_in_use_port = None @@ -710,7 +716,7 @@ def _init_ina226_for_motor_test(self) -> bool: if not mgr.open(port): mgr.close() if self._logging: - print(f"T:INA226 - no sensors found on port {port}") + 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") @@ -718,13 +724,13 @@ def _init_ina226_for_motor_test(self) -> bool: self._ina226 = sensor self._ina226_sensor_mgr = mgr if self._logging: - print(f"T:INA226 found @ 0x{sensor.i2c_addr:02X} on port {port}") + 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"T:INA226 init failed: {e}") + print(f"HT:INA226 init failed: {e}") return False @@ -749,7 +755,7 @@ def _sample_ina226_in_background(self, delta: int) -> None: # pylint: disabl self._ina226_sample_count += 1 except Exception as e: # pylint: disable=broad-exception-caught if self._logging: - print(f"T:INA226 sample error: {e}") + print(f"HT:INA226 sample error: {e}") return @@ -769,22 +775,25 @@ def _consume_ina226_average(self) -> int | None: 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._scan_direction = 1 + 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 - self._rotation_rate_motor_power = 0 - else: - # Advance to next 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))) + # 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 @@ -896,7 +905,7 @@ def update(self, delta: int): self.notification = None except Exception as e: # pylint: disable=broad-exception-caught if self.logging: - print(f"T:Error: checking notification status: {e}") + 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 @@ -920,7 +929,7 @@ def update(self, delta: int): menu.update(delta) if menu.is_animating != "none": if self.logging: - print("T:Menu is animating") + 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() @@ -935,7 +944,7 @@ def update(self, delta: int): if self.current_state != self.previous_state: if self.logging: - print(f"T:State: {self.previous_state} -> {self.current_state}") + 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 @@ -944,7 +953,7 @@ def update(self, delta: int): def _update_state_message(self, delta: int): # pylint: disable=unused-argument if self.button_states.get(BUTTON_TYPES["CONFIRM"]): if self.logging: - print("T:Message acknowledged by user") + 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. @@ -1031,7 +1040,7 @@ def _motor_test_update(self, delta: int): # pylint: disable=unused-argument power = self._rotation_rate_motor_power self._rotation_rate_rpms = [0] * len(self._rotation_rate_counters) if self._logging: - print(f"T:Auto Scan Step {self._scan_step}/{_AUTO_SCAN_STEPS} - Power: {power}, Rate: 0 rpm, Current: {current_ma}mA") + 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: @@ -1053,7 +1062,7 @@ def _motor_test_update(self, delta: int): # pylint: disable=unused-argument 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) + 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 @@ -1067,7 +1076,7 @@ def _motor_test_update(self, delta: int): # pylint: disable=unused-argument self._max_current_ma = current_abs power = self._rotation_rate_motor_power if self._logging: - print(f"T:Auto Scan Step {self._scan_step}/{_AUTO_SCAN_STEPS} - Power: {power}, Rates: {self._rotation_rate_rpms} rpm, Current: {current_ma}mA") + 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: @@ -1083,11 +1092,11 @@ def _motor_test_update(self, delta: int): # pylint: disable=unused-argument 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_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"T:Rotation Rates: {self._rotation_rate_rpms}") + # print(f"HT:Rotation Rates: {self._rotation_rate_rpms}") # Manual mode button handling if self.button_states.get(BUTTON_TYPES["UP"]): @@ -1097,7 +1106,7 @@ def _motor_test_update(self, delta: int): # pylint: disable=unused-argument else: self.rotation_rate_emitter_duty = min(255, self.rotation_rate_emitter_duty + _IR_EMITTER_PWM_STEP_SIZE) if self.logging: - print(f"T:IR+Emitter Duty: {self.rotation_rate_emitter_duty}") + 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() @@ -1106,19 +1115,19 @@ def _motor_test_update(self, delta: int): # pylint: disable=unused-argument else: self.rotation_rate_emitter_duty = max(0, self.rotation_rate_emitter_duty - _IR_EMITTER_PWM_STEP_SIZE) if self.logging: - print(f"T:IR-Emitter Duty: {self.rotation_rate_emitter_duty}") + 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"T:Motor+Power: {self._rotation_rate_motor_power}") + 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"T:Motor-Power: {self._rotation_rate_motor_power}") + print(f"HT:Motor-Power: {self._rotation_rate_motor_power}") self.refresh = True @@ -1255,6 +1264,8 @@ def _draw_auto_scan(self, ctx): 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 @@ -1308,26 +1319,29 @@ def _draw_auto_scan(self, ctx): # Instantaneous current label (updated live during the scan) ctx.font_size = label_font_size - 8 for index, rpm in enumerate(self._rotation_rate_rpms): - ctx.rgb(*self._colour_for_index(index)).move_to(chart_left+20, chart_bottom + 5 + ((index + 2) * (ctx.font_size))).text(f"Mtr{index+1}: {rpm}rpm") + 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.0).move_to(-15, chart_top - 5).text("Max") - ctx.rgb(0.0, 1.0, 0.5).move_to(chart_left+10, chart_top - 5).text(f"rpm:{self._max_rpm}") + 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]: - if index == 0: - return (0.0, 1.0, 0.5) - elif index == 1: - return (1.0, 0.5, 0.0) - else: - return (1.0, 1.0, 1.0) + # 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 + @@ -1336,7 +1350,7 @@ def _colour_for_index(self, index: int) -> tuple[float, float, float]: 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("T:Returning to menu") + print("HT:Returning to menu") if menu_name is not None: self.set_menu(menu_name) self.update_period = DEFAULT_BACKGROUND_UPDATE_PERIOD @@ -1349,7 +1363,7 @@ def show_message(self, msg_content, msg_colours, msg_type = None, return_state: 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"T:Showing message: '{msg_content}' with type {msg_type}") + 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 @@ -1367,7 +1381,7 @@ def set_menu(self, menu_name: str | None = "main"): #: Literal["main"]): does i 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"T:Set Menu {menu_name}") + print(f"HT:Set Menu {menu_name}") if self.menu is not None: try: self.menu._cleanup() # pylint: disable=protected-access @@ -1402,7 +1416,7 @@ def set_menu(self, menu_name: str | None = "main"): #: Literal["main"]): does i # 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"T:Main Menu {item} at index {idx}") + 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(): @@ -1426,16 +1440,16 @@ def _main_menu_select_handler(self, item: str, idx: int): def _settings_menu_select_handler(self, item: str, idx: int): if self.logging: - print(f"T:Setting {item} @ {idx}") + print(f"HT:Setting {item} @ {idx}") if idx == 0: #Save if self.logging: - print("T:Settings Save All") + print("HT:Settings Save All") platform_settings.save() self.notification = Notification(" Settings Saved") self.set_menu() elif idx == 1: #Default if self.logging: - print("T:Settings Default All") + print("HT:Settings Default All") for s in self.settings: self.settings[s].v = self.settings[s].d self.settings[s].persist() @@ -1492,7 +1506,7 @@ def settings_mgr_start(self, item: str) -> bool: self.refresh = True self.auto_repeat_clear() if self._logging: - print("T:Entered Settings editing mode") + print("HT:Entered Settings editing mode") self.edit_setting = item self.edit_setting_value = self.settings[item].v return True @@ -1508,13 +1522,13 @@ def _settings_mgr_update(self, delta): 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"T:Setting: {self.edit_setting} (+) Value: {self.edit_setting_value}") + 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"T:Setting: {self.edit_setting} (-) Value: {self.edit_setting_value}") + print(f"HT:Setting: {self.edit_setting} (-) Value: {self.edit_setting_value}") self.refresh = True else: self.auto_repeat_clear() @@ -1522,18 +1536,18 @@ def _settings_mgr_update(self, delta): self.button_states.clear() self.edit_setting_value = self.settings[self.edit_setting].d if self._logging: - print(f"T:Setting: {self.edit_setting} Default: {self.edit_setting_value}") + 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"T:Setting: {self.edit_setting} Cancelled") + 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"T:Setting: {self.edit_setting} = {self.edit_setting_value}") + 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}") @@ -2301,7 +2315,7 @@ def __init__(self, logging: bool = False): self._read_interval_ms = 10 self._type = "Generic" if self._logging: - print("T:SensorManager initialised") + print("HT:SensorManager initialised") # ------------------------------------------------------------------ @@ -2331,18 +2345,18 @@ def open(self, port: int) -> bool: self._i2c = I2C(port) except Exception as e: # pylint: disable=broad-exception-caught if self._logging: - print(f"T:Cannot open I2C port {port}: {e}") + 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"T:I2C scan failed on port {port}: {e}") + print(f"HT:I2C scan failed on port {port}: {e}") return False if self._logging: - print(f"T:Port {port} scan: {[hex(a) for a in found_addrs]}") + print(f"HT:Port {port} scan: {[hex(a) for a in found_addrs]}") used_addrs = set() for cls in ALL_SENSOR_CLASSES: @@ -2360,9 +2374,9 @@ def open(self, port: int) -> bool: self._sensors.append(sensor) used_addrs.add(address) if self._logging: - print(f"T: + {cls.NAME} @ 0x{sensor.i2c_addr:02X} {cls.TYPE}") + print(f"HT: + {cls.NAME} @ 0x{sensor.i2c_addr:02X} {cls.TYPE}") elif self._logging: - print(f"T: - {cls.NAME} @ 0x{address:02X} begin() failed") + print(f"HT: - {cls.NAME} @ 0x{address:02X} begin() failed") self._index = 0 self._last_data = {} @@ -2380,7 +2394,7 @@ def open(self, port: int) -> bool: if len(self._sensors) > 0 and any(getattr(s, 'TYPE', '') == 'Colour' for s in self._sensors): config = HexpansionConfig(port) if self._logging: - print(f"T:LED On port {port} pin {config.ls_pin[_LED_PIN]} for colour sensor") + 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) @@ -2402,7 +2416,7 @@ def close(self): 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"T:LED Off port {self._port}") + print(f"HT:LED Off port {self._port}") config = HexpansionConfig(self._port) if config is not None: config.ls_pin[_LED_PIN].value(0) diff --git a/app.py b/app.py index 885429f..fc05a74 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 @@ -357,9 +357,39 @@ def __init__(self): 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) + + # Apply BLE logging setting now that _ble_controller exists + if self.ble_logging: + enable_ble_logging(self._ble_controller) + +# TESTING I2S START +# # use frequencies for musical notes A4 and C6 +# 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") @@ -549,12 +579,12 @@ def fast_settings_update(self): 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() + 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): diff --git a/hexpansion_mgr.py b/hexpansion_mgr.py index 5469eaf..2281ae4 100644 --- a/hexpansion_mgr.py +++ b/hexpansion_mgr.py @@ -20,6 +20,7 @@ from events.input import BUTTON_TYPES from machine import I2C from system.eventbus import eventbus +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 @@ -251,6 +252,8 @@ async def _handle_removal(self, event: HexpansionRemovalEvent): print(f"H:Hexpansion removed from port {port}") + # 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 @@ -762,6 +765,26 @@ 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"]): From 6df7909749d5625040bbed0c44998b3dd5a49b9e Mon Sep 17 00:00:00 2001 From: robotmad Date: Thu, 11 Jun 2026 15:08:08 +0100 Subject: [PATCH 48/48] fix formatting for BLE logging property --- app.py | 62 +++++++++++++++++++++++++++++----------------------------- 1 file changed, 31 insertions(+), 31 deletions(-) diff --git a/app.py b/app.py index fc05a74..6166d62 100644 --- a/app.py +++ b/app.py @@ -363,31 +363,31 @@ def __init__(self): enable_ble_logging(self._ble_controller) # TESTING I2S START -# # use frequencies for musical notes A4 and C6 -# 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() + 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 @@ -418,12 +418,12 @@ 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 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