From 06185b8ab18b2826d54745029f7c59ee15e48681 Mon Sep 17 00:00:00 2001 From: robotmad Date: Tue, 16 Jul 2024 23:05:34 +0100 Subject: [PATCH 01/16] use more of badge s/w hexpansion header handling code --- app.py | 68 +++++++++++++++++++++------------------------------------- 1 file changed, 24 insertions(+), 44 deletions(-) diff --git a/app.py b/app.py index 9a88b35..c6e0f02 100644 --- a/app.py +++ b/app.py @@ -14,7 +14,7 @@ from system.eventbus import eventbus from system.hexpansion.events import (HexpansionInsertionEvent, HexpansionRemovalEvent) -from system.hexpansion.header import HexpansionHeader +from system.hexpansion.header import HexpansionHeader, write_header, read_header from system.hexpansion.util import get_hexpansion_block_devices from system.patterndisplay.events import PatternDisable, PatternEnable from system.scheduler import scheduler @@ -354,18 +354,16 @@ def scan_ports(self): def check_port_for_hexdrive(self, port) -> bool: - # avoiding use of badge read_hexpansion_header as this triggers a full i2c scan each time # we know the EEPROM address so we can just read the header directly if port not in range(1, 7): return False + # We want to do this in two parts so that we detect if there is a valid EEPROM or not try: - header_bytes = I2C(port).readfrom_mem(_EEPROM_ADDR, 0, 32, addrsize = (8*_EEPROM_NUM_ADDRESS_BYTES)) + hexpansion_header = read_header(port, addr_len=_EEPROM_NUM_ADDRESS_BYTES) except OSError: # no EEPROM on this port return False - try: - read_header = HexpansionHeader.from_bytes(header_bytes) - except Exception: + except RuntimeError: # not a valid header if self._settings['logging'].v: print(f"H:Found EEPROM on port {port}") @@ -373,7 +371,7 @@ def check_port_for_hexdrive(self, port) -> bool: return True # check is this is a HexDrive header by scanning the _HEXDRIVE_TYPES list for index, hexpansion_type in enumerate(self._HEXDRIVE_TYPES): - if read_header.vid == hexpansion_type.vid and read_header.pid == hexpansion_type.pid: + if hexpansion_header.vid == hexpansion_type.vid and hexpansion_header.pid == hexpansion_type.pid: if self._settings['logging'].v: print(f"H:Found '{hexpansion_type.name}' HexDrive on port {port}") if port not in self.ports_with_latest_hexdrive: @@ -393,13 +391,13 @@ def update_app_in_eeprom(self, port, addr) -> bool: except Exception as e: print(f"H:Error opening I2C port {port}: {e}") return False - header = self.read_hexpansion_header(i2c=i2c) + header = read_header(port, addr_len = _EEPROM_NUM_ADDRESS_BYTES) if header is None: if self._settings['logging'].v: print(f"H:Error reading header on port {port}") return False try: - _, partition = get_hexpansion_block_devices(i2c, header, addr) + _, partition = get_hexpansion_block_devices(i2c, header, addr, addr_len = _EEPROM_NUM_ADDRESS_BYTES) except RuntimeError as e: print(f"H:Error getting block devices: {e}") return False @@ -481,32 +479,22 @@ def prepare_eeprom(self, port, addr) -> bool: # Write and read back header efficiently try: i2c = I2C(port) - i2c.writeto(addr, bytes([0]*_EEPROM_NUM_ADDRESS_BYTES) + hexdrive_header.to_bytes()) except Exception as e: - print(f"H:Error writing header: {e}") + print(f"H:Error opening I2C port {port}: {e}") return False - # Poll ACK - while True: - try: - if i2c.writeto(addr, bytes([0]*_EEPROM_NUM_ADDRESS_BYTES)): # Poll ACK - break - except OSError: - pass - finally: - time.sleep_ms(1) try: - header_bytes = i2c.readfrom_mem(addr, 0, 32, addrsize = (8*_EEPROM_NUM_ADDRESS_BYTES)) + write_header(port, hexdrive_header, addr_len = _EEPROM_NUM_ADDRESS_BYTES, page_size = _EEPROM_PAGE_SIZE) except Exception as e: - print(f"H:Error reading header back: {e}") + print(f"H:Error writing header: {e}") return False try: - read_header = HexpansionHeader.from_bytes(header_bytes) + hexpansion_header = read_header(port, addr_len = _EEPROM_NUM_ADDRESS_BYTES) except Exception as e: - print(f"H:Error parsing header: {e}") + print(f"H:Error reading header back: {e}") return False try: # Get block devices - _, partition = get_hexpansion_block_devices(i2c, read_header, addr) + _, partition = get_hexpansion_block_devices(i2c, hexpansion_header, addr, addr_len = _EEPROM_NUM_ADDRESS_BYTES) except RuntimeError as e: print(f"H:Error getting block devices: {e}") return False @@ -543,14 +531,16 @@ def erase_eeprom(self, port, addr) -> bool: print(f"H:Erasing EEPROM on port {port}") try: i2c = I2C(port) - i2c.writeto(addr, bytes([0]*_EEPROM_NUM_ADDRESS_BYTES)) # loop through all pages and erase them for page in range(_EEPROM_TOTAL_SIZE // _EEPROM_PAGE_SIZE): - i2c.writeto(addr, bytes([page >> 8, page & 0xFF]) + bytes([0xFF]*_EEPROM_PAGE_SIZE)) + mem_addr = page * _EEPROM_PAGE_SIZE + #generate a bit mask for the address based on the number of address bytes + mem_addr_mask = 1<<(_EEPROM_NUM_ADDRESS_BYTES*8)-1 + i2c.writeto_mem((addr | (mem_addr >> (8*_EEPROM_NUM_ADDRESS_BYTES))), (mem_addr & mem_addr_mask), bytes([0xFF]*_EEPROM_PAGE_SIZE), addrsize = (8*_EEPROM_NUM_ADDRESS_BYTES)) # check Ack while True: - try: - if i2c.writeto(addr, bytes([page >> 8, page & 0xFF])): # Poll ACK + try: # Poll Ack + if i2c.writeto((addr | (mem_addr >> (8*_EEPROM_NUM_ADDRESS_BYTES))), bytes([mem_addr & 0xFF]) if _EEPROM_NUM_ADDRESS_BYTES == 1 else bytes([mem_addr >> 8, mem_addr & 0xFF])): break except OSError: pass @@ -559,19 +549,7 @@ def erase_eeprom(self, port, addr) -> bool: except Exception as e: print(f"H:Error erasing EEPROM: {e}") return False - return True - - - def read_hexpansion_header(self, i2c=None, port=None) -> HexpansionHeader: - try: - if i2c is None: - if port is None: - return None - i2c = I2C(port) - header_bytes = i2c.readfrom_mem(_EEPROM_ADDR, 0, 32, addrsize = (8*_EEPROM_NUM_ADDRESS_BYTES)) - return HexpansionHeader.from_bytes(header_bytes) - except OSError: - return None + return True def find_hexdrive_app(self, port) -> app: @@ -776,10 +754,12 @@ def _update_state_programming(self, delta): if self.prepare_eeprom(self.detected_port, _EEPROM_ADDR): self.notification = Notification("Initialised", port = self.detected_port) self.upgrade_port = self.detected_port + self.hexpansion_slot_type[self.detected_port-1] = self.hexpansion_init_type self.current_state = STATE_UPGRADE else: self.notification = Notification("Failed", port = self.detected_port) self.error_message = ["EEPROM","initialisation","failed"] + self.hexpansion_slot_type[self.detected_port-1] = None self.current_state = STATE_ERROR self.detected_port = None elif self._settings['logging'].v: @@ -1263,8 +1243,8 @@ def draw(self, ctx): clear_background(ctx) ctx.save() ctx.font_size = label_font_size - if ctx.text_align != ctx.LEFT: - print(f"H:Font alignment {ctx.text_align}!") + 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 if self.current_state == STATE_LOGO: From 80bed3ff4f911ce45a9a629842057bcb5ea803e0 Mon Sep 17 00:00:00 2001 From: robotmad Date: Thu, 1 Aug 2024 00:56:24 +0100 Subject: [PATCH 02/16] stepper wip --- app.py | 546 ++++++++++++++++++++++++++++++++++++++++++---------- hexdrive.py | 328 +++++++++++++++---------------- 2 files changed, 598 insertions(+), 276 deletions(-) diff --git a/app.py b/app.py index c6e0f02..56b6f5b 100644 --- a/app.py +++ b/app.py @@ -10,7 +10,7 @@ from app_components import Menu from events.input import BUTTON_TYPES, Button, Buttons, ButtonUpEvent from frontboards.twentyfour import BUTTONS -from machine import I2C +from machine import I2C, Timer from system.eventbus import eventbus from system.hexpansion.events import (HexpansionInsertionEvent, HexpansionRemovalEvent) @@ -31,9 +31,9 @@ # Hard coded to talk to EEPROMs on address 0x50 - because we know that is what is on the HexDrive Hexpansion # makes it a lot more efficient than scanning the I2C bus for devices and working out what they are -CURRENT_APP_VERSION = 4 # HEXDRIVE.PY Integer Version Number - checked against the EEPROM app.py version to determine if it needs updating +CURRENT_APP_VERSION = 5 # HEXDRIVE.PY Integer Version Number - checked against the EEPROM app.py version to determine if it needs updating -_APP_VERSION = "1.2" # BadgeBot App Version Number +_APP_VERSION = "1.3" # BadgeBot App Version Number # If you change the URL then you will need to regenerate the QR code _QR_CODE = [0x1fcf67f, @@ -83,6 +83,13 @@ _SERVO_MAX_TRIM = 1000 # us _MAX_SERVO_RANGE = 1400 # 1400us either side of centre (VERY WIDE) +# Stepper Tester - Defaults +_STEPPER_MAX_SPEED = 1000 # full steps per second +_STEPPER_MAX_POSITION = 6200 # half steps +_STEPPER_DEFAULT_SPEED = 50 # full steps per second +_STEPPER_NUM_PHASES = 8 # half steps +_STEPPER_DEFAULT_SPR = 200 # steps per revolution + # Timings _TICK_MS = 10 # Smallest unit of change for power, in ms @@ -115,7 +122,8 @@ STATE_MESSAGE = 14 # Message display STATE_LOGO = 15 # Logo display STATE_SERVO = 16 # Servo test -STATE_SETTINGS = 17 # Edit Settings +STATE_STEPPER = 17 # Stepper test +STATE_SETTINGS = 18 # Edit Settings # App states where user can minimise app _MINIMISE_VALID_STATES = [0, 1, 7, 12, 13, 14, 15] @@ -133,8 +141,18 @@ _ERASE_SLOT = 0 # Slot for user to set if they want to erase EEPROMs on HexDrives # -_main_menu_items = ["Motor Moves", "Servo Test", "Settings", "About","Exit"] +_main_menu_items = ["Motor Moves", "Stepper Test", "Servo Test", "Settings", "About","Exit"] + +class StepperMode: + OFF = 0 + POSITION = 1 + SPEED = 2 +class ServoMode: + OFF = 0 + TRIM = 1 + POSITION = 2 + SCANNING = 3 class BadgeBotApp(app.App): def __init__(self): @@ -142,29 +160,29 @@ def __init__(self): # UI Button Controls self.button_states = Buttons(self) self.last_press: Button = BUTTON_TYPES["CANCEL"] - self.long_press_delta = 0 + self.long_press_delta: int = 0 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 = 0 - self._auto_repeat_count = 0 - self._auto_repeat_level = 0 + self._auto_repeat: int = 0 + self._auto_repeat_count: int = 0 + self._auto_repeat_level: int = 0 # UI Feature Controls - self._refresh = True - self.rpm = 5 # logo rotation speed in RPM - self._animation_counter = 0 - self._pattern_status = True # True = Pattern Enabled, False = Pattern Disabled + self._refresh: bool = True + self.rpm: float = 5 # logo rotation speed in RPM + self._animation_counter: float = 0 + self._pattern_status: bool = True # True = Pattern Enabled, False = Pattern Disabled self.qr_code = _QR_CODE - self.b_msg = f"BadgeBot V{_APP_VERSION}" - self.t_msg = "RobotMad" - self.is_scroll = False - self.scroll_offset = 0 + self.b_msg: str = f"BadgeBot V{_APP_VERSION}" + self.t_msg: str = "RobotMad" + self.is_scroll: bool = False + self.scroll_offset: int = 0 self.notification = None self.error_message = [] self.current_menu = None self.menu = None # BadgeBot Control Sequence Variables - self.run_countdown_elapsed_ms = 0 + self.run_countdown_elapsed_ms: int = 0 self.instructions = [] self.current_instruction = None self.current_power_duration = ((0,0,0,0), 0) @@ -183,7 +201,8 @@ def __init__(self): self._settings['logging'] = MySetting(self._settings, _LOGGING, False, True) self._settings['erase_slot'] = MySetting(self._settings, _ERASE_SLOT, 0, 6) - self._edit_setting = None + + self._edit_setting: int = None self._edit_setting_value = None self.update_settings() @@ -202,36 +221,42 @@ def __init__(self): self._HEXDRIVE_TYPES = [HexDriveType(0xCBCB, motors=2, servos=4), HexDriveType(0xCBCA, motors=2, name="2 Motor"), HexDriveType(0xCBCC, servos=4, name="4 Servo"), - HexDriveType(0xCBCD, motors=1, servos=2, name = "1 Mot 2 Srvo")] + HexDriveType(0xCBCD, motors=1, servos=2, name="1 Mot 2 Srvo"), + HexDriveType(0xCBCE, steppers=1, name="Stepper")] self.hexpansion_slot_type = [None]*6 - self.hexpansion_init_type = 0 - self.detected_port = None - self.waiting_app_port = None - self.erase_port = None - self.upgrade_port = None - self.hexdrive_port = None + self.hexpansion_init_type: int = 0 + self.detected_port: int = None + self.waiting_app_port: int = None + self.erase_port: int = None + self.upgrade_port: int = None + self.hexdrive_port: int = None self.ports_with_blank_eeprom = set() self.ports_with_hexdrive = set() self.ports_with_latest_hexdrive = set() self.hexdrive_app = None - self.hexpansion_update_required = False # flag from async to main loop + self.hexpansion_update_required: bool = False # flag from async to main loop eventbus.on_async(HexpansionInsertionEvent, self._handle_hexpansion_insertion, self) eventbus.on_async(HexpansionRemovalEvent, self._handle_hexpansion_removal, self) # Motor Driver - self.num_motors = 2 # Default assumed for a single HexDrive + self.num_motors: int = 2 # Default assumed for a single HexDrive + self.num_steppers: int = 1 # Default assumed for a single HexDrive + self._stepper = None + #self._stepper_modes = list(StepperMode) + self.stepper_mode = StepperMode.OFF + self.stepper_pos: int = 0 # Servo Tester - self._time_since_last_update = 0 - self._keep_alive_period = 500 # ms (half the value used in hexdrive.py) - self.num_servos = 4 # Default assumed for a single HexDrive - self.servo = [None]*4 # Servo Positions - self.servo_centre = [_SERVO_DEFAULT_CENTRE]*4 # Trim Servo Centre - self.servo_range = [_SERVO_DEFAULT_RANGE]*4 # Limit Servo Range - self.servo_rate = [_SERVO_DEFAULT_RATE]*4 # Servo Rate of Change - self.servo_mode = [_SERVO_DEFAULT_MODE]*4 # Servo Mode [0:Position, 1: Scan] - self.servo_selected = 0 - self._servo_modes = ['Off','Trim','Position','Scanning'] + self._time_since_last_update: int = 0 + self._keep_alive_period: int = 500 # ms (half the value used in hexdrive.py) + self.num_servos: int = 4 # Default assumed for a single HexDrive + self.servo = [None]*4 # Servo Positions + self.servo_centre = [_SERVO_DEFAULT_CENTRE]*4 # Trim Servo Centre + self.servo_range = [_SERVO_DEFAULT_RANGE]*4 # Limit Servo Range + self.servo_rate = [_SERVO_DEFAULT_RATE]*4 # Servo Rate of Change + self.servo_mode = [ServoMode.OFF]*4 # Servo Mode + self.servo_selected: int = 0 + #self._servo_modes = list(ServoMode) # Overall app state (controls what is displayed and what user inputs are accepted) self.current_state = STATE_INIT @@ -242,8 +267,7 @@ def __init__(self): eventbus.on_async(RequestForegroundPopEvent, self._lose_focus, self) # We start with focus on launch, without an event emmited - self._gain_focus(RequestForegroundPushEvent(self)) - + self._gain_focus(RequestForegroundPushEvent(self)) ### ASYNC EVENT HANDLERS ### @@ -316,7 +340,7 @@ async def background_task(self): ### NON-ASYNC FUCNTIONS ### - def background_update(self, delta): + def background_update(self, delta: int): if self.current_state == STATE_RUN: output = self.get_current_power_level(delta) if output is None: @@ -353,7 +377,7 @@ def scan_ports(self): self.check_port_for_hexdrive(port) - def check_port_for_hexdrive(self, port) -> bool: + def check_port_for_hexdrive(self, port: int) -> bool: # we know the EEPROM address so we can just read the header directly if port not in range(1, 7): return False @@ -382,7 +406,7 @@ def check_port_for_hexdrive(self, port) -> bool: return False - def update_app_in_eeprom(self, port, addr) -> bool: + def update_app_in_eeprom(self, port: int, addr: int) -> bool: # Copy hexdrive.mpy to EEPROM as app.mpy if self._settings['logging'].v: print(f"H:Updating HexDrive app.mpy on port {port}") @@ -463,7 +487,7 @@ def update_app_in_eeprom(self, port, addr) -> bool: return True - def prepare_eeprom(self, port, addr) -> bool: + def prepare_eeprom(self, port: int, addr: int) -> bool: if self._settings['logging'].v: print(f"H:Initialising EEPROM on port {port}") hexdrive_header = HexpansionHeader( @@ -526,7 +550,7 @@ def prepare_eeprom(self, port, addr) -> bool: return True - def erase_eeprom(self, port, addr) -> bool: + def erase_eeprom(self, port: int, addr: int) -> bool: if self._settings['logging'].v: print(f"H:Erasing EEPROM on port {port}") try: @@ -552,7 +576,7 @@ def erase_eeprom(self, port, addr) -> bool: return True - def find_hexdrive_app(self, port) -> app: + def find_hexdrive_app(self, port: int) -> app: for an_app in scheduler.apps: if hasattr(an_app, "config") and hasattr(an_app.config, "port") and an_app.config.port == port: return an_app @@ -578,7 +602,7 @@ def _pattern_management(self): ### MAIN APP CONTROL FUNCTIONS ### - def update(self, delta): + def update(self, delta: int): if self.notification: self.notification.update(delta) @@ -607,7 +631,7 @@ def update(self, delta): ### START UI FOR HEXPANSION INITIALISATION AND UPGRADE ### - def _update_hexpansion_management(self, delta): + def _update_hexpansion_management(self, delta: int): if self.current_state == STATE_INIT: # One Time initialisation self.scan_ports() @@ -651,7 +675,7 @@ def _update_hexpansion_management(self, delta): self._update_state_check(delta) - def _update_main_application(self, delta): + def _update_main_application(self, delta: int): if self.current_state == STATE_MENU: if self.current_menu is None: self.set_menu("main") @@ -684,13 +708,17 @@ def _update_main_application(self, delta): elif self.current_state == STATE_SERVO: self._update_state_servo(delta) + ### Stepper Tester Application ### + elif self.current_state == STATE_STEPPER: + self._update_state_stepper(delta) + ### Settings Capability ### elif self.current_state == STATE_SETTINGS: self._update_state_settings(delta) ### End of Update ### - def _update_state_warning(self, delta): + def _update_state_warning(self, delta: int): if self.button_states.get(BUTTON_TYPES["CONFIRM"]): self.button_states.clear() if self.current_state == STATE_WARNING or self.hexdrive_port is not None: @@ -722,7 +750,7 @@ def _update_state_warning(self, delta): tildagonos.leds[i] = (255,0,0) - def _update_state_error(self, delta): + def _update_state_error(self, delta: int): if self.button_states.get(BUTTON_TYPES["CONFIRM"]): # Error has been acknowledged by the user self.button_states.clear() @@ -733,7 +761,7 @@ def _update_state_error(self, delta): tildagonos.leds[i] = (0,255,0) if self.current_state == STATE_MESSAGE else (255,0,0) - def _update_state_programming(self, delta): + def _update_state_programming(self, delta: int): if self.upgrade_port is not None: if self.update_app_in_eeprom(self.upgrade_port, _EEPROM_ADDR): self.notification = Notification("Upgraded", port = self.upgrade_port) @@ -766,7 +794,7 @@ def _update_state_programming(self, delta): print("H:Error - no port to program") - def _update_state_detected(self, delta): + def _update_state_detected(self, delta: int): # We are currently asking the user if they want hexpansion EEPROM initialising if self.button_states.get(BUTTON_TYPES["CONFIRM"]): self.button_states.clear() @@ -795,7 +823,7 @@ def _update_state_detected(self, delta): self._refresh = True - def _update_state_erase(self, delta): + def _update_state_erase(self, delta: int): # We are currently asking the user if they want hexpansion EEPROM Erased if self.button_states.get(BUTTON_TYPES["CONFIRM"]): # Yes @@ -818,7 +846,7 @@ def _update_state_erase(self, delta): self.current_state = STATE_CHECK - def _update_state_upgrade(self, delta): + def _update_state_upgrade(self, delta: int): if self.button_states.get(BUTTON_TYPES["CONFIRM"]): # Yes self.button_states.clear() @@ -833,7 +861,7 @@ def _update_state_upgrade(self, delta): self.current_state = STATE_CHECK - def _update_state_check(self, delta): + def _update_state_check(self, delta: int): #print(f"Check: {self.ports_with_latest_hexdrive}") if 0 < len(self.ports_with_latest_hexdrive): # We have at least one HexDrive with the latest App.mpy @@ -849,10 +877,11 @@ def _update_state_check(self, delta): self.hexdrive_port = valid_port self.hexdrive_app = hexdrive_app if self.hexpansion_slot_type[valid_port-1] is not None: - self.num_motors = self._HEXDRIVE_TYPES[self.hexpansion_slot_type[valid_port-1]].motors - self.num_servos = self._HEXDRIVE_TYPES[self.hexpansion_slot_type[valid_port-1]].servos + self.num_motors = self._HEXDRIVE_TYPES[self.hexpansion_slot_type[valid_port-1]].motors + self.num_servos = self._HEXDRIVE_TYPES[self.hexpansion_slot_type[valid_port-1]].servos + self.num_steppers = self._HEXDRIVE_TYPES[self.hexpansion_slot_type[valid_port-1]].steppers # only intended for use with a single active HexDrive at once at present - if self.hexdrive_app.get_status(): + if (0 < self._HEXDRIVE_TYPES[self.hexpansion_slot_type[valid_port-1]].steppers) or self.hexdrive_app.get_status(): if self._settings['logging'].v: print(f"H:HexDrive [{valid_port}] OK") self.current_state = STATE_MENU @@ -880,7 +909,7 @@ def _update_state_check(self, delta): self.current_state = STATE_WARNING - def _check_hexpansion_ports(self, delta) -> bool: + def _check_hexpansion_ports(self, delta: int) -> bool: if 0 < len(self.ports_with_blank_eeprom): # if there are any ports with blank eeproms # Show the UI prompt and wait for button press @@ -891,7 +920,7 @@ def _check_hexpansion_ports(self, delta) -> bool: return False - def _check_hexdrive_ports(self, delta) -> bool: + def _check_hexdrive_ports(self, delta: int) -> bool: #print(f"Check HexDrive Ports: {self.waiting_app_port} {self.ports_with_hexdrive}") if self.waiting_app_port is not None or (0 < len(self.ports_with_hexdrive)): # if there are any ports with HexDrives - check if they need upgrading/erasing @@ -944,7 +973,7 @@ def _check_hexdrive_ports(self, delta) -> bool: return False - def _update_state_help(self, delta): + def _update_state_help(self, delta: int): if self.button_states.get(BUTTON_TYPES["CANCEL"]): self.button_states.clear() self.current_state = STATE_MENU @@ -962,7 +991,7 @@ def _update_state_help(self, delta): self.current_state = STATE_LOGO - def _update_state_receive_instr(self, delta): + def _update_state_receive_instr(self, delta: int): # Enable/disable scrolling and check for long press if self.button_states.get(BUTTON_TYPES["CONFIRM"]): self.long_press_delta += delta @@ -1020,7 +1049,7 @@ def _update_state_receive_instr(self, delta): self._set_direction_leds(self.last_press) - def _set_direction_leds(self, direction): + def _set_direction_leds(self, direction: Button): if direction == BUTTON_TYPES["RIGHT"]: # Green = Starboard = Right self.clear_leds() @@ -1043,7 +1072,7 @@ def _set_direction_leds(self, direction): tildagonos.leds[7] = (255, 0, 255) - def _update_state_countdown(self, delta): + def _update_state_countdown(self, delta: int): self.clear_leds() self.run_countdown_elapsed_ms += delta if self.run_countdown_elapsed_ms >= _RUN_COUNTDOWN_MS: @@ -1053,7 +1082,7 @@ def _update_state_countdown(self, delta): self.current_state = STATE_RUN - def _update_state_done(self, delta): + def _update_state_done(self, delta: int): if self.button_states.get(BUTTON_TYPES["CANCEL"]): self.button_states.clear() if self.hexdrive_app is not None: @@ -1067,14 +1096,89 @@ def _update_state_done(self, delta): self.current_power_duration = ((0,0,0,0), 0) self.current_state = STATE_COUNTDOWN + # Servo Tester: + def _update_state_stepper(self, delta: int): + # Left/Right to adjust position + if self.button_states.get(BUTTON_TYPES["RIGHT"]): + if self._auto_repeat_check(delta, True): + if self.stepper_mode == StepperMode.SPEED: # Speed + speed = self._stepper.get_speed() + speed = self._inc(speed, self._auto_repeat_level+1) + if _STEPPER_MAX_SPEED < speed: + speed = _STEPPER_MAX_SPEED + self._stepper.speed(speed) + else: + if self.stepper_mode != StepperMode.POSITION: # Position Mode + self.stepper_mode = StepperMode.POSITION + self._stepper.speed(_STEPPER_DEFAULT_SPEED) + self._stepper.enable(True) + self._stepper.track_target() + pos = self._stepper.get_pos() + pos = self._inc(pos, self._auto_repeat_level+1) + self._stepper.target(pos) + self._refresh = True + elif self.button_states.get(BUTTON_TYPES["LEFT"]): + if self._auto_repeat_check(delta, True): + if self.stepper_mode == StepperMode.SPEED: # Speed + speed = self._stepper.get_speed() + speed = self._dec(speed, self._auto_repeat_level+1) + if -_STEPPER_MAX_SPEED > speed: + speed = -_STEPPER_MAX_SPEED + self._stepper.speed(speed) + else: # Position Mode + if self.stepper_mode != StepperMode.POSITION: + self.stepper_mode = StepperMode.POSITION + self._stepper.speed(_STEPPER_DEFAULT_SPEED) + self._stepper.enable(True) + self._stepper.track_target() + pos = self._stepper.get_pos() + pos = self._dec(pos, self._auto_repeat_level+1) + self._stepper.target(pos) + self._refresh = True + else: + self._auto_repeat_clear() + # non auto-repeating buttons + if self.button_states.get(BUTTON_TYPES["CANCEL"]): + self.button_states.clear() + if self.hexdrive_app is not None: + self._stepper.stop() + self.current_state = STATE_MENU + return + elif self.button_states.get(BUTTON_TYPES["CONFIRM"]): #Cycle Through Modes + self.button_states.clear() + #self.stepper_mode = self._stepper_modes[(self._stepper_modes.index(self.stepper_mode) + 1) % len(self._stepper_modes)] + #self.stepper_mode = (self.stepper_mode + 1) % len(StepperMode) + if self.stepper_mode == StepperMode.POSITION: # Position Mode + self._stepper.speed(_STEPPER_DEFAULT_SPEED) + self._stepper.enable(True) + self._stepper.target(self._stepper.get_pos()) + self._stepper.track_target() + elif self.stepper_mode == StepperMode.SPEED: # Speed Mode + self._stepper.enable(True) + self._stepper.speed(0) + self._stepper.free_run(1) + else: # Off + self._stepper.enable(False) + self._refresh = True + self.notification = Notification(f" Stepper:\n {self.stepper_mode}") + print(f"Stepper:{self.stepper_mode}") + if self.stepper_mode == StepperMode.SPEED: # Speed Mode + self._refresh = True + self._time_since_last_update += delta + if self._time_since_last_update > self._keep_alive_period: + self._stepper.step() + self._time_since_last_update = 0 + + _refresh = self._refresh + - def _update_state_servo(self, delta): + def _update_state_servo(self, delta: int): # Servo Tester: # Up/Down to select Servo # Left/Right to adjust position if self.button_states.get(BUTTON_TYPES["RIGHT"]): - if self._auto_repeat_check(delta, not (self.servo_mode[self.servo_selected] == 3)): - if self.servo_mode[self.servo_selected] == 1: # Trim mode: + if self._auto_repeat_check(delta, (self.servo_mode[self.servo_selected] != ServoMode.SCANNING)): + if self.servo_mode[self.servo_selected] == ServoMode.TRIM: # adjust the servo centre position self.servo_centre[self.servo_selected] += self._settings['servo_step'].v if self.servo_centre[self.servo_selected] > (_SERVO_DEFAULT_CENTRE + _SERVO_MAX_TRIM): @@ -1082,7 +1186,7 @@ def _update_state_servo(self, delta): if self.hexdrive_app is not None: if not self.hexdrive_app.set_servocentre(self.servo_centre[self.servo_selected], self.servo_selected): print("H:Failed to set servo centre") - elif self.servo_mode[self.servo_selected] == 3: # Scanning Mode + elif self.servo_mode[self.servo_selected] == ServoMode.SCANNING: # as the rate changes sign when it reaches the range, we must be careful to modify it in the correct direction if self.servo_rate[self.servo_selected] < 0: negative = True @@ -1100,15 +1204,15 @@ def _update_state_servo(self, delta): else: # Position Mode if self.servo[self.servo_selected] is None: self.servo[self.servo_selected] = 0 - self.servo_mode[self.servo_selected] = 2 + self.servo_mode[self.servo_selected] = ServoMode.POSITION self.servo[self.servo_selected] += self._settings['servo_step'].v if self.servo[self.servo_selected] is not None: if self.servo_range[self.servo_selected] < (self.servo[self.servo_selected] + (self.servo_centre[self.servo_selected] - _SERVO_DEFAULT_CENTRE)): self.servo[self.servo_selected] = self.servo_range[self.servo_selected] - (self.servo_centre[self.servo_selected] - _SERVO_DEFAULT_CENTRE) self._refresh = True elif self.button_states.get(BUTTON_TYPES["LEFT"]): - if self._auto_repeat_check(delta, not (self.servo_mode[self.servo_selected] == 3)): - if self.servo_mode[self.servo_selected] == 1: # Trim mode: + if self._auto_repeat_check(delta, (self.servo_mode[self.servo_selected] != ServoMode.SCANNING)): + if self.servo_mode[self.servo_selected] == ServoMode.TRIM: # adjust the servo centre position self.servo_centre[self.servo_selected] -= self._settings['servo_step'].v if self.servo_centre[self.servo_selected] < (_SERVO_DEFAULT_CENTRE - _SERVO_MAX_TRIM): @@ -1116,7 +1220,7 @@ def _update_state_servo(self, delta): if self.hexdrive_app is not None: if not self.hexdrive_app.set_servocentre(self.servo_centre[self.servo_selected], self.servo_selected): print("H:Failed to set servo centre") - elif self.servo_mode[self.servo_selected] == 3: # Scanning Mode + elif self.servo_mode[self.servo_selected] == ServoMode.SCANNING: # as the rate changes sign when it reaches the range, we must be careful to modify it in the correct direction if self.servo_rate[self.servo_selected] < 0: negative = True @@ -1134,7 +1238,7 @@ def _update_state_servo(self, delta): else: # Position Mode if self.servo[self.servo_selected] is None: self.servo[self.servo_selected] = 0 - self.servo_mode[self.servo_selected] = 2 + self.servo_mode[self.servo_selected] = ServoMode.POSITION self.servo[self.servo_selected] -= self._settings['servo_step'].v if self.servo[self.servo_selected] is not None: if -self.servo_range[self.servo_selected] > (self.servo[self.servo_selected] + (self.servo_centre[self.servo_selected] - _SERVO_DEFAULT_CENTRE)): @@ -1160,13 +1264,14 @@ def _update_state_servo(self, delta): return elif self.button_states.get(BUTTON_TYPES["CONFIRM"]): #Cycle Through Modes self.button_states.clear() - self.servo_mode[self.servo_selected] = (self.servo_mode[self.servo_selected] + 1) % 4 - if self.servo_mode[self.servo_selected] == 0: + #self.servo_mode[self.servo_selected] = self._servo_modes[(self._servo_modes.index(self.servo_mode[self.servo_selected]) + 1) % len(self._servo_modes)] + #self.servo_mode[self.servo_selected] = (self.servo_mode[self.servo_selected] + 1) % len(ServoMode) + if self.servo_mode[self.servo_selected] == ServoMode.OFF: if self.hexdrive_app is not None: self.hexdrive_app.set_servoposition(self.servo_selected, None) else: self._refresh = True - self.notification = Notification(f" Servo {self.servo_selected}:\n {self._servo_modes[self.servo_mode[self.servo_selected]]}") + self.notification = Notification(f" Servo {self.servo_selected}:\n {self.servo_mode[self.servo_selected]}") self._time_since_last_update += delta if self._time_since_last_update > self._keep_alive_period: @@ -1175,7 +1280,7 @@ def _update_state_servo(self, delta): for i in range(self.num_servos): _refresh = self._refresh - if self.servo_mode[i] == 3: + if self.servo_mode[i] == ServoMode.SCANNING: # for any servo set to Scan mode, update the position if self.servo[self.servo_selected] is None: self.servo[self.servo_selected] = 0 @@ -1189,12 +1294,12 @@ def _update_state_servo(self, delta): self.servo_rate[i] = -self.servo_rate[i] self.servo[i] = -self.servo_range[i] - (self.servo_centre[i] - _SERVO_DEFAULT_CENTRE) _refresh = True - if _refresh and self.hexdrive_app is not None and self.servo_mode[i] != 0 and self.servo[i] is not None: + if _refresh and self.hexdrive_app is not None and self.servo_mode[i] != ServoMode.OFF and self.servo[i] is not None: # scanning servo or the selected servo self.hexdrive_app.set_servoposition(i, int(self.servo[i])) - def _update_state_settings(self, delta): + def _update_state_settings(self, delta: int): 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) @@ -1223,7 +1328,7 @@ def _update_state_settings(self, delta): # leave setting unchanged if self._settings['logging'].v: print(f"Setting: {self._edit_setting} Cancelled") - self.set_menu(_main_menu_items[2]) + self.set_menu(_main_menu_items[3]) self.current_state = STATE_MENU elif self.button_states.get(BUTTON_TYPES["CONFIRM"]): self.button_states.clear() @@ -1233,7 +1338,7 @@ def _update_state_settings(self, delta): 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.set_menu(_main_menu_items[2]) + self.set_menu(_main_menu_items[3]) self.current_state = STATE_MENU @@ -1297,6 +1402,8 @@ def draw(self, ctx): button_labels(ctx, confirm_label="Replay", cancel_label="Restart") elif self.current_state == STATE_SERVO: self._draw_state_servo(ctx) + elif self.current_state == STATE_STEPPER: + self._draw_state_stepper(ctx) elif self.current_state == STATE_SETTINGS: self.draw_message(ctx, ["Edit Setting",f"{self._edit_setting}:",f"{self._edit_setting_value}"], [(1,1,1),(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") @@ -1335,6 +1442,51 @@ def _draw_receive_instr(self, ctx): ctx.rgb(*colour).move_to(H_START, V_START + label_font_size * (self.scroll_offset + i_num)).text(str(instr)) + def _draw_state_stepper(self, ctx): + stepper_text = ["S"]*(1+self.num_steppers) # Servo Text + stepper_text_colours = [(0.4,0.0,0.0)]*(1+self.num_steppers) # Red + stepper_text[0] = "Stepper Test" + stepper_text_colours[0] = (1,1,1) # Title - White + if self._stepper is not None: + i = 0 + # Select Colour according to mode + if self.stepper_mode == StepperMode.OFF: + body_colour = (0.2,0.2,0.2) # Not activated - Grey + bar_colour = (0.4,0.4,0.4) # Not activated - Grey + else: + body_colour = (0.1,0.1,0.5) # Active - Blue + bar_colour = (0.1,0.1,1.0) # Active - Blue + stepper_text_colours[1] = (0.4,0.4,0.0) # Active - Yellow + + # draw the servo positions + ctx.save() + # y = i-1.5 for 4 servos, y = i-0.5 for 2 servos + ctx.translate(0, (i-(self.num_steppers/2)+0.5) * label_font_size) + # background for the servo position - grey + background_colour = (0.15,0.15,0.15) + ctx.rgb(*background_colour).rectangle(-100,1,200,label_font_size-2).fill() + c = 0 + # draw the stepper position + x = 100 * (self._stepper.get_pos() / _STEPPER_MAX_POSITION) + # vertical bar at servo position + ctx.rgb(*bar_colour).rectangle(x-2,1,5,label_font_size-2).fill() + # horizontal bar from 0 to servo position, not covering the centre marker or the servo position bar + ctx.rgb(*body_colour) + if x > (c+4): + ctx.rectangle(c+1, 3, x-c-4, label_font_size-6).fill() + elif x < (c-4): + ctx.rectangle(x+4, 3, c-x-4, label_font_size-6).fill() + # marker for the centre - black (drawn last as it may have to go through the servo position bar) + ctx.rgb(0,0,0).move_to(c,0).line_to(c,label_font_size).stroke() + ctx.restore() + if self.stepper_mode == StepperMode.SPEED: # Speed + stepper_text[i+1] = f"{int(self._stepper.get_speed()):4}/s" # Speed in steps per second + else: # Position + stepper_text[i+1] = "Off" if (self.stepper_mode == StepperMode.OFF) else f"{int(self._stepper.get_pos()):+6} " + self.draw_message(ctx, stepper_text, stepper_text_colours, label_font_size) + button_labels(ctx, confirm_label="Mode", cancel_label="Exit", left_label="<--", right_label="-->") + + def _draw_state_servo(self, ctx): servo_text = ["S"]*(1+self.num_servos) # Servo Text servo_text_colours = [(0.4,0.0,0.0)]*(1+self.num_servos) # Red @@ -1343,10 +1495,10 @@ def _draw_state_servo(self, ctx): for i in range(self.num_servos): # Select Colour according to mode - if self.servo[i] is None or self.servo_mode[i] == 0: + if self.servo[i] is None or self.servo_mode[i] == ServoMode.OFF: body_colour = (0.2,0.2,0.2) # Not activated - Grey bar_colour = (0.4,0.4,0.4) # Not activated - Grey - elif self.servo_mode[i] == 3: + elif self.servo_mode[i] == ServoMode.SCANNING: body_colour = (0.1,0.5,0.1) # Scanning - Green bar_colour = (0.1,1.0,0.1) # Scanning - Green servo_text_colours[1+i] = (0.4,0.0,0.4) # Scanning - Magenta @@ -1379,17 +1531,17 @@ def _draw_state_servo(self, ctx): # marker for the centre - black (drawn last as it may have to go through the servo position bar) ctx.rgb(0,0,0).move_to(c,0).line_to(c,label_font_size).stroke() ctx.restore() - if self.servo_mode[i] == 3: + if self.servo_mode[i] == ServoMode.SCANNING: servo_text[i+1] = f"{int(abs(self.servo_rate[i])):4}/s" # Scanning Rate else: # Position - servo_text[i+1] = "Off" if (self.servo[i] is None or self.servo_mode[i] == 0) else f"{int(self.servo[i]):+5} " + servo_text[i+1] = "Off" if (self.servo[i] is None or self.servo_mode[i] == ServoMode.OFF) else f"{int(self.servo[i]):+5} " # Selected Servo - Brighter Text servo_text_colours[1+self.servo_selected] = tuple(int(j * 2.5) for j in servo_text_colours[1+self.servo_selected]) self.draw_message(ctx, servo_text, servo_text_colours, label_font_size) - if self.servo_mode[self.servo_selected] == 3: + if self.servo_mode[self.servo_selected] == ServoMode.SCANNING: # Scanning mode button_labels(ctx, up_label="^", down_label="\u25BC", confirm_label="Mode", cancel_label="Exit", left_label="Slower", right_label="Faster") - elif self.servo_mode[self.servo_selected] == 1: + elif self.servo_mode[self.servo_selected] == ServoMode.TRIM: button_labels(ctx, up_label="^", down_label="\u25BC", confirm_label="Mode", cancel_label="Exit", left_label="Trim-", right_label="+Trim") else: #Position mode @@ -1398,7 +1550,7 @@ def _draw_state_servo(self, ctx): # Value increment/decrement functions for positive integers only - def _inc(self, v, l): + def _inc(self, v: int, l: int): if l==0: return v+1 else: @@ -1406,7 +1558,7 @@ def _inc(self, v, l): v = ((v // d) + 1) * d # round up to the next multiple of 10^l return v - def _dec(self, v, l): + def _dec(self, v: int, l: int): if l==0: return v-1 else: @@ -1485,9 +1637,11 @@ def set_menu(self, menu_name = "main"): #: Literal["main"]): does it work witho # construct the main menu based on template menu_items = _main_menu_items.copy() if self.num_servos == 0: - menu_items.remove("Servo Test") + menu_items.remove(_main_menu_items[2]) + if self.num_steppers == 0: + menu_items.remove(_main_menu_items[1]) if self.num_motors == 0: - menu_items.remove("Motor Moves") + menu_items.remove(_main_menu_items[0]) self.menu = Menu( self, menu_items, @@ -1508,7 +1662,7 @@ def set_menu(self, menu_name = "main"): #: Literal["main"]): does it work witho # this appears to be able to be called at any time - def _main_menu_select_handler(self, item, idx): + def _main_menu_select_handler(self, item: str, idx: int): if self._settings['logging'].v: print(f"H:Main Menu {item} at index {idx}") if item == _main_menu_items[0]: # Motor Test - Turtle/Logo mode @@ -1522,7 +1676,27 @@ def _main_menu_select_handler(self, item, idx): self._animation_counter = 0 self.current_state = STATE_HELP self._refresh = True - elif item == _main_menu_items[1]: # Servo Test + elif item == _main_menu_items[1]: # Stepper Test + if self.num_steppers == 0: + self.notification = Notification("No Steppers") + else: + if self._stepper is None: + # try timer IDs 0-3 until one is free + for i in range(4): + try: + self._stepper = Stepper(self.hexdrive_app, timer_id=i) # self._settings['stepper_steps'].v) + break + except: + pass + if self._stepper is None: + self.notification = Notification("No Free Timers") + else: + self.set_menu(None) + self.button_states.clear() + self.current_state = STATE_STEPPER + self._refresh = True + self._auto_repeat_clear() + elif item == _main_menu_items[2]: # Servo Test if self.num_servos == 0: self.notification = Notification("No Servos") else: @@ -1532,24 +1706,24 @@ def _main_menu_select_handler(self, item, idx): self.current_state = STATE_SERVO self._refresh = True self._auto_repeat_clear() - elif item == _main_menu_items[2]: # Settings - self.set_menu(_main_menu_items[2]) - elif item == _main_menu_items[3]: # About + elif item == _main_menu_items[3]: # Settings + self.set_menu(_main_menu_items[3]) + elif item == _main_menu_items[4]: # About self.set_menu(None) self.button_states.clear() self._animation_counter = 0 self.current_state = STATE_LOGO self._refresh = True - elif item == _main_menu_items[4]: # Exit + elif item == _main_menu_items[5]: # Exit eventbus.remove(HexpansionInsertionEvent, self._handle_hexpansion_insertion, self) eventbus.remove(HexpansionRemovalEvent, self._handle_hexpansion_removal, self) 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, idx): + def _settings_menu_select_handler(self, item: str, idx: int): if self._settings['logging'].v: - print(f"H:Setting {item} at index {idx}") + print(f"H:Setting {item} @ {idx}") if idx == 0: #Save if self._settings['logging'].v: print("H:Settings Save All") @@ -1596,7 +1770,7 @@ def _handle_instruction_press(self, press_type: Button): # multi level auto repeat # if speed_up is True, the auto repeat gets faster the longer the button is held # otherwise it is a fixed rate, but the level is used to determine the scale of the increase in the setttings inc() and dec() functions - def _auto_repeat_check(self, delta, speed_up = True) -> bool: + def _auto_repeat_check(self, delta: int, speed_up: bool = True) -> bool: 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]: @@ -1635,7 +1809,7 @@ def reset_robot(self): self.power_plan_iter = iter([]) - def get_current_power_level(self, delta) -> int: + def get_current_power_level(self, delta: int) -> int: # takes in delta as ms since last call # if delta was > 10... what to do if delta >= _TICK_MS: @@ -1667,13 +1841,175 @@ def finalize_instruction(self): self.current_instruction = None +######## STEPPER MOTOR CLASS ######## + +class Stepper: + def __init__(self, hexdrive_app, steps_per_rev: int = _STEPPER_DEFAULT_SPR, speed_sps: int = _STEPPER_DEFAULT_SPEED, max_sps: int = _STEPPER_MAX_SPEED, max_pos: int = _STEPPER_MAX_POSITION, timer_id: int = 0): + self._hexdrive_app = hexdrive_app + self._phase = 0 + self._calibrated = False + self._timer = Timer(timer_id) + self._timer_is_running=False + self._free_run_mode=0 # direction of free run mode + self._enabled=False + self._target_pos = 0 + self._pos = 0 + self._max_sps = int(max_sps) + self._steps_per_sec = int(speed_sps) + self._steps_per_rev = int(steps_per_rev) # full steps per revolution + self._max_pos = int(max_pos) + self._freq = 0 + self.track_target() + + def speed(self,sps): + if self._free_run_mode == 1 and sps < 0: + self._free_run_mode = -1 + elif self._free_run_mode == -1 and sps > 0: + self._free_run_mode = 1 + if sps > self._max_sps: + sps = self._max_sps + elif sps < -self._max_sps: + sps = -self._max_sps + self._steps_per_sec = int(sps) + self._update_timer(2*abs(self._steps_per_sec)) # half steps per second + + def speed_rps(self,rps): + self.speed(rps*self._steps_per_rev) + + def get_speed(self) -> int: + return self._steps_per_sec + + def target(self,t): + self._target_pos = int(t) + + def target_deg(self,deg): + self.target(self._steps_per_rev*deg/180.0) # target pos is in half steps + + def target_rad(self,rad): + self.target(self._steps_per_rev*rad/pi) # target pos is in half steps + + def get_pos(self) -> int: + return self._pos + + def get_pos_deg(self) -> float: + return self.get_pos()*180.0/self._steps_per_rev # half steps to degrees + + def get_pos_rad(self) -> float: + return self.get_pos()*pi/self._steps_per_rev # half steps to radians + + def overwrite_pos(self,p=0): + self._pos = int(p) + + def overwrite_pos_deg(self,deg): + self.overwrite_pos(deg*self._steps_per_rev/180.0) # degrees to half steps + + def overwrite_pos_rad(self,rad): + self.overwrite_pos(rad*self._steps_per_rev/pi) # radians to half steps + + def step(self,d=0): + if self._enabled: + if d>0: + self._pos+=1 + elif d<0: + self._pos-=1 + # Check position limits + if self._calibrated and self._pos < 0: + self._pos = 0 + return + elif self._calibrated and self._pos > self._max_pos: + self._pos = self._max_pos + return + elif d>0: + self._phase = (self._phase-1)%_STEPPER_NUM_PHASES + elif d<0: + self._phase = (self._phase+1)%_STEPPER_NUM_PHASES + #print(f"p:{self._pos}") + try: + if not self._hexdrive_app.motor_step(self._phase): + # we have reached the endstop + self._hit_endstop() + except Exception as e: + print(e) + + def _hit_endstop(self): + print("Endstop - hit") + if not self._calibrated: + self._calibrated = True + # set this as the new zero position + self.overwrite_pos(0) + + + def _timer_callback(self,t): + if self._free_run_mode>0: + self.step(1) + elif self._free_run_mode<0: + self.step(-1) + elif self._target_pos>self._pos: + self.step(1) + elif self._target_pos bool: + return self._enabled + +########## END OF STEPPER CLASS ########## + + + class HexDriveType: - def __init__(self, pid, vid=0xCAFE, motors=0, servos=0, name="Unknown"): + def __init__(self, pid, vid = 0xCAFE, motors = 0, steppers = 0, servos = 0, name ="Unknown"): self.vid = vid self.pid = pid self.name = name self.motors = motors self.servos = servos + self.steppers = steppers class MySetting: @@ -1759,7 +2095,7 @@ def persist(self): class Instruction: def __init__(self, press_type: Button) -> None: self._press_type = press_type - self._duration: int = 1 + self._duration = 1 self.power_plan = [] diff --git a/hexdrive.py b/hexdrive.py index f06194a..f4d109e 100644 --- a/hexdrive.py +++ b/hexdrive.py @@ -4,18 +4,19 @@ import asyncio import ota -from machine import I2C, PWM, Pin +from machine import PWM, Pin from system.eventbus import eventbus from system.scheduler.events import RequestStopAppEvent import app # HexDrive.py App Version - used to check if upgrade is required -APP_VERSION = 4 +APP_VERSION = 5 -_ENABLE_PIN = 0 # First LS pin used to enable the SMPSU -_DETECT_PIN = 1 # Second LS pin used to sense if the SMPSU has a source of power +_ENABLE_PIN = 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 +_ENDSTOP_PIN = 4 # Endstop switch input pin _DEFAULT_PWM_FREQ = 20000 _DEFAULT_SERVO_FREQ = 50 # 20mS period @@ -25,10 +26,11 @@ _MAX_SERVO_RANGE = 1400 # 1400us either side of centre (VERY WIDE) _SERVO_MAX_TRIM = 1000 # us +_STEPPER_NUM_PHASES = 8 + _EEPROM_ADDR = 0x50 _EEPROM_NUM_ADDRESS_BYTES = 2 _PID_ADDR = 0x12 -_SYSTEM_I2C_BUS = 7 class HexDriveApp(app.App): @@ -39,22 +41,28 @@ def __init__(self, config=None): self._HEXDRIVE_TYPES = [HexDriveType(0xCB, motors=2, servos=4), HexDriveType(0xCA, motors=2, name="2 Motor"), HexDriveType(0xCC, servos=4, name="4 Servo"), - HexDriveType(0xCD, motors=1, servos=2, name = "1 Mot 2 Srvo")] + HexDriveType(0xCD, motors=1, servos=2, name = "1 Mot 2 Srvo"), + HexDriveType(0xCE, steppers=1, name = "Stepper")] self._hexdrive_type_index = None - self._use_custom_ls_pin_functions = True self._keep_alive_period = _DEFAULT_KEEP_ALIVE_PERIOD self._power_state = None - self._pwm_setup_failed = True + self._pwm_setup = False self._time_since_last_update = 0 self._outputs_energised = False self.PWMOutput = [None] * 4 self._freq = [0] * 4 self._motor_output = [0] * 2 + # define the stepping sequence for a 8-phase stepper motor as a list of 4-tuples + self._step = [(1,0,1,0), (0,0,1,0), (0,1,1,0), (0,1,0,0), (0,1,0,1), (0,0,0,1), (1,0,0,1), (1,0,0,0)] + self._stepper = False if config is None: print("H:No Config!") - return + return + # LS Pins self._power_detect = self.config.ls_pin[_DETECT_PIN] - self._power_control = self.config.ls_pin[_ENABLE_PIN] + self._power_control = self.config.ls_pin[_ENABLE_PIN] + self._endstop = self.config.ls_pin[_ENDSTOP_PIN] # For Stepper Motor + self._servo_centre = [_SERVO_CENTRE] * 4 eventbus.on_async(RequestStopAppEvent, self._handle_stop_app, self) try: @@ -62,104 +70,56 @@ def __init__(self, config=None): print(f"H:S/W {ver}") # e.g. v1.9.0-beta.1 if ver >= [1, 9, 0]: - self._use_custom_ls_pin_functions = False + pass + else: + print(f"H:BadgeOS Upgrade to v1.9.0+ required") + return except Exception as e: print(f"H:Ver check failed {e}") self.initialise() def initialise(self) -> bool: - self._pwm_setup_failed = True + self._pwm_setup = False if self.config is None: return False # report app starting and which port it is running on print(f"H:HexDrive V{APP_VERSION} by RobotMad on port {self.config.port}") + # 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) # LS Pins - if self._use_custom_ls_pin_functions: - try: - # Badge s/w changes can break even the simplest of things so we need to be prepared - # Set Power Detect Pin to Input and Power Enable Pin to Output - self._set_pin_direction(self._power_detect.pin, 1) # input - self._set_pin_direction(self._power_control.pin, 0) # output - except Exception as e: - print(f"H:{self.config.port}:Legacy ls_pin setup failed {e}") - # as old method failed we will try the new method - self._use_custom_ls_pin_functions = False - if not self._use_custom_ls_pin_functions: - # Try to use v1.9.0+ method of setting up pins - try: - self._power_detect.init(mode=Pin.IN) - self._power_control.init(mode=Pin.OUT) - except Exception as e: - print(f"H:{self.config.port}:Direct ls_pin setup failed {e}") - return False + try: + self._power_detect.init(mode=Pin.IN) + self._power_control.init(mode=Pin.OUT) + self._endstop.init(mode=Pin.IN) # ideally want to configure this as an input with pullup and interrupt + except Exception as e: + print(f"H:{self.config.port}:ls_pin setup failed {e}") + return False self.set_power(False) # read hexpansion header from EEPROM to find out which type we are # and allocate PWM outputs accordingly self._hexdrive_type_index = self._check_port_for_hexdrive(self.config.port) if self._logging and self._hexdrive_type_index is not None: - print(f"H:{self.config.port}:Type:'{self._HEXDRIVE_TYPES[self._hexdrive_type_index].name}'") + print(f"H:{self.config.port}:Type:'{self._HEXDRIVE_TYPES[self._hexdrive_type_index]._name}'") - # HS Pins - if self.config.pin is not None and len(self.config.pin) == 4: - # Allocate PWM generation to pins - for channel, hs_pin in enumerate(self.config.pin): - self._freq[channel] = 0 - try: - # Set HexDrive Hexpansion HS pins to low level outputs - hs_pin.init(mode=Pin.OUT) - hs_pin.value(0) - except Exception as e: - #print(f"H:{self.config.port}:hs pin setup failed {e}") - return False - if self._hexdrive_type_index is not None: - if channel < (2 * self._HEXDRIVE_TYPES[self._hexdrive_type_index].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"H:{self.config.port}:Motor PWM[{channel}]") - else: - # ignore the motor PWM output on odd channel - we will switch it on when needed - pass - else: - # Remaining channels are for servos (can be 4, 2 or 0 servos - self._freq[channel] = _DEFAULT_SERVO_FREQ - #print(f"H:{self.config.port}:Servo PWM[{channel}]") - if 0 < self._freq[channel]: - try: - self.PWMOutput[channel] = PWM(hs_pin, freq = self._freq[channel], duty_u16 = 0) - if self._logging: - print(f"H:{self.config.port}:PWM[{channel}]:{self.PWMOutput[channel]}") - except Exception as e: - # There are a finite number of PWM resources so it is possible that we run out - print(f"H:{self.config.port}:PWM[{channel}]:PWM(init) failed {e}") - return False - self._pwm_setup_failed = False - return not self._pwm_setup_failed + return(self._pwm_init()) def deinitialise(self) -> bool: # Turn off all PWM outputs & release resources self.set_power(False) - for channel, pwm in enumerate(self.PWMOutput): - if self.PWMOutput[channel] is not None: - try: - self.PWMOutput[channel].deinit() - except: - pass - self.PWMOutput[channel] = None - self._freq[channel] = 0 - self._motor_output[(channel>>1)] = 0 + self._pwm_deinit() for hs_pin in self.config.pin: hs_pin.init(mode=Pin.OUT) hs_pin.value(0) return True - # Handle the RequestStopAppEvent so that ew can release resources + # Handle the RequestStopAppEvent so that we can release resources async def _handle_stop_app(self, event): try: if event.app == self: @@ -172,22 +132,26 @@ async def _handle_stop_app(self, event): # Check keep alive period and turn off PWM outputs if exceeded def background_update(self, delta): - if (self.config is None) or self._pwm_setup_failed: + if (self.config is None) or not (self._pwm_setup or self._stepper): return self._time_since_last_update += delta if self._time_since_last_update > self._keep_alive_period: self._time_since_last_update = 0 - for channel,pwm in enumerate(self.PWMOutput): - if self.PWMOutput[channel] is not None: - try: - self.PWMOutput[channel].duty_u16(0) - except Exception as e: - print(f"H:{self.config.port}:PWM[{pwm}]:Off failed {e}") if self._outputs_energised: self._outputs_energised = False # First time the keep alive period has expired so report it if self._logging: - print(f"H:{self.config.port}:Timeout") + print(f"H:{self.config.port}:Timeout") + if self._pwm_setup: + for channel,pwm in enumerate(self.PWMOutput): + if self.PWMOutput[channel] is not None: + try: + self.PWMOutput[channel].duty_u16(0) + except Exception as e: + print(f"H:{self.config.port}:PWM[{pwm}]:Off failed {e}") + elif self._stepper: + self.motor_release() + # we keep retriggering in case anything else has corrupted the PWM outputs @@ -197,7 +161,7 @@ def get_version(self) -> int: # Get the current status of the HexDrive App def get_status(self) -> bool: - return not self._pwm_setup_failed + return self._pwm_setup # Set the logging state @@ -220,20 +184,12 @@ def set_power(self, state) -> bool: print(f"H:{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 - if self._use_custom_ls_pin_functions: - try: - self._set_pin_state(self._power_control.pin, state) - self._set_pin_direction(self._power_control.pin, 0) # in case it gets corrupted by other code - except Exception as e: - print(f"H:{self.config.port}:Legacy power control failed {e}") - return False - else: - try: - self._power_control.init(mode=Pin.OUT) - self._power_control.value(state) - except Exception as e: - print(f"H:{self.config.port}:Direct power control failed {e}") - return False + try: + self._power_control.init(mode=Pin.OUT) + self._power_control.value(state) + except Exception as e: + print(f"H:{self.config.port}:power control failed {e}") + return False self._power_state = state return self._power_state @@ -245,18 +201,11 @@ def get_power(self) -> bool: # Get the current state of the SMPSU power source def get_booster_power(self) -> bool: - if self._use_custom_ls_pin_functions: - try: - return self._get_pin_state(self._power_detect.pin) - except Exception as e: - print(f"H:{self.config.port}:Legacy power detect failed {e}") - return False - else: - try: - return self._power_detect.value() - except Exception as e: - print(f"H:{self.config.port}:Direct power detect failed {e}") - return False + try: + return self._power_detect.value() + except Exception as e: + print(f"H:{self.config.port}:power detect failed {e}") + return False def set_keep_alive(self, period): @@ -265,7 +214,7 @@ def set_keep_alive(self, period): # Use 50 to 200 for Servos and 5000 to 20000 for motors def set_freq(self, freq, channel=None) -> bool: - if self._pwm_setup_failed: + if not self._pwm_setup: return False for this_channel, pwm in enumerate(self.PWMOutput): if channel is None or this_channel == channel: @@ -282,7 +231,7 @@ def set_freq(self, freq, channel=None) -> bool: # Get the current PWM frequency for a specific output def get_freq(self, channel=0) -> int: - if self._pwm_setup_failed: + if not self._pwm_setup: return 0 if channel < 0 or channel >= 4: return 0 @@ -300,7 +249,7 @@ def get_freq(self, channel=0) -> int: # 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) def set_servoposition(self, channel=None, position=None) -> bool: - if self._pwm_setup_failed: + 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) @@ -332,6 +281,7 @@ def set_servoposition(self, channel=None, position=None) -> bool: if abs(position) > _MAX_SERVO_RANGE: return False self._outputs_energised = True + self._stepper = False try: if _MAX_SERVO_FREQ < self.PWMOutput[int(channel)].freq(): # Ensure PWM frequency is suitable for use with Servos @@ -361,7 +311,7 @@ def set_servoposition(self, channel=None, position=None) -> bool: # it will only affect the position next time it is set # you can use this to trim the centre position of the servo def set_servocentre(self, centre, channel=None) -> bool: - if self._pwm_setup_failed: + if not self._pwm_setup: return False if channel is not None and (channel < 0 or channel >= 4): return False @@ -376,7 +326,7 @@ def set_servocentre(self, centre, channel=None) -> bool: # Set pairs of PWM duty cycles in one go using a signed value per motor channel (0-65535) def set_motors(self, outputs) -> bool: - if self._pwm_setup_failed: + if not self._pwm_setup: return False for motor, output in enumerate(outputs): if abs(output) > 65535: @@ -396,14 +346,14 @@ def set_motors(self, outputs) -> bool: self.PWMOutput[(motor<<1)+1] = PWM(self.config.pin[(motor<<1)+1], freq = self._freq[(motor<<1)], duty_u16 = -int(output)) self._set_pwmoutput((motor<<1)+1, -int(output)) self._motor_output[motor] = output - self._check_outputs_energised() + 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) -> bool: - if self._pwm_setup_failed: + if not self._pwm_setup: return False self._outputs_energised = any(duty_cycles) for channel, duty_cycle in enumerate(duty_cycles): @@ -415,7 +365,7 @@ def set_pwm(self, duty_cycles) -> bool: # Get the current PWM duty cycle for a specific output (0-65535) def get_pwm(self, channel=0) -> int: - if self._pwm_setup_failed: + if not self._pwm_setup: return 0 if channel >= len(self.PWMOutput): return 0 @@ -425,6 +375,85 @@ def get_pwm(self, channel=0) -> int: pwm = 0 return pwm +## Stepper Motor Support + # Stepper Motor Support + def motor_step(self, phase) -> bool: + if phase > _STEPPER_NUM_PHASES: + return None + if not self._stepper: + # not currently configured for stepper motor - configure + self._pwm_deinit() + self._stepper = True + # if we have reached the endstop then stop + # we are assuming that the endstop is active low + for channel, value in enumerate(self._step[phase]): + self.config.pin[channel].value(value) + self._outputs_energised = True + self._time_since_last_update = 0 + if not self._endstop.value(): + return False + return True + + + def motor_release(self): + for channel in range(4): + self.config.pin[channel].value(0) + self._outputs_energised = False + self._time_since_last_update = 0 + + + 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, hs_pin in enumerate(self.config.pin): + self._freq[channel] = 0 + if self._hexdrive_type_index is not None: + if channel < (2 * self._HEXDRIVE_TYPES[self._hexdrive_type_index]._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"H:{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_TYPES[self._hexdrive_type_index]._motors) + self._HEXDRIVE_TYPES[self._hexdrive_type_index]._servos): + # Remaining channels are for servos (can be 4, 2 or 0 servos + self._freq[channel] = _DEFAULT_SERVO_FREQ + #print(f"H:{self.config.port}:Servo PWM[{channel}]") + else: + # ignore the remaining channels - we will switch them on when needed + pass + if 0 < self._freq[channel]: + try: + self.PWMOutput[channel] = PWM(hs_pin, freq = self._freq[channel], duty_u16 = 0) + self._stepper = False + if self._logging: + print(f"H:{self.config.port}:PWM[{channel}]:{self.PWMOutput[channel]}") + except Exception as e: + # There are a finite number of PWM resources so it is possible that we run out + print(f"H:{self.config.port}:PWM[{channel}]:PWM(init) failed {e}") + return False + self._pwm_setup = True + return self._pwm_setup + + + # De-initialise all PWM outputs + def _pwm_deinit(self): + for channel, _ in enumerate(self.PWMOutput): + if self.PWMOutput[channel] is not None: + try: + self.PWMOutput[channel].deinit() + except: + pass + self.PWMOutput[channel] = None + self._freq[channel] = 0 + self._motor_output[(channel>>1)] = 0 + self._pwm_setup = False + # are any of the PWM outputs energised? def _check_outputs_energised(self): @@ -500,57 +529,14 @@ def _parse_version(self, version): return components - ### Legacy LS Pin functions for use with BadgeOS < v1.9.0 ### - - # Set the state of a specific LS output pin - def _set_pin_state(self, pin, state) -> bool: - try: - i2c = I2C(_SYSTEM_I2C_BUS) - output_reg = i2c.readfrom_mem(pin[0], 0x02+pin[1], 1)[0] - output_reg = (output_reg | pin[2]) if state else (output_reg & ~pin[2]) - i2c.writeto_mem(pin[0], 0x02+pin[1], bytes([output_reg])) - #if self._logging: - # print(f"H:{self.config.port}:Write to {hex(pin[0])} address {hex(0x02+pin[1])} value {hex(output_reg)}") - return True - except Exception as e: - #print(f"H:{self.config.port}: {e}") - return False - - # Get the state of a specific LS input pin - def _get_pin_state(self, pin) -> bool: - try: - i2c = I2C(_SYSTEM_I2C_BUS) - input_reg = i2c.readfrom_mem(pin[0], 0x00+pin[1], 1)[0] - return (input_reg & pin[2]) != 0 - except Exception as e: - #print(f"H:{self.config.port}: {e}") - return False - - - # Set the direction of a specific LS pin - def _set_pin_direction(self, pin, direction) -> bool: - try: - # Use a Try in case access to i2C(7) is blocked for apps in future - # presumably if this happens then the code will have been updated to - # handle the GPIO direction correctly anyway. - i2c = I2C(_SYSTEM_I2C_BUS) - config_reg = i2c.readfrom_mem(pin[0], 0x04+pin[1], 1)[0] - config_reg = (config_reg | pin[2]) if (1 == direction) else (config_reg & ~pin[2]) - i2c.writeto_mem(pin[0], 0x04+pin[1], bytes([config_reg])) - #if self._logging: - # print(f"H:{self.config.port}:Write to {hex(pin[0])} address {hex(0x04+pin[1])} value {hex(config_reg)}") - return True - except Exception as e: - #print(f"H:{self.config.port}: {e}") - return False - - + class HexDriveType: - def __init__(self, pid_byte, motors=0, servos=0, name="Unknown"): + def __init__(self, pid_byte, motors=0, servos=0, steppers=0, name="Unknown"): self.pid_byte = pid_byte - self.name = name - self.motors = motors - self.servos = servos + self._name = name + self._motors = motors + self._servos = servos + self._steppers = steppers __app_export__ = HexDriveApp From dfb06b04186d51f6444ae479320104e46a8c422c Mon Sep 17 00:00:00 2001 From: robotmad Date: Fri, 2 Aug 2024 00:50:12 +0100 Subject: [PATCH 03/16] stepper wip --- app.py | 118 +++++++++++++++++++++++++++++++++++++--------------- hexdrive.py | 50 +++++++++++----------- 2 files changed, 109 insertions(+), 59 deletions(-) diff --git a/app.py b/app.py index 56b6f5b..744af40 100644 --- a/app.py +++ b/app.py @@ -147,12 +147,46 @@ class StepperMode: OFF = 0 POSITION = 1 SPEED = 2 + stepper_modes = ["OFF", "POSITION", "SPEED"] + + def __init__(self, mode = OFF): + self.mode = mode + + def set(self, mode): + self.mode = mode + + def inc(self): + self.mode = (self.mode + 1) % 3 + + def __eq__(self, other): + return self.mode == other + + def __str__(self): + return self.stepper_modes[self.mode] + class ServoMode: OFF = 0 TRIM = 1 POSITION = 2 SCANNING = 3 + servo_modes = ["OFF", "TRIM", "POSITION", "SCANNING"] + + def __init__(self, mode = OFF): + self.mode = mode + + def set(self, mode): + self.mode = mode + + def inc(self): + self.mode = (self.mode + 1) % 4 + + def __eq__(self, other): + return self.mode == other + + def __str__(self): + return self.servo_modes[self.mode] + class BadgeBotApp(app.App): def __init__(self): @@ -176,15 +210,15 @@ def __init__(self): self.t_msg: str = "RobotMad" self.is_scroll: bool = False self.scroll_offset: int = 0 - self.notification = None - self.error_message = [] - self.current_menu = None - self.menu = None + self.notification: Notification = None + self.error_message = [str] + self.current_menu: str = None + self.menu: Menu = None # BadgeBot Control Sequence Variables self.run_countdown_elapsed_ms: int = 0 - self.instructions = [] - self.current_instruction = None + self.instructions = [Instruction] + self.current_instruction: Instruction = None self.current_power_duration = ((0,0,0,0), 0) self.power_plan_iter = iter([]) @@ -200,9 +234,9 @@ def __init__(self): self._settings['brightness'] = MySetting(self._settings, _BRIGHTNESS, 0.1, 1.0) self._settings['logging'] = MySetting(self._settings, _LOGGING, False, True) self._settings['erase_slot'] = MySetting(self._settings, _ERASE_SLOT, 0, 6) + self._settings['step_max_pos'] = MySetting(self._settings, _STEPPER_MAX_POSITION, 0, 65535) - - self._edit_setting: int = None + self._edit_setting: int = None self._edit_setting_value = None self.update_settings() @@ -227,7 +261,7 @@ def __init__(self): self.hexpansion_init_type: int = 0 self.detected_port: int = None self.waiting_app_port: int = None - self.erase_port: int = None + self.erase_port: int = None self.upgrade_port: int = None self.hexdrive_port: int = None self.ports_with_blank_eeprom = set() @@ -241,12 +275,13 @@ def __init__(self): # Motor Driver self.num_motors: int = 2 # Default assumed for a single HexDrive self.num_steppers: int = 1 # Default assumed for a single HexDrive - self._stepper = None - #self._stepper_modes = list(StepperMode) - self.stepper_mode = StepperMode.OFF + self._stepper: Stepper = None + self.stepper_mode = StepperMode() self.stepper_pos: int = 0 # Servo Tester + self._time_since_last_input: int = 0 + self._timeout_period: int = 60000 # ms self._time_since_last_update: int = 0 self._keep_alive_period: int = 500 # ms (half the value used in hexdrive.py) self.num_servos: int = 4 # Default assumed for a single HexDrive @@ -254,9 +289,8 @@ def __init__(self): self.servo_centre = [_SERVO_DEFAULT_CENTRE]*4 # Trim Servo Centre self.servo_range = [_SERVO_DEFAULT_RANGE]*4 # Limit Servo Range self.servo_rate = [_SERVO_DEFAULT_RATE]*4 # Servo Rate of Change - self.servo_mode = [ServoMode.OFF]*4 # Servo Mode + self.servo_mode = [ServoMode()]*4 # Servo Mode self.servo_selected: int = 0 - #self._servo_modes = list(ServoMode) # Overall app state (controls what is displayed and what user inputs are accepted) self.current_state = STATE_INIT @@ -1109,7 +1143,7 @@ def _update_state_stepper(self, delta: int): self._stepper.speed(speed) else: if self.stepper_mode != StepperMode.POSITION: # Position Mode - self.stepper_mode = StepperMode.POSITION + self.stepper_mode.set(StepperMode.POSITION) self._stepper.speed(_STEPPER_DEFAULT_SPEED) self._stepper.enable(True) self._stepper.track_target() @@ -1127,7 +1161,7 @@ def _update_state_stepper(self, delta: int): self._stepper.speed(speed) else: # Position Mode if self.stepper_mode != StepperMode.POSITION: - self.stepper_mode = StepperMode.POSITION + self.stepper_mode.set(StepperMode.POSITION) self._stepper.speed(_STEPPER_DEFAULT_SPEED) self._stepper.enable(True) self._stepper.track_target() @@ -1146,8 +1180,7 @@ def _update_state_stepper(self, delta: int): return elif self.button_states.get(BUTTON_TYPES["CONFIRM"]): #Cycle Through Modes self.button_states.clear() - #self.stepper_mode = self._stepper_modes[(self._stepper_modes.index(self.stepper_mode) + 1) % len(self._stepper_modes)] - #self.stepper_mode = (self.stepper_mode + 1) % len(StepperMode) + self.stepper_mode.inc() if self.stepper_mode == StepperMode.POSITION: # Position Mode self._stepper.speed(_STEPPER_DEFAULT_SPEED) self._stepper.enable(True) @@ -1162,15 +1195,21 @@ def _update_state_stepper(self, delta: int): self._refresh = True self.notification = Notification(f" Stepper:\n {self.stepper_mode}") print(f"Stepper:{self.stepper_mode}") - if self.stepper_mode == StepperMode.SPEED: # Speed Mode - self._refresh = True + if self._refresh: + self._time_since_last_input = 0 + else: + self._time_since_last_input += delta + if self._time_since_last_input > self._timeout_period: + self._stepper.stop() + self.current_state = STATE_MENU + self.notification = Notification(" Stepper:\n Timeout") + elif self.stepper_mode == StepperMode.SPEED: # Speed Mode + self._refresh = True self._time_since_last_update += delta if self._time_since_last_update > self._keep_alive_period: self._stepper.step() self._time_since_last_update = 0 - _refresh = self._refresh - def _update_state_servo(self, delta: int): # Servo Tester: @@ -1204,7 +1243,7 @@ def _update_state_servo(self, delta: int): else: # Position Mode if self.servo[self.servo_selected] is None: self.servo[self.servo_selected] = 0 - self.servo_mode[self.servo_selected] = ServoMode.POSITION + self.servo_mode[self.servo_selected].set(ServoMode.POSITION) self.servo[self.servo_selected] += self._settings['servo_step'].v if self.servo[self.servo_selected] is not None: if self.servo_range[self.servo_selected] < (self.servo[self.servo_selected] + (self.servo_centre[self.servo_selected] - _SERVO_DEFAULT_CENTRE)): @@ -1238,7 +1277,7 @@ def _update_state_servo(self, delta: int): else: # Position Mode if self.servo[self.servo_selected] is None: self.servo[self.servo_selected] = 0 - self.servo_mode[self.servo_selected] = ServoMode.POSITION + self.servo_mode[self.servo_selected].set(ServoMode.POSITION) self.servo[self.servo_selected] -= self._settings['servo_step'].v if self.servo[self.servo_selected] is not None: if -self.servo_range[self.servo_selected] > (self.servo[self.servo_selected] + (self.servo_centre[self.servo_selected] - _SERVO_DEFAULT_CENTRE)): @@ -1264,8 +1303,7 @@ def _update_state_servo(self, delta: int): return elif self.button_states.get(BUTTON_TYPES["CONFIRM"]): #Cycle Through Modes self.button_states.clear() - #self.servo_mode[self.servo_selected] = self._servo_modes[(self._servo_modes.index(self.servo_mode[self.servo_selected]) + 1) % len(self._servo_modes)] - #self.servo_mode[self.servo_selected] = (self.servo_mode[self.servo_selected] + 1) % len(ServoMode) + self.servo_mode[self.servo_selected].inc() if self.servo_mode[self.servo_selected] == ServoMode.OFF: if self.hexdrive_app is not None: self.hexdrive_app.set_servoposition(self.servo_selected, None) @@ -1273,6 +1311,17 @@ def _update_state_servo(self, delta: int): self._refresh = True self.notification = Notification(f" Servo {self.servo_selected}:\n {self.servo_mode[self.servo_selected]}") + if self._refresh: + self._time_since_last_input = 0 + else: + self._time_since_last_input += delta + if self._time_since_last_input > self._timeout_period: + if self.hexdrive_app is not None: + self.hexdrive_app.set_power(False) + self.hexdrive_app.set_servoposition() # All Off + self.current_state = STATE_MENU + self.notification = Notification(" Servo:\n Timeout") + self._time_since_last_update += delta if self._time_since_last_update > self._keep_alive_period: self._time_since_last_update = 0 @@ -1467,7 +1516,7 @@ def _draw_state_stepper(self, ctx): ctx.rgb(*background_colour).rectangle(-100,1,200,label_font_size-2).fill() c = 0 # draw the stepper position - x = 100 * (self._stepper.get_pos() / _STEPPER_MAX_POSITION) + x = 100 * (self._stepper.get_pos() / self._settings['step_max_pos'].v) # vertical bar at servo position ctx.rgb(*bar_colour).rectangle(x-2,1,5,label_font_size-2).fill() # horizontal bar from 0 to servo position, not covering the centre marker or the servo position bar @@ -1615,6 +1664,7 @@ def reset_servo(self): # leave the servo modes as they are self.servo_selected = 0 self._time_since_last_update = 0 + self._time_since_last_input = 0 @@ -1684,7 +1734,7 @@ def _main_menu_select_handler(self, item: str, idx: int): # try timer IDs 0-3 until one is free for i in range(4): try: - self._stepper = Stepper(self.hexdrive_app, timer_id=i) # self._settings['stepper_steps'].v) + self._stepper = Stepper(self.hexdrive_app, timer_id=i, max_pos=self._settings['step_max_pos'].v) break except: pass @@ -1929,7 +1979,7 @@ def step(self,d=0): # we have reached the endstop self._hit_endstop() except Exception as e: - print(e) + print(f"step phase {self._phase} failed:{e}") def _hit_endstop(self): print("Endstop - hit") @@ -1937,7 +1987,7 @@ def _hit_endstop(self): self._calibrated = True # set this as the new zero position self.overwrite_pos(0) - + self.speed(0) def _timer_callback(self,t): if self._free_run_mode>0: @@ -1965,7 +2015,7 @@ def _update_timer(self,freq): self._freq = 0 self._timer_is_running=False except Exception as e: - print(e) + print(f"update_timer failed:{e}") if 0 != freq and freq != self._freq: try: print(f"Timer: {freq}Hz") @@ -1973,7 +2023,7 @@ def _update_timer(self,freq): self._freq = freq self._timer_is_running=True except Exception as e: - print(e) + print(f"update_timer failed:{e}") def stop(self): self._free_run_mode=0 @@ -1981,7 +2031,7 @@ def stop(self): try: self._hexdrive_app.motor_release() except Exception as e: - print(e) + print(f"stop failed:{e}") def enable(self,e = True): self._enabled=e @@ -1992,7 +2042,7 @@ def enable(self,e = True): self._hexdrive_app.motor_release() self._hexdrive_app.set_power(e) except Exception as e: - print(e) + print(f"enable failed:{e}") def is_enabled(self) -> bool: diff --git a/hexdrive.py b/hexdrive.py index f4d109e..270136a 100644 --- a/hexdrive.py +++ b/hexdrive.py @@ -131,7 +131,7 @@ async def _handle_stop_app(self, event): # Check keep alive period and turn off PWM outputs if exceeded - def background_update(self, delta): + def background_update(self, delta: int): if (self.config is None) or not (self._pwm_setup or self._stepper): return self._time_since_last_update += delta @@ -177,7 +177,7 @@ def get_logging(self) -> bool: # Turn the SMPPSU on or off # Just because the SPMSU is turned off does not mean that the outputs are NOT energised # as there could be external battery power - def set_power(self, state) -> bool: + def set_power(self, state: bool) -> bool: if (self.config is None) or (state == self._power_state): return False if self._logging: @@ -208,12 +208,12 @@ def get_booster_power(self) -> bool: return False - def set_keep_alive(self, period): + def set_keep_alive(self, period: int): self._keep_alive_period = period # Use 50 to 200 for Servos and 5000 to 20000 for motors - def set_freq(self, freq, channel=None) -> bool: + def set_freq(self, freq: int, channel: int | None = None) -> bool: if not self._pwm_setup: return False for this_channel, pwm in enumerate(self.PWMOutput): @@ -230,13 +230,13 @@ def set_freq(self, freq, channel=None) -> bool: # Get the current PWM frequency for a specific output - def get_freq(self, channel=0) -> int: + def get_freq(self, channel: int = 0) -> int: if not self._pwm_setup: return 0 if channel < 0 or channel >= 4: return 0 try: - f = self.PWMOutput[int(channel)].freq() + f = self.PWMOutput[channel].freq() except: f = 0 return f @@ -248,7 +248,7 @@ def get_freq(self, channel=0) -> int: # 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) - def set_servoposition(self, channel=None, position=None) -> bool: + def set_servoposition(self, channel: int | None = None, position: int | None = None) -> bool: if not self._pwm_setup: return False if position is None: @@ -267,11 +267,11 @@ def set_servoposition(self, channel=None, position=None) -> bool: elif channel < 0 or channel >= 4: return False try: - self.PWMOutput[int(channel)].duty_ns(0) + self.PWMOutput[channel].duty_ns(0) if self._logging: - print(f"H:{self.config.port}:PWM[{int(channel)}]:Off") + print(f"H:{self.config.port}:PWM[{channel}]:Off") except Exception as e: - print(f"H:{self.config.port}:PWM[{int(channel)}]:Off failed {e}") + print(f"H:{self.config.port}:PWM[{channel}]:Off failed {e}") return False # check if all channels are now off and set outputs_energised accordingly self._check_outputs_energised() @@ -283,10 +283,10 @@ def set_servoposition(self, channel=None, position=None) -> bool: self._outputs_energised = True self._stepper = False try: - if _MAX_SERVO_FREQ < self.PWMOutput[int(channel)].freq(): + if _MAX_SERVO_FREQ < self.PWMOutput[channel].freq(): # Ensure PWM frequency is suitable for use with Servos # otherwise the pulse width will not be accepted - self.PWMOutput[int(channel)].freq(_DEFAULT_SERVO_FREQ) + self.PWMOutput[channel].freq(_DEFAULT_SERVO_FREQ) if self._logging: print(f"H:{self.config.port}:PWM[{channel}]:{_DEFAULT_SERVO_FREQ}Hz for Servo") except Exception as e: @@ -295,12 +295,12 @@ def set_servoposition(self, channel=None, position=None) -> bool: # Scale servo position to PWM duty cycle (500-2500us) pulse_width = int((self._servo_centre[channel] + position) * 1000) try: - if pulse_width != self.PWMOutput[int(channel)].duty_ns(): - self.PWMOutput[int(channel)].duty_ns(pulse_width) + if pulse_width != self.PWMOutput[channel].duty_ns(): + self.PWMOutput[channel].duty_ns(pulse_width) if self._logging: - print(f"H:{self.config.port}:PWM[{int(channel)}]:{pulse_width//1000}us") + print(f"H:{self.config.port}:PWM[{channel}]:{pulse_width//1000}us") except Exception as e: - print(f"H:{self.config.port}:PWM[{int(channel)}]:set pwm failed {e}") + print(f"H:{self.config.port}:PWM[{channel}]:set pwm failed {e}") return False self._time_since_last_update = 0 return True @@ -310,7 +310,7 @@ def set_servoposition(self, channel=None, position=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 - def set_servocentre(self, centre, channel=None) -> bool: + def set_servocentre(self, centre: int, channel: int | None = None) -> bool: if not self._pwm_setup: return False if channel is not None and (channel < 0 or channel >= 4): @@ -318,9 +318,9 @@ def set_servocentre(self, centre, channel=None) -> bool: if centre < (_SERVO_CENTRE - _SERVO_MAX_TRIM ) or centre > (_SERVO_CENTRE + _SERVO_MAX_TRIM): return False if channel is None: - self._servo_centre = [int(centre)] * 4 + self._servo_centre = [centre] * 4 else: - self._servo_centre[int(channel)] = int(centre) + self._servo_centre[channel] = centre return True @@ -364,7 +364,7 @@ def set_pwm(self, duty_cycles) -> bool: # Get the current PWM duty cycle for a specific output (0-65535) - def get_pwm(self, channel=0) -> int: + def get_pwm(self, channel: int = 0) -> int: if not self._pwm_setup: return 0 if channel >= len(self.PWMOutput): @@ -377,8 +377,8 @@ def get_pwm(self, channel=0) -> int: ## Stepper Motor Support # Stepper Motor Support - def motor_step(self, phase) -> bool: - if phase > _STEPPER_NUM_PHASES: + def motor_step(self, phase: int) -> bool: + if phase >= _STEPPER_NUM_PHASES: return None if not self._stepper: # not currently configured for stepper motor - configure @@ -473,7 +473,7 @@ def _check_outputs_energised(self): # Set a single PWM duty cycle (0-65535) for a specific output - def _set_pwmoutput(self, channel, duty_cycle) -> bool: + def _set_pwmoutput(self, channel: int, duty_cycle: int) -> bool: if duty_cycle < 0 or duty_cycle > 65535: return False try: @@ -487,7 +487,7 @@ def _set_pwmoutput(self, channel, duty_cycle) -> bool: return True - def _check_port_for_hexdrive(self, port) -> int: + def _check_port_for_hexdrive(self, port: int) -> int: #just read the part of the header which contains the PID try: pid_bytes = self.config.i2c.readfrom_mem(_EEPROM_ADDR, _PID_ADDR, 2, addrsize = (8*_EEPROM_NUM_ADDRESS_BYTES)) @@ -531,7 +531,7 @@ def _parse_version(self, version): class HexDriveType: - def __init__(self, pid_byte, motors=0, servos=0, steppers=0, name="Unknown"): + def __init__(self, pid_byte: int , motors: int = 0, servos: int = 0, steppers: int = 0, name="Unknown"): self.pid_byte = pid_byte self._name = name self._motors = motors From 19700a2b42a2bc291acba4e3f981ace9f9e74907 Mon Sep 17 00:00:00 2001 From: robotmad Date: Tue, 20 Aug 2024 22:01:14 +0100 Subject: [PATCH 04/16] when scanning for hexdriveapp include check for name --- app.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/app.py b/app.py index 744af40..4169e60 100644 --- a/app.py +++ b/app.py @@ -612,8 +612,9 @@ def erase_eeprom(self, port: int, addr: int) -> bool: def find_hexdrive_app(self, port: int) -> app: for an_app in scheduler.apps: - if hasattr(an_app, "config") and hasattr(an_app.config, "port") and an_app.config.port == port: - return an_app + if type(an_app).__name__ is 'HexDriveApp': + if hasattr(an_app, "config") and hasattr(an_app.config, "port") and an_app.config.port == port: + return an_app return None From 0bae6e0ed71d78ca9de52fc95bb67cdf3dee395b Mon Sep 17 00:00:00 2001 From: robotmad Date: Tue, 20 Aug 2024 22:32:03 +0100 Subject: [PATCH 05/16] comments --- hexdrive.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/hexdrive.py b/hexdrive.py index 270136a..553f071 100644 --- a/hexdrive.py +++ b/hexdrive.py @@ -61,7 +61,7 @@ def __init__(self, config=None): # LS Pins self._power_detect = self.config.ls_pin[_DETECT_PIN] self._power_control = self.config.ls_pin[_ENABLE_PIN] - self._endstop = self.config.ls_pin[_ENDSTOP_PIN] # For Stepper Motor + self._endstop = self.config.ls_pin[_ENDSTOP_PIN] # For Stepper Motor mounted on linear rail with endstop switch self._servo_centre = [_SERVO_CENTRE] * 4 eventbus.on_async(RequestStopAppEvent, self._handle_stop_app, self) @@ -376,7 +376,7 @@ def get_pwm(self, channel: int = 0) -> int: return pwm ## Stepper Motor Support - # Stepper Motor Support + # Stepper Motor Support - force output to a specific phase def motor_step(self, phase: int) -> bool: if phase >= _STEPPER_NUM_PHASES: return None @@ -384,12 +384,12 @@ def motor_step(self, phase: int) -> bool: # not currently configured for stepper motor - configure self._pwm_deinit() self._stepper = True - # if we have reached the endstop then stop - # we are assuming that the endstop is active low for channel, value in enumerate(self._step[phase]): self.config.pin[channel].value(value) self._outputs_energised = True self._time_since_last_update = 0 + # check if we have reached the endstop + # we are assuming that the endstop is active low if not self._endstop.value(): return False return True From 2117982429b68ee493a9d977dbe6061531db67a0 Mon Sep 17 00:00:00 2001 From: robotmad Date: Tue, 20 Aug 2024 22:33:44 +0100 Subject: [PATCH 06/16] readme update --- README.md | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/README.md b/README.md index 7291c00..ae6deba 100644 --- a/README.md +++ b/README.md @@ -2,7 +2,7 @@ Companion app for HexDrive expansion, assuming BadgeBot configuration with 2 motors or 4 servos. -This guide is current for Badgebot version 1.2 +This guide is current for Badgebot version 1.3 As this application has become quite complicated if you are looking for example code to use a HexDrive please see [HexDriveUseTemplate](https://github.com/TeamRobotmad/HexDriveUseTemplate) @@ -10,19 +10,21 @@ As this application has become quite complicated if you are looking for example Install the BadgeBot app and then plug your HexDrive board into any of the hexpansion slots on your EMF Camp 2024 Badge. If your HexDrive EEPROM has not been initialised before you will be promted to confirm that the hexpansion is a HexDrive, if you have other hexpansions plugged in which have uninitialised EEPROMs then please be careful to only initialise the correct one as being a HexDrive. -If your HexDrive software (stored on the EEPROM on the hexpansion) is not the latest version then you will be prompted to update this. You can select from 4 'flavours' of configuration suitable for: +If your HexDrive software (stored on the EEPROM on the hexpansion) is not the latest version then you will be prompted to update this. You can select from 5 'flavours' of configuration suitable for: - 2 Motor - 4 Servo - 1 Motor and 2 Servos +- Stepper - Unknown +The board can drive 2 brushed DC motors, 4 RC servos, 1 DC motor and 2 servos or a single two phase Stepper Motor. 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. ### Main Menu ### -The main menu will present options for a demonstration for motors "Motor Moves" or a simple "Servo Test" function. +The main menu will present options for a demonstration for a 2 motor robot "Motor Moves", a test/demo a single Stepper Motor "Stepper Test", or a simple "Servo Test" function for up to 4 servos. ### Settings ### @@ -46,6 +48,7 @@ The main menu includes a sub-menu of Settings which can be adjusted. | brightness | LED brightness | 1.0 | 0.1 | 1.0 | | logging | Enable or disable logging | False | False | True | | erase_slot | Slot to offer erase function | 0 (i.e. none) | 0 | 6 | +| stepper_max_pos | Maximum stepper position | 6200 | 0 | 65535 | ### Limitations ### @@ -53,7 +56,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 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', 'Stepper' 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.) If you unplug a HexDrive the PWM resources will be released immediately so you can move them around the badge easily. @@ -116,15 +119,17 @@ Call ```set_pwm()``` to set the duty cycle of the 4 PWM channels which control t note the extra set of brackets as the function argument is a single tupple of 4 values rather than being 4 individual values. -To protect against most badge/software crashes causing the motors or servos to run out of control there is a keep alive mechanism which means that if you do not make a call to the ```set_pwm```, ```set_motors``` or ```set_servoposition``` functions the motors/servos will be turned off after 1000mS (default - which can be changed with a call to ```set_keep_alive()```). +To protect against most badge/software crashes causing the motors or servos to run out of control there is a keep alive mechanism which means that if you do not make a call to the ```set_pwm```, ```set_motors```, ```motor_step``` or ```set_servoposition``` functions the motors/servos will be turned off after 1000mS (default - which can be changed with a call to ```set_keep_alive()```). You can adjust the PWM frequency, default 20000Hz for motors and 50Hz for servos by calling the ```set_freq()``` function. ### Servos You can control 1,2,3 or 4 RC hobby servos (centre pulse width 1500us). The first time you set a pulse width for a channel using ```set_servo()``` the PWM frequency for that channel will be set to 50Hz. -The first two Channels take up signals that would otherwise control Motor 1 and teh second two Channels take up the signals that are used for Motor 2. +The first two Channels take up signals that would otherwise control Motor 1 and the second two Channels take up the signals that are used for Motor 2. You can use one motor and 1 or 2 servos simultaneously. +### Stepper Motor +You can control a single 2 phase stepper motor using ```motor_step()``` specifying which of the 8 possible phases to output in the range 0 to 7. There are 8 possible values as half stepping is supported. To use only full steps specify phase values of 0, 2, 4 and 6. Information on the pros and cons of using full or half stepping can be found online and what is right for you will depend on your motor and the application. The motor can be released (so that it is not takign power to hold it in a fixed position) using ```motor_release()```. ### Developers setup This is to help develop the BadgeBot application From 9aba8c5137d363365b68a387fc8ce0445407b6c6 Mon Sep 17 00:00:00 2001 From: robotmad Date: Tue, 3 Sep 2024 22:24:28 +0100 Subject: [PATCH 07/16] stepper motor working --- README.md | 2 +- app.py | 218 ++++++++++++++++++++++++++++++++---------------------- 2 files changed, 132 insertions(+), 88 deletions(-) diff --git a/README.md b/README.md index ae6deba..65b9444 100644 --- a/README.md +++ b/README.md @@ -129,7 +129,7 @@ The first two Channels take up signals that would otherwise control Motor 1 and You can use one motor and 1 or 2 servos simultaneously. ### Stepper Motor -You can control a single 2 phase stepper motor using ```motor_step()``` specifying which of the 8 possible phases to output in the range 0 to 7. There are 8 possible values as half stepping is supported. To use only full steps specify phase values of 0, 2, 4 and 6. Information on the pros and cons of using full or half stepping can be found online and what is right for you will depend on your motor and the application. The motor can be released (so that it is not takign power to hold it in a fixed position) using ```motor_release()```. +You can control a single 2 phase stepper motor using ```motor_step()``` specifying which of the 8 possible phases to output in the range 0 to 7. There are 8 possible values as half stepping is supported. To use only full steps specify phase values of 0, 2, 4 and 6. Information on the pros and cons of using full or half stepping can be found online and what is right for you will depend on your motor and the application. The motor can be released (so that it is not taking power to hold it in a fixed position) using ```motor_release()```. ### Developers setup This is to help develop the BadgeBot application diff --git a/app.py b/app.py index 4169e60..c44cf5d 100644 --- a/app.py +++ b/app.py @@ -84,12 +84,12 @@ _MAX_SERVO_RANGE = 1400 # 1400us either side of centre (VERY WIDE) # Stepper Tester - Defaults -_STEPPER_MAX_SPEED = 1000 # full steps per second -_STEPPER_MAX_POSITION = 6200 # half steps +_STEPPER_MAX_SPEED = 200 # full steps per second +_STEPPER_MAX_POSITION = 3100 # full steps from h/w endstop to s/w endstop at the other end _STEPPER_DEFAULT_SPEED = 50 # full steps per second _STEPPER_NUM_PHASES = 8 # half steps -_STEPPER_DEFAULT_SPR = 200 # steps per revolution - +_STEPPER_DEFAULT_SPR = 200 # full steps per revolution +_STEPPER_DEFAULT_STEP = 1 # half steps, (2 = full steps) # Timings _TICK_MS = 10 # Smallest unit of change for power, in ms @@ -211,13 +211,13 @@ def __init__(self): self.is_scroll: bool = False self.scroll_offset: int = 0 self.notification: Notification = None - self.error_message = [str] + self.error_message = [] self.current_menu: str = None self.menu: Menu = None # BadgeBot Control Sequence Variables self.run_countdown_elapsed_ms: int = 0 - self.instructions = [Instruction] + self.instructions = [] self.current_instruction: Instruction = None self.current_power_duration = ((0,0,0,0), 0) self.power_plan_iter = iter([]) @@ -281,7 +281,7 @@ def __init__(self): # Servo Tester self._time_since_last_input: int = 0 - self._timeout_period: int = 60000 # ms + self._timeout_period: int = 120000 # ms (2 minutes - without any user input) self._time_since_last_update: int = 0 self._keep_alive_period: int = 500 # ms (half the value used in hexdrive.py) self.num_servos: int = 4 # Default assumed for a single HexDrive @@ -289,13 +289,13 @@ def __init__(self): self.servo_centre = [_SERVO_DEFAULT_CENTRE]*4 # Trim Servo Centre self.servo_range = [_SERVO_DEFAULT_RANGE]*4 # Limit Servo Range self.servo_rate = [_SERVO_DEFAULT_RATE]*4 # Servo Rate of Change - self.servo_mode = [ServoMode()]*4 # Servo Mode + self.servo_mode = [ServoMode()]*4 # Servo Mode self.servo_selected: int = 0 # Overall app state (controls what is displayed and what user inputs are accepted) self.current_state = STATE_INIT self.previous_state = self.current_state - + self._update_period = 50 # ms eventbus.on_async(RequestForegroundPushEvent, self._gain_focus, self) eventbus.on_async(RequestForegroundPopEvent, self._lose_focus, self) @@ -367,8 +367,7 @@ async def background_task(self): cur_time = time.ticks_ms() delta_ticks = time.ticks_diff(cur_time, last_time) self.background_update(delta_ticks) - s = 10 if self.current_state == STATE_RUN else 50 - await asyncio.sleep_ms(s) + await asyncio.sleep_ms(self._update_period) last_time = cur_time @@ -376,9 +375,11 @@ async def background_task(self): def background_update(self, delta: int): if self.current_state == STATE_RUN: + # DC Motor Contorl output = self.get_current_power_level(delta) if output is None: self.current_state = STATE_DONE + self._update_period = 50 elif self.hexdrive_app is not None: self.hexdrive_app.set_motors(output) @@ -1115,6 +1116,7 @@ def _update_state_countdown(self, delta: int): if self.hexdrive_app is not None: self.hexdrive_app.set_power(True) self.current_state = STATE_RUN + self._update_period = 10 def _update_state_done(self, delta: int): @@ -1131,7 +1133,7 @@ def _update_state_done(self, delta: int): self.current_power_duration = ((0,0,0,0), 0) self.current_state = STATE_COUNTDOWN - # Servo Tester: + # Stepper Tester: def _update_state_stepper(self, delta: int): # Left/Right to adjust position if self.button_states.get(BUTTON_TYPES["RIGHT"]): @@ -1143,10 +1145,9 @@ def _update_state_stepper(self, delta: int): speed = _STEPPER_MAX_SPEED self._stepper.speed(speed) else: - if self.stepper_mode != StepperMode.POSITION: # Position Mode + if self.stepper_mode != StepperMode.POSITION: # Position Mode self.stepper_mode.set(StepperMode.POSITION) self._stepper.speed(_STEPPER_DEFAULT_SPEED) - self._stepper.enable(True) self._stepper.track_target() pos = self._stepper.get_pos() pos = self._inc(pos, self._auto_repeat_level+1) @@ -1160,11 +1161,10 @@ def _update_state_stepper(self, delta: int): if -_STEPPER_MAX_SPEED > speed: speed = -_STEPPER_MAX_SPEED self._stepper.speed(speed) - else: # Position Mode + else: # Position Mode if self.stepper_mode != StepperMode.POSITION: self.stepper_mode.set(StepperMode.POSITION) self._stepper.speed(_STEPPER_DEFAULT_SPEED) - self._stepper.enable(True) self._stepper.track_target() pos = self._stepper.get_pos() pos = self._dec(pos, self._auto_repeat_level+1) @@ -1176,7 +1176,7 @@ def _update_state_stepper(self, delta: int): if self.button_states.get(BUTTON_TYPES["CANCEL"]): self.button_states.clear() if self.hexdrive_app is not None: - self._stepper.stop() + self._stepper.enable(False) self.current_state = STATE_MENU return elif self.button_states.get(BUTTON_TYPES["CONFIRM"]): #Cycle Through Modes @@ -1184,15 +1184,13 @@ def _update_state_stepper(self, delta: int): self.stepper_mode.inc() if self.stepper_mode == StepperMode.POSITION: # Position Mode self._stepper.speed(_STEPPER_DEFAULT_SPEED) - self._stepper.enable(True) self._stepper.target(self._stepper.get_pos()) self._stepper.track_target() - elif self.stepper_mode == StepperMode.SPEED: # Speed Mode - self._stepper.enable(True) + elif self.stepper_mode == StepperMode.SPEED: # Speed Mode self._stepper.speed(0) self._stepper.free_run(1) - else: # Off - self._stepper.enable(False) + else: # Off + self._stepper.stop() self._refresh = True self.notification = Notification(f" Stepper:\n {self.stepper_mode}") print(f"Stepper:{self.stepper_mode}") @@ -1202,8 +1200,11 @@ def _update_state_stepper(self, delta: int): self._time_since_last_input += delta if self._time_since_last_input > self._timeout_period: self._stepper.stop() + self._stepper.speed(0) + self._stepper.enable(False) self.current_state = STATE_MENU - self.notification = Notification(" Stepper:\n Timeout") + self.notification = Notification(" Stepper:\n Timeout") + print("Stepper:Timeout") elif self.stepper_mode == StepperMode.SPEED: # Speed Mode self._refresh = True self._time_since_last_update += delta @@ -1516,11 +1517,11 @@ def _draw_state_stepper(self, ctx): background_colour = (0.15,0.15,0.15) ctx.rgb(*background_colour).rectangle(-100,1,200,label_font_size-2).fill() c = 0 - # draw the stepper position - x = 100 * (self._stepper.get_pos() / self._settings['step_max_pos'].v) - # vertical bar at servo position + # draw the stepper position (based on a centre halfway through the range) + x = 200 * (self._stepper.get_pos() / self._settings['step_max_pos'].v) - 100 + # vertical bar at stepper position ctx.rgb(*bar_colour).rectangle(x-2,1,5,label_font_size-2).fill() - # horizontal bar from 0 to servo position, not covering the centre marker or the servo position bar + # horizontal bar from 0 to stepper position, not covering the centre marker or the stepper position bar ctx.rgb(*body_colour) if x > (c+4): ctx.rectangle(c+1, 3, x-c-4, label_font_size-6).fill() @@ -1735,7 +1736,7 @@ def _main_menu_select_handler(self, item: str, idx: int): # try timer IDs 0-3 until one is free for i in range(4): try: - self._stepper = Stepper(self.hexdrive_app, timer_id=i, max_pos=self._settings['step_max_pos'].v) + self._stepper = Stepper(self, self.hexdrive_app, step_size=1, timer_id=i, max_pos=self._settings['step_max_pos'].v) break except: pass @@ -1744,9 +1745,11 @@ def _main_menu_select_handler(self, item: str, idx: int): else: self.set_menu(None) self.button_states.clear() - self.current_state = STATE_STEPPER + self.current_state = STATE_STEPPER self._refresh = True - self._auto_repeat_clear() + self._auto_repeat_clear() + self._stepper.enable(True) + self._time_since_last_input = 0 elif item == _main_menu_items[2]: # Servo Test if self.num_servos == 0: self.notification = Notification("No Servos") @@ -1895,24 +1898,36 @@ def finalize_instruction(self): ######## STEPPER MOTOR CLASS ######## class Stepper: - def __init__(self, hexdrive_app, steps_per_rev: int = _STEPPER_DEFAULT_SPR, speed_sps: int = _STEPPER_DEFAULT_SPEED, max_sps: int = _STEPPER_MAX_SPEED, max_pos: int = _STEPPER_MAX_POSITION, timer_id: int = 0): + def __init__(self, container, hexdrive_app, step_size: int = 1, steps_per_rev: int = _STEPPER_DEFAULT_SPR, speed_sps: int = _STEPPER_DEFAULT_SPEED, max_sps: int = _STEPPER_MAX_SPEED, max_pos: int = _STEPPER_MAX_POSITION, timer_id: int = 0): + self._container = container self._hexdrive_app = hexdrive_app self._phase = 0 self._calibrated = False - self._timer = Timer(timer_id) - self._timer_is_running=False - self._free_run_mode=0 # direction of free run mode - self._enabled=False + self._timer = Timer(timer_id) + self._timer_is_running = False + self._timer_mode = 0 + self._free_run_mode = 0 # direction of free run mode + self._enabled = False self._target_pos = 0 - self._pos = 0 - self._max_sps = int(max_sps) - self._steps_per_sec = int(speed_sps) + self._pos = 0 # current position in half steps + self._max_sps = int(max_sps) # max speed in full steps per second + self._steps_per_sec = int(speed_sps) # current speed in full steps per second self._steps_per_rev = int(steps_per_rev) # full steps per revolution - self._max_pos = int(max_pos) + self._max_pos = 2*int(max_pos) # max position stored in half steps self._freq = 0 + self._min_period = 0 + self._step_size = int(step_size) # 1 = half steps, 2 = full steps + self._last_step_time = 0 self.track_target() - def speed(self,sps): + def step_size(self,sz=1): + if sz < 1: + sz = 1 + elif sz > 2: + sz = 2 + self._step_size = int(sz) + + def speed(self,sps): # speed in FULL steps per second if self._free_run_mode == 1 and sps < 0: self._free_run_mode = -1 elif self._free_run_mode == -1 and sps > 0: @@ -1922,7 +1937,7 @@ def speed(self,sps): elif sps < -self._max_sps: sps = -self._max_sps self._steps_per_sec = int(sps) - self._update_timer(2*abs(self._steps_per_sec)) # half steps per second + self._update_timer((2//self._step_size)*abs(self._steps_per_sec)) # steps per second def speed_rps(self,rps): self.speed(rps*self._steps_per_rev) @@ -1931,56 +1946,68 @@ def get_speed(self) -> int: return self._steps_per_sec def target(self,t): - self._target_pos = int(t) + if self._calibrated and t < 0: + # when already calibrated limit to 0 + self._target_pos = 0 + elif self._calibrated and (2*int(t)) > self._max_pos: + # when already calibrated limit to max + self._target_pos = self._max_pos + else: + self._target_pos = 2*int(t) def target_deg(self,deg): - self.target(self._steps_per_rev*deg/180.0) # target pos is in half steps + self.target(self._steps_per_rev*deg/360.0) # target pos is in steps def target_rad(self,rad): - self.target(self._steps_per_rev*rad/pi) # target pos is in half steps + self.target(self._steps_per_rev*rad/(2*pi)) # target pos is in steps def get_pos(self) -> int: - return self._pos + return (self._pos//2) # convert half steps to full steps def get_pos_deg(self) -> float: - return self.get_pos()*180.0/self._steps_per_rev # half steps to degrees + return self._pos*180.0/self._steps_per_rev # half steps to degrees def get_pos_rad(self) -> float: - return self.get_pos()*pi/self._steps_per_rev # half steps to radians + return self._pos*pi/self._steps_per_rev # half steps to radians def overwrite_pos(self,p=0): - self._pos = int(p) + self._pos = 2*int(p) # convert full steps to half steps def overwrite_pos_deg(self,deg): - self.overwrite_pos(deg*self._steps_per_rev/180.0) # degrees to half steps + self._pos = deg*self._steps_per_rev/180.0 # degrees to half steps def overwrite_pos_rad(self,rad): - self.overwrite_pos(rad*self._steps_per_rev/pi) # radians to half steps + self._pos = rad*self._steps_per_rev/pi # radians to half steps def step(self,d=0): - if self._enabled: - if d>0: - self._pos+=1 - elif d<0: - self._pos-=1 - # Check position limits - if self._calibrated and self._pos < 0: - self._pos = 0 - return - elif self._calibrated and self._pos > self._max_pos: - self._pos = self._max_pos - return - elif d>0: - self._phase = (self._phase-1)%_STEPPER_NUM_PHASES - elif d<0: - self._phase = (self._phase+1)%_STEPPER_NUM_PHASES - #print(f"p:{self._pos}") - try: - if not self._hexdrive_app.motor_step(self._phase): - # we have reached the endstop - self._hit_endstop() - except Exception as e: - print(f"step phase {self._phase} failed:{e}") + cur_time = time.ticks_ms() + if time.ticks_diff(cur_time, self._last_step_time) < self._min_period: + # avoid stepping too quickly as this causes skipped steps + return + self._last_step_time = cur_time + if d>0: + self._pos+=self._step_size + self._phase = (self._phase-self._step_size)%_STEPPER_NUM_PHASES + elif d<0: + self._pos-=self._step_size + self._phase = (self._phase+self._step_size)%_STEPPER_NUM_PHASES + # Check position limits + if self._calibrated and self._pos < 0: + print("s/w min endstop") + self._pos = 0 + self.speed(0) + return + elif self._calibrated and self._pos > self._max_pos: + print("s/w max endstop") + self._pos = self._max_pos + self.speed(0) + return + try: + if not self._hexdrive_app.motor_step(self._phase): + # we have reached the endstop + self._hit_endstop() + except Exception as e: + print(f"step phase {self._phase} failed:{e}") def _hit_endstop(self): print("Endstop - hit") @@ -1988,26 +2015,32 @@ def _hit_endstop(self): self._calibrated = True # set this as the new zero position self.overwrite_pos(0) - self.speed(0) + # if we were moving towards the endstop, stop + if self._free_run_mode < 0: + self.speed(0) + elif self._free_run_mode == 0 and self._target_pos < self._pos: + self.speed(0) + + def _timer_callback_fwd(self,t): + self.step(1) + + def _timer_callback_rev(self,t): + self.step(-1) def _timer_callback(self,t): - if self._free_run_mode>0: - self.step(1) - elif self._free_run_mode<0: - self.step(-1) - elif self._target_pos>self._pos: + if self._target_pos>self._pos: self.step(1) elif self._target_pos0: + self._timer.init(freq=freq,callback=self._timer_callback_fwd) + elif self._free_run_mode<0: + self._timer.init(freq=freq,callback=self._timer_callback_rev) + else: + self._timer.init(freq=freq,callback=self._timer_callback) self._freq = freq + self._min_period = (1000//freq) - 1 self._timer_is_running=True + self._timer_mode = self._free_run_mode except Exception as e: - print(f"update_timer failed:{e}") + print(f"update_timer failed:{e}") + elif freq == 0: + print("Timer: 0Hz") + def stop(self): - self._free_run_mode=0 self._update_timer(0) try: self._hexdrive_app.motor_release() @@ -2038,14 +2080,16 @@ def enable(self,e = True): self._enabled=e try: if e: + if self._free_run_mode!=0: + self._update_timer((2//self._step_size)*abs(self._steps_per_sec)) # half steps per second self._hexdrive_app.motor_step(self._phase) else: + self._update_timer(0) self._hexdrive_app.motor_release() self._hexdrive_app.set_power(e) except Exception as e: print(f"enable failed:{e}") - def is_enabled(self) -> bool: return self._enabled From e87fe024d657338ca4f46bde7f00891035e64388 Mon Sep 17 00:00:00 2001 From: robotmad Date: Fri, 6 Sep 2024 20:26:59 +0100 Subject: [PATCH 08/16] consistent use of enumerate, fix bug in remembering when channel not in use due to motor direction. --- hexdrive.py | 96 ++++++++++++++++++++++++++++++----------------------- 1 file changed, 55 insertions(+), 41 deletions(-) diff --git a/hexdrive.py b/hexdrive.py index 553f071..ea74a4a 100644 --- a/hexdrive.py +++ b/hexdrive.py @@ -144,11 +144,12 @@ def background_update(self, delta: int): print(f"H:{self.config.port}:Timeout") if self._pwm_setup: for channel,pwm in enumerate(self.PWMOutput): - if self.PWMOutput[channel] is not None: + if pwm is not None: try: - self.PWMOutput[channel].duty_u16(0) + pwm.duty_u16(0) except Exception as e: - print(f"H:{self.config.port}:PWM[{pwm}]:Off failed {e}") + print(f"H:{self.config.port}:PWM[{channel}]:Off failed {e}") + self.PWMOutput[channel] = None # Tidy Up elif self._stepper: self.motor_release() @@ -217,7 +218,7 @@ def set_freq(self, freq: int, channel: int | None = None) -> bool: if not self._pwm_setup: return False for this_channel, pwm in enumerate(self.PWMOutput): - if channel is None or this_channel == channel: + if (channel is None or this_channel == channel) and pwm is not None: try: pwm.freq(int(freq)) if self._logging: @@ -255,24 +256,26 @@ def set_servoposition(self, channel: int | None = None, position: int | None = N # 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 channel in self.PWMOutput: - try: - channel.duty_ns(0) - except: - pass + for channel, pwm in enumerate(self.PWMOutput): + if pwm is not None: + try: + pwm.duty_ns(0) + except: + pass if self._logging: print(f"H:{self.config.port}:PWM:[All]:Off") self._outputs_energised = False return True elif channel < 0 or channel >= 4: return False - try: - self.PWMOutput[channel].duty_ns(0) - if self._logging: - print(f"H:{self.config.port}:PWM[{channel}]:Off") - except Exception as e: - print(f"H:{self.config.port}:PWM[{channel}]:Off failed {e}") - return False + else: + try: + self.PWMOutput[channel].duty_ns(0) + if self._logging: + print(f"H:{self.config.port}:PWM[{channel}]:Off") + except Exception as e: + print(f"H:{self.config.port}:PWM[{channel}]:Off failed {e}") + return False # check if all channels are now off and set outputs_energised accordingly self._check_outputs_energised() else: @@ -331,20 +334,31 @@ def set_motors(self, outputs) -> bool: for motor, output in enumerate(outputs): if abs(output) > 65535: return False - if output >= 0: - if 0 > self._motor_output[motor]: - # switch which signal is being driven as the PWM output - self.PWMOutput[(motor<<1)+1].deinit() - self.config.pin[(motor<<1)+1].value(0) - self.PWMOutput[(motor<<1)] = PWM(self.config.pin[(motor<<1)], freq = self._freq[(motor<<1)], duty_u16 = int(output)) - self._set_pwmoutput((motor<<1), int(output)) - else: - if 0 <= self._motor_output[motor]: - # switch which signal is being driven as the PWM output - self.PWMOutput[(motor<<1)].deinit() - self.config.pin[(motor<<1)].value(0) - self.PWMOutput[(motor<<1)+1] = PWM(self.config.pin[(motor<<1)+1], freq = self._freq[(motor<<1)], duty_u16 = -int(output)) - self._set_pwmoutput((motor<<1)+1, -int(output)) + try: + if output >= 0: + if 0 > self._motor_output[motor]: + # switch which signal is being driven as the PWM output + self.PWMOutput[(motor<<1)+1].deinit() + self.PWMOutput[(motor<<1)+1] = None + self.config.pin[(motor<<1)+1].value(0) + self.PWMOutput[(motor<<1)] = PWM(self.config.pin[(motor<<1)], freq = self._freq[(motor<<1)], duty_u16 = int(output)) + if self._logging: + print(f"H:{self.config.port}:<>PWM[{(motor<<1)}]:{output}") + else: + self._set_pwmoutput((motor<<1), int(output)) + else: + if 0 <= self._motor_output[motor]: + # switch which signal is being driven as the PWM output + self.PWMOutput[(motor<<1)].deinit() + self.PWMOutput[(motor<<1)] = None + self.config.pin[(motor<<1)].value(0) + self.PWMOutput[(motor<<1)+1] = PWM(self.config.pin[(motor<<1)+1], freq = self._freq[(motor<<1)], duty_u16 = -int(output)) + if self._logging: + print(f"H:{self.config.port}:<>PWM[{(motor<<1)+1}]:{-int(output)}") + else: + self._set_pwmoutput((motor<<1)+1, -int(output)) + except Exception as e: + print(f"H:{self.config.port}:Motor{motor}:{output} set failed {e}") self._motor_output[motor] = output self._check_outputs_energised() self._time_since_last_update = 0 @@ -443,10 +457,10 @@ def _pwm_init(self) -> bool: # De-initialise all PWM outputs def _pwm_deinit(self): - for channel, _ in enumerate(self.PWMOutput): - if self.PWMOutput[channel] is not None: + for channel, pwm in enumerate(self.PWMOutput): + if pwm is not None: try: - self.PWMOutput[channel].deinit() + pwm.deinit() except: pass self.PWMOutput[channel] = None @@ -458,14 +472,14 @@ def _pwm_deinit(self): # are any of the PWM outputs energised? def _check_outputs_energised(self): energised_output = False - for channel in self.PWMOutput: - try: - if 0 < channel.duty_ns(): - energised_output = True - break - except Exception as e: - print(f"H:{self.config.port}:PWM:Check failed {e}") - pass + 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: + print(f"H:{self.config.port}:PWM[{channel}]:Check failed {e}") if self._outputs_energised != energised_output: if self._logging: print(f"H:{self.config.port}:Outputs {'Energised' if energised_output else 'De-energised'}") From 21bdfc5191fb318e1a536bbc9b6860a26ec7d2c0 Mon Sep 17 00:00:00 2001 From: robotmad Date: Tue, 1 Oct 2024 21:40:51 +0100 Subject: [PATCH 09/16] drop the hard coded endstop pin --- hexdrive.py | 13 +- xystage.py | 1172 +++++++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 1174 insertions(+), 11 deletions(-) create mode 100644 xystage.py diff --git a/hexdrive.py b/hexdrive.py index ea74a4a..63abb30 100644 --- a/hexdrive.py +++ b/hexdrive.py @@ -11,12 +11,11 @@ import app # HexDrive.py App Version - used to check if upgrade is required -APP_VERSION = 5 +APP_VERSION = 6 _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 -_ENDSTOP_PIN = 4 # Endstop switch input pin _DEFAULT_PWM_FREQ = 20000 _DEFAULT_SERVO_FREQ = 50 # 20mS period @@ -61,7 +60,6 @@ def __init__(self, config=None): # LS Pins self._power_detect = self.config.ls_pin[_DETECT_PIN] self._power_control = self.config.ls_pin[_ENABLE_PIN] - self._endstop = self.config.ls_pin[_ENDSTOP_PIN] # For Stepper Motor mounted on linear rail with endstop switch self._servo_centre = [_SERVO_CENTRE] * 4 eventbus.on_async(RequestStopAppEvent, self._handle_stop_app, self) @@ -94,7 +92,6 @@ def initialise(self) -> bool: try: self._power_detect.init(mode=Pin.IN) self._power_control.init(mode=Pin.OUT) - self._endstop.init(mode=Pin.IN) # ideally want to configure this as an input with pullup and interrupt except Exception as e: print(f"H:{self.config.port}:ls_pin setup failed {e}") return False @@ -391,7 +388,7 @@ def get_pwm(self, channel: int = 0) -> int: ## Stepper Motor Support # Stepper Motor Support - force output to a specific phase - def motor_step(self, phase: int) -> bool: + def motor_step(self, phase: int): if phase >= _STEPPER_NUM_PHASES: return None if not self._stepper: @@ -402,12 +399,6 @@ def motor_step(self, phase: int) -> bool: self.config.pin[channel].value(value) self._outputs_energised = True self._time_since_last_update = 0 - # check if we have reached the endstop - # we are assuming that the endstop is active low - if not self._endstop.value(): - return False - return True - def motor_release(self): for channel in range(4): diff --git a/xystage.py b/xystage.py new file mode 100644 index 0000000..5ec147f --- /dev/null +++ b/xystage.py @@ -0,0 +1,1172 @@ +import asyncio +import aioble +import bluetooth +import os +import time +from math import cos, pi +import ota +import settings +import vfs +from app_components.notification import Notification +from app_components.tokens import label_font_size, twentyfour_pt, clear_background, button_labels +from app_components import Menu +from events.input import BUTTON_TYPES, Button, Buttons, ButtonUpEvent +from frontboards.twentyfour import BUTTONS +from machine import I2C, Timer, Pin +from system.eventbus import eventbus +from system.hexpansion.events import (HexpansionInsertionEvent, + HexpansionRemovalEvent) +from system.hexpansion.header import HexpansionHeader, write_header, read_header +from system.hexpansion.util import get_hexpansion_block_devices +from system.hexpansion.config import HexpansionConfig +from system.patterndisplay.events import PatternDisable, PatternEnable +from system.scheduler import scheduler +from system.scheduler.events import (RequestForegroundPopEvent, + RequestForegroundPushEvent, + RequestStopAppEvent) + +from tildagonos import tildagonos + +import app + +from .utils import chain, draw_logo_animated, parse_version + + + +# See the following for generating UUIDs: +# https://www.uuidgenerator.net/ +_BLE_SERVICE_UUID = bluetooth.UUID('19b10000-e8f2-537e-4f6c-d104768a1214') +_BLE_SENSOR_CHAR_UUID = bluetooth.UUID('19b10001-e8f2-537e-4f6c-d104768a1214') +_BLE_LED_UUID = bluetooth.UUID('19b10002-e8f2-537e-4f6c-d104768a1214') +# How frequently to send advertising beacons. +_ADV_INTERVAL_MS = 250_000 + + +# Hard coded to talk to EEPROMs on address 0x50 - because we know that is what is on the HexDrive Hexpansion +# makes it a lot more efficient than scanning the I2C bus for devices and working out what they are + +CURRENT_APP_VERSION = 5 # HEXDRIVE.PY Integer Version Number - checked against the EEPROM app.py version to determine if it needs updating + +_APP_VERSION = "1.0" # BadgeBot App Version Number + + +# Stepper Tester - Defaults +_STEPPER_MAX_SPEED = 200 # full steps per second +_STEPPER_MAX_POSITION = 3100 # full steps from h/w endstop to s/w endstop at the other end +_STEPPER_DEFAULT_SPEED = 50 # full steps per second +_STEPPER_NUM_PHASES = 8 # half steps +_STEPPER_DEFAULT_STEP = 1 # half steps, (2 = full steps) + +# Timings +_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 + + +# App states +STATE_INIT = -1 +STATE_WARNING = 0 +STATE_MENU = 1 +STATE_XYSTAGE = 2 +STATE_ERROR = 3 # Hexpansion error +STATE_MESSAGE = 4 # Message display +STATE_SETTINGS = 5 # Edit Settings + +# App states where user can minimise app +_MINIMISE_VALID_STATES = [0, 1, 3, 4, 5] + +# HexDrive Hexpansion constants +_EEPROM_ADDR = 0x50 +_EEPROM_NUM_ADDRESS_BYTES = 2 +_EEPROM_PAGE_SIZE = 32 +_EEPROM_TOTAL_SIZE = 64 * 1024 // 8 + +# HexDrive used for Y - Drive (autodetected) + +XYSTAGE_HEXPANSION = 1 # Hexpansion slot for XYStage - X Driver +# Dedicated Pins - to drive an external stepper driver +X_DIR = 0 # ls pin (LSA) +X_ENABLE = 1 # ls pin (LSB) - active low +Y_ENDSTOP = 0 # hs pin (HSF) - switch to ground +Y_STEP = 1 # hs pin (HSG) NOT USED +X_ENDSTOP = 2 # hs pin (HSH) - switch to ground +X_STEP = 3 # hs pin (HSI) + +_USABLE_X_PIXELS = 200 +_USABLE_Y_PIXELS = 150 +_WIDTH_DEFAULT = 2000 +_HEIGHT_DEFAULT = 3000 +_XRANGE_DEFAULT = 1940 # Driver configured for half steps +_YRANGE_DEFAULT = 3100 + + +#Misceallaneous Settings +_LOGGING = True + +# Menu Items +_main_menu_items = ["XYStage", "Settings", "About","Exit"] + +class XYStageApp(app.App): + def __init__(self): + super().__init__() + # UI Button Controls + self.button_states = Buttons(self) + self.last_press: Button = BUTTON_TYPES["CANCEL"] + self.long_press_delta: int = 0 + 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 + + # UI Feature Controls + self._refresh: bool = True + self.notification: Notification = None + self.error_message = [] + self.current_menu: str = None + self.menu: Menu = None + + # Settings + self._settings = {} + self._settings['logging'] = MySetting(self._settings, _LOGGING, False, True) + self._settings['width'] = MySetting(self._settings, _WIDTH_DEFAULT, 100, 10000) + self._settings['height'] = MySetting(self._settings, _HEIGHT_DEFAULT, 100, 10000) + self._settings['XRange'] = MySetting(self._settings, _XRANGE_DEFAULT, 100, 10000) + self._settings['YRange'] = MySetting(self._settings, _YRANGE_DEFAULT, 100, 10000) + + self._edit_setting: int = None + self._edit_setting_value = None + self.update_settings() + + # Check what version of the Badge s/w we are running on + try: + ver = parse_version(ota.get_version()) + if ver is not None: + if self._settings['logging'].v: + print(f"XYStage V{ver}") + # Potential to do things differently based on badge s/w version + # e.g. if ver < [1, 9, 0]: + except: + pass + + # Hexpansion related + self._HEXDRIVE_TYPES = [HexDriveType(0xCBCB, motors=2, servos=4), + HexDriveType(0xCBCA, motors=2, name="2 Motor"), + HexDriveType(0xCBCC, servos=4, name="4 Servo"), + HexDriveType(0xCBCD, motors=1, servos=2, name="1 Mot 2 Srvo"), + HexDriveType(0xCBCE, steppers=1, name="Stepper")] + self.hexpansion_slot_type = [None]*6 + self.hexdrive_port: int = None + self.ports_with_hexdrive = set() + self.hexdrive_app = None + eventbus.on_async(HexpansionInsertionEvent, self._handle_hexpansion_insertion, self) + eventbus.on_async(HexpansionRemovalEvent, self._handle_hexpansion_removal, self) + + # Motor Driver + self.num_steppers: int = 1 # Default assumed for a single HexDrive + self._stepperX: Stepper = None + self._stepperY: Stepper = None + self.xystage = {} + self.xystage['x'] = 0 + self.xystage['y'] = 0 + self._time_since_last_update: int = 0 + self._keep_alive_period: int = 500 # ms (half the value used in hexdrive.py) + self._timeout_period: int = 120000 # ms (2 minutes - without any user input) + + # Overall app state (controls what is displayed and what user inputs are accepted) + self.current_state = STATE_INIT + self.previous_state = self.current_state + print("XYStageApp:Init") + + + ### ASYNC EVENT HANDLERS ### + + async def _handle_hexpansion_removal(self, event: HexpansionRemovalEvent): + self.hexpansion_slot_type[event.port-1] = None + if event.port in self.ports_with_hexdrive: + self.ports_with_hexdrive.remove(event.port) + if event.port == self.hexdrive_port: + self.hexdrive_port = None + self.hexdrive_app = None + self.current_state = STATE_WARNING + self.notification = Notification("HexDrive Removed") + + async def _handle_hexpansion_insertion(self, event: HexpansionInsertionEvent): + if self.check_port_for_hexdrive(event.port): + pass + ### HEXPANSION FUNCTIONS ### + + # Scan the Hexpansion ports for EEPROMs and HexDrives in case they are already plugged in when we start + def scan_ports(self): + for port in range(1, 7): + self.check_port_for_hexdrive(port) + + + def check_port_for_hexdrive(self, port: int) -> bool: + # we know the EEPROM address so we can just read the header directly + if port not in range(1, 7): + return False + # We want to do this in two parts so that we detect if there is a valid EEPROM or not + try: + hexpansion_header = read_header(port, addr_len=_EEPROM_NUM_ADDRESS_BYTES) + except OSError: + # no EEPROM on this port + return False + except RuntimeError: + # not a valid header + if self._settings['logging'].v: + print(f"H:Found EEPROM on port {port}") + return True + # check is this is a HexDrive header by scanning the _HEXDRIVE_TYPES list + for index, hexpansion_type in enumerate(self._HEXDRIVE_TYPES): + if hexpansion_header.vid == hexpansion_type.vid and hexpansion_header.pid == hexpansion_type.pid: + if self._settings['logging'].v: + print(f"H:Found '{hexpansion_type.name}' HexDrive on port {port}") + if port not in self.ports_with_hexdrive: + self.ports_with_hexdrive.add(port) + self.hexpansion_slot_type[port-1] = index + return True + # we are not interested in this type of hexpansion + return False + + + def find_hexdrive_app(self, port: int) -> app: + for an_app in scheduler.apps: + if type(an_app).__name__ is 'HexDriveApp': + if hasattr(an_app, "config") and hasattr(an_app.config, "port") and an_app.config.port == port: + return an_app + return None + + + def update_settings(self): + for s in self._settings: + self._settings[s].v = settings.get(f"xystage.{s}", self._settings[s].d) + + + ### MAIN APP CONTROL FUNCTIONS ### + + def update(self, delta: int): + if self.notification: + self.notification.update(delta) + + if self.current_state == STATE_INIT: + # One Time initialisation + self.scan_ports() + if (len(self.ports_with_hexdrive) == 0): + # There are currently no possible HexDrives plugged in + self.current_state = STATE_WARNING + else: + # We have a HexDrive so we can start the main app + # remember which port it is on + self.hexdrive_port = list(self.ports_with_hexdrive)[0] + self.hexdrive_app = self.find_hexdrive_app(self.hexdrive_port) + self.current_state = STATE_MENU + + # manage PatternEnable/Disable for all states + self._update_main_application(delta) + + if self.current_state != self.previous_state: + if self._settings['logging'].v: + print(f"State: {self.previous_state} -> {self.current_state}") + self.previous_state = self.current_state + # something has changed - so worth redrawing + self._refresh = True + + + def _update_main_application(self, delta: int): + if self.current_state == STATE_MENU: + if self.current_menu is None: + self.set_menu("main") + self._refresh = True + else: + self.menu.update(delta) + if self.menu.is_animating != "none": + if self._settings['logging'].v: + print("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() + + ### XY Stage Application ### + elif self.current_state == STATE_XYSTAGE: + self._update_state_xystage(delta) + + ### Settings Capability ### + elif self.current_state == STATE_SETTINGS: + self._update_state_settings(delta) + ### End of Update ### + + # Stepper Tester: + def _update_state_xystage(self, delta: int): + # Left/Right to adjust position + pressed = False + if self.button_states.get(BUTTON_TYPES["RIGHT"]): + pressed = True + if self._auto_repeat_check(delta, True): + pos = self._stepperX.get_pos() + pos = self._inc(pos, self._auto_repeat_level + 1) + # Limit to the range of the stepper + if pos <= self._settings['XRange'].v: + self._stepperX.target(pos) + self._refresh = True + elif self.button_states.get(BUTTON_TYPES["LEFT"]): + pressed = True + if self._auto_repeat_check(delta, True): + pos = self._stepperX.get_pos() + pos = self._dec(pos, self._auto_repeat_level + 1) + # Limit to the range of the stepper + if pos >= 0: + self._stepperX.target(pos) + self._refresh = True + if self.button_states.get(BUTTON_TYPES["UP"]): + pressed = True + if self._auto_repeat_check(delta, True): + pos = self._stepperY.get_pos() + pos = self._inc(pos, self._auto_repeat_level + 1) + # Limit to the range of the stepper + if pos >= 0: + self._stepperY.target(pos) + self._refresh = True + elif self.button_states.get(BUTTON_TYPES["DOWN"]): + pressed = True + if self._auto_repeat_check(delta, True): + pos = self._stepperY.get_pos() + pos = self._dec(pos, self._auto_repeat_level + 1) + # Limit to the range of the stepper + if pos <= self._settings['YRange'].v: + self._stepperY.target(pos) + self._refresh = True + if pressed: + self._time_since_last_input = 0 + else: + self._auto_repeat_clear() + # non auto-repeating buttons + if self.button_states.get(BUTTON_TYPES["CANCEL"]): + self.button_states.clear() + if self.hexdrive_app is not None: + self._stepperX.enable(False) + self._stepperY.enable(False) + self.current_state = STATE_MENU + return + self._time_since_last_input += delta + if self._time_since_last_input > self._timeout_period: + self._stepperX.stop() + self._stepperX.speed(0) + self._stepperX.enable(False) + self._stepperY.stop() + self._stepperY.speed(0) + self._stepperY.enable(False) + self.current_state = STATE_MENU + self.notification = Notification(" Stepper:\n Timeout") + print("Stepper:Timeout") + + # if the stepperX or stepperY position has changed then update the display by setting refresh to True + if (self._stepperX.get_pos() - self._settings['XRange'].v//2) != self.xystage['x']: + self._refresh = True + self.xystage['x'] = self._stepperX.get_pos() - self._settings['XRange'].v//2 + if (self._stepperY.get_pos() - self._settings['YRange'].v//2) != self.xystage['y']: + self._refresh = True + self.xystage['y'] = self._stepperY.get_pos() - self._settings['YRange'].v//2 + if self._refresh: + print(f"X:{self.xystage['x']} Y:{self.xystage['y']}") + + # Check if we need to keep the stepper alive + self._time_since_last_update += delta + #if self._time_since_last_update > self._keep_alive_period: + # #self._stepperX.step() + # #self._stepperY.step() + # self._time_since_last_update = 0 + + def _update_state_settings(self, delta: int): + 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._settings['logging'].v: + print(f"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._settings['logging'].v: + print(f"Setting: {self._edit_setting} (-) Value: {self._edit_setting_value}") + self._refresh = True + else: + # non auto-repeating buttons + self._auto_repeat_clear() + if self.button_states.get(BUTTON_TYPES["RIGHT"]) or self.button_states.get(BUTTON_TYPES["LEFT"]): + self.button_states.clear() + # Force default value + self._edit_setting_value = self._settings[self._edit_setting].d + if self._settings['logging'].v: + print(f"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() + # leave setting unchanged + if self._settings['logging'].v: + print(f"Setting: {self._edit_setting} Cancelled") + self.set_menu(_main_menu_items[3]) + self.current_state = STATE_MENU + elif self.button_states.get(BUTTON_TYPES["CONFIRM"]): + self.button_states.clear() + # set setting + if self._settings['logging'].v: + print(f"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.set_menu(_main_menu_items[3]) + self.current_state = STATE_MENU + + + def draw(self, ctx): + if self._refresh or self.notification is not None: + self._refresh = False + clear_background(ctx) + ctx.save() + 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 + ctx.rgb(0,0,0).rectangle(-120,-120,240,240).fill() + # Main screen content + if self.current_state == STATE_WARNING: + self.draw_message(ctx, ["StepperXY requires","HexDrive hexpansion","from RobotMad","github.com","/TeamRobotmad","/BadgeBot"], [(1,1,1),(1,1,0),(1,1,0),(1,1,1),(1,1,1),(1,1,1)], label_font_size) + elif self.current_state == STATE_ERROR: + self.draw_message(ctx, self.error_message, [(1,0,0)]*len(self.error_message), label_font_size) + elif self.current_state == STATE_MESSAGE: + self.draw_message(ctx, self.error_message, [(0,1,0)]*len(self.error_message), label_font_size) + elif self.current_state == STATE_XYSTAGE: + self._draw_state_xystage(ctx) + elif self.current_state == STATE_SETTINGS: + self.draw_message(ctx, ["Edit Setting",f"{self._edit_setting}:",f"{self._edit_setting_value}"], [(1,1,1),(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") + ctx.restore() + + # These need to be drawn every frame as they contain animations + if self.current_state == STATE_MENU: + clear_background(ctx) + self.menu.draw(ctx) + + if self.notification: + self.notification.draw(ctx) + + def _draw_state_xystage(self, ctx): + # Draw outer rectangle for the XYStage based on the largest that can fit on the screen + # top left of the rectangle is at -100,-100 i.e. Y is inverted + ctx.rgb(0.3,0.3,0.3).rectangle(-_USABLE_X_PIXELS//2,-_USABLE_Y_PIXELS//2,_USABLE_X_PIXELS,_USABLE_Y_PIXELS).stroke() + x,y = self._scale_xystage(self.xystage['x']-(self._settings['width'].v//2),-self.xystage['y']-(self._settings['height'].v//2)) + sx,sy = self._scale_xystage(self._settings['width'].v,self._settings['height'].v) + ctx.rgb(0.0,1.0,0.5).rectangle(x,y,sx,sy).fill() + #stepper_text = "XYStage" + #stepper_text_colours[0] = (1,1,1) # Title - White + #self.draw_message(ctx, stepper_text, stepper_text_colours, label_font_size) + #button_labels(ctx, confirm_label="Stop", cancel_label="Exit", left_label="<--", right_label="-->") + + def _scale_xystage(self, x: int, y: int) -> (int, int): + # scale x,y to the canvas range: + # x,y are in the range -'XRange'/2 to 'XRange'/2 and -'YRange'/2 to 'YRange'/2 + x = int(_USABLE_X_PIXELS*x/(self._settings['XRange'].v + self._settings['width'].v)) + y = int(_USABLE_Y_PIXELS*y/(self._settings['YRange'].v + self._settings['height'].v)) + return x, y + + # Value increment/decrement functions for positive integers only + def _inc(self, v: int, l: int): + if l==0: + return v+1 + else: + d = 10**l + v = ((v // d) + 1) * d # round up to the next multiple of 10^l + return v + + def _dec(self, v: int, l: int): + if l==0: + return v-1 + else: + d = 10**l + v = (((v+(9*(10**(l-1)))) // d) - 1) * d # round down to the next multiple of 10^l + return v + + + def draw_message(self, ctx, message, colours, size=label_font_size): + 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) + +### MENU FUNCTIONALITY ### + + + def set_menu(self, menu_name = "main"): #: Literal["main"]): does it work without the type hint? + if self._settings['logging'].v: + print(f"H:Set Menu {menu_name}") + if self.menu is not None: + try: + self.menu._cleanup() + 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() + if self.num_steppers == 0: + menu_items.remove(_main_menu_items[0]) + self.menu = Menu( + self, + menu_items, + select_handler=self._main_menu_select_handler, + back_handler=self._menu_back_handler, + ) + elif menu_name == "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._settings['logging'].v: + print(f"H:Main Menu {item} at index {idx}") + if item == _main_menu_items[0]: # XYStage + if self.num_steppers == 0: + self.notification = Notification("No Steppers") + print("No Steppers") + else: + if self._stepperX is None or self._stepperY is None: + # try timer IDs 0-3 until one is free + for i in range(4): + if self._stepperX is None: + try: + # End stop - needs to be a HS pin to support interrupts + endstop_pin = HexpansionConfig(XYSTAGE_HEXPANSION).pin[X_ENDSTOP] + self._stepperX = Stepper(self, endstop = endstop_pin, name = "X", step_size=1, timer_id=i, max_pos=self._settings['XRange'].v) + print(f"StepperX:Init {i}") + # Start off assuming stage is in last known position + self._stepperX.overwrite_pos(self.xystage['x'] + self._settings['XRange'].v//2) + pos = self._stepperX.get_pos() + self._stepperX.target(pos) + continue + except: + print(f"StepperX:Init {i} Failed") + pass + elif self._stepperY is None: + try: + # End stop - needs to be a HS pin to support interrupts + endstop_pin = HexpansionConfig(XYSTAGE_HEXPANSION).pin[Y_ENDSTOP] + self._stepperY = StepperHex(self, self.hexdrive_app, endstop = endstop_pin, name = "Y", step_size=1, timer_id=i, max_pos=self._settings['YRange'].v) + print(f"StepperY:Init {i}") + # Start off assuming stage is in last known position + self._stepperY.overwrite_pos(self.xystage['y'] + self._settings['YRange'].v//2) + pos = self._stepperY.get_pos() + self._stepperY.target(pos) + continue + except: + print(f"StepperY:Init {i} Failed") + pass + else: + break + if self._stepperX is None or self._stepperY is None: + self.notification = Notification("No Free Timers") + else: + self.set_menu(None) + self.button_states.clear() + self.current_state = STATE_XYSTAGE + self._refresh = True + self._auto_repeat_clear() + self._stepperX.enable(True) + self._stepperX.speed(_STEPPER_DEFAULT_SPEED) + self._stepperY.enable(True) + self._stepperY.speed(_STEPPER_DEFAULT_SPEED) + self._time_since_last_input = 0 + elif item == _main_menu_items[1]: # Settings + self.set_menu(_main_menu_items[3]) + elif item == _main_menu_items[2]: # About + self.set_menu(None) + self.button_states.clear() + self.error_message = ["XYStage","Version: 1.0"] + self.current_state = STATE_MESSAGE + self._refresh = True + elif item == _main_menu_items[3]: # Exit + eventbus.remove(HexpansionInsertionEvent, self._handle_hexpansion_insertion, self) + eventbus.remove(HexpansionRemovalEvent, self._handle_hexpansion_removal, self) + eventbus.emit(RequestStopAppEvent(self)) + + def _settings_menu_select_handler(self, item: str, idx: int): + if self._settings['logging'].v: + print(f"H:Setting {item} @ {idx}") + if idx == 0: #Save + if self._settings['logging'].v: + print("H:Settings Save All") + settings.save() + self.notification = Notification(" Settings Saved") + self.set_menu("main") + elif idx == 1: #Default + if self._settings['logging'].v: + print("H: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("main") + else: + self.set_menu(None) + self.button_states.clear() + self.current_state = STATE_SETTINGS + self._refresh = True + self._auto_repeat_clear() + self._edit_setting = item + self._edit_setting_value = self._settings[item].v + + + def _menu_back_handler(self): + if self.current_menu == "main": + self.minimise() + # There are only two menus so this is the only other option + self.set_menu("main") + + +### BADGEBOT DEMO FUNCTIONALITY ### + + # multi level auto repeat + # if speed_up is True, the auto repeat gets faster the longer the button is held + # otherwise it is a fixed rate, but the level is used to determine the scale of the increase in the setttings inc() and dec() functions + def _auto_repeat_check(self, delta: int, speed_up: bool = True) -> bool: + 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._settings['logging'].v: + print(f"Auto Repeat Level: {self._auto_repeat_level}") + + return True + return False + + + def _auto_repeat_clear(self): + 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 + + + + + + + + + +######## STEPPER MOTOR CLASS ######## + +class Stepper: + def __init__(self, container, endstop: Pin = None, name: str = "", step_size: int = 1, speed_sps: int = _STEPPER_DEFAULT_SPEED, max_sps: int = _STEPPER_MAX_SPEED, max_pos: int = _STEPPER_MAX_POSITION, timer_id: int = 0): + self._container = container + self._name = name + self._calibrated = False + self._timer = Timer(timer_id) + self._timer_is_running = False + self._timer_mode = 0 + self._free_run_mode = 0 # direction of free run mode + self._enabled = False + self._target_pos = 0 + self._pos = 0 # current position in half steps + self._max_sps = int(max_sps) # max speed in full steps per second + self._steps_per_sec = int(speed_sps) # current speed in full steps per second + self._max_pos = 2*int(max_pos) # max position stored in half steps + self._freq = 0 + self._min_period = 0 + self._step_size = int(step_size) # 1 = half steps, 2 = full steps + self._last_step_time = 0 + self.track_target() + + # LS Pins for external stepper driver + self._hexpansion_config = HexpansionConfig(XYSTAGE_HEXPANSION) + self._pins = {} + # Direction Pin + self._pins["x_dir"] = self._hexpansion_config.ls_pin[X_DIR] + self._pins["x_dir"].init(mode=Pin.OUT) + self._pins["x_dir"].off() + # Enable Pin + self._pins["x_en"] = self._hexpansion_config.ls_pin[X_ENABLE] + self._pins["x_en"].init(mode=Pin.OUT) + self._pins["x_en"].on() + # Step Pin + self._pins["x_step"] = self._hexpansion_config.pin[X_STEP] + self._pins["x_step"].init(mode=Pin.OUT) + self._pins["x_step"].off() + # End stop - needs to be a HS pin to support interrupts + if endstop is not None: + endstop.init(mode=Pin.IN, pull=Pin.PULL_UP) + endstop.irq(trigger=Pin.IRQ_FALLING, handler=self._hit_endstop) + + + def step_size(self,sz=1): + if sz < 1: + sz = 1 + elif sz > 2: + sz = 2 + self._step_size = int(sz) + + def speed(self,sps): # speed in FULL steps per second + if self._free_run_mode == 1 and sps < 0: + self._free_run_mode = -1 + elif self._free_run_mode == -1 and sps > 0: + self._free_run_mode = 1 + if sps > self._max_sps: + sps = self._max_sps + elif sps < -self._max_sps: + sps = -self._max_sps + self._steps_per_sec = int(sps) + self._update_timer((2//self._step_size)*abs(self._steps_per_sec)) # steps per second + + def get_speed(self) -> int: + return self._steps_per_sec + + def target(self,t): + if self._calibrated and t < 0: + # when already calibrated limit to 0 + self._target_pos = 0 + elif self._calibrated and (2*int(t)) > self._max_pos: + # when already calibrated limit to max + self._target_pos = self._max_pos + else: + self._target_pos = 2*int(t) + + def get_pos(self) -> int: + return (self._pos//2) # convert half steps to full steps + + def overwrite_pos(self,p=0): + self._pos = 2*int(p) # convert full steps to half steps + + def step(self,d=0): + cur_time = time.ticks_ms() + if time.ticks_diff(cur_time, self._last_step_time) < self._min_period: + # avoid stepping too quickly as this causes skipped steps + return + self._last_step_time = cur_time + if d>0: + self._pins["x_en"].off() # active low enable + self._pins["x_dir"].off() + self._pos+=self._step_size + elif d<0: + self._pins["x_en"].off() # active low enable + self._pins["x_dir"].on() + self._pos-=self._step_size + # Check position limits + if self._calibrated and self._pos < 0: + print(f"{self._name} s/w min endstop") + self._pos = 0 + self.speed(0) + return + elif self._calibrated and self._pos > self._max_pos: + print(f"{self._name} s/w max endstop") + self._pos = self._max_pos + self.speed(0) + return + try: + self._pins["x_step"].on() + self._pins["x_step"].off() + except Exception as e: + print(f"{self._name} step failed:{e}") + + def _hit_endstop(self, pin: Pin): + # double check the endstop is hit + # if not, ignore the interrupt + if pin.value() == 0: + print(f"{self._name} Endstop - hit") + if not self._calibrated: + self._calibrated = True + # if we were moving towards the endstop, stop + if self._free_run_mode < 0: + self.speed(0) + # set this as the new zero position + self.overwrite_pos(0) + elif self._free_run_mode == 0 and self._target_pos < self._pos: + self.speed(0) + else: + print(f"{self._name} Endstop - false alarm") + + def _timer_callback_fwd(self,t): + self.step(1) + + def _timer_callback_rev(self,t): + self.step(-1) + + def _timer_callback(self,t): + if self._target_pos>self._pos: + self.step(1) + elif self._target_pos0: + self._timer.init(freq=freq,callback=self._timer_callback_fwd) + self._pins["x_dir"].off() + elif self._free_run_mode<0: + self._timer.init(freq=freq,callback=self._timer_callback_rev) + self._pins["x_dir"].on() + else: + self._timer.init(freq=freq,callback=self._timer_callback) + self._freq = freq + self._min_period = (1000//freq) - 1 + self._timer_is_running=True + self._timer_mode = self._free_run_mode + except Exception as e: + print(f"{self._name} update_timer failed:{e}") + elif freq == 0: + print(f"{self._name} Timer: 0Hz") + self._pins["x_en"].on() + + def stop(self): + self._update_timer(0) + + + def enable(self,e = True): + self._enabled=e + self._pins["x_en"].value(not e) + try: + if e: + if self._free_run_mode!=0: + self._update_timer((2//self._step_size)*abs(self._steps_per_sec)) # half steps per second + else: + self._update_timer(0) + except Exception as e: + print(f"{self._name} enable failed:{e}") + + def is_enabled(self) -> bool: + return self._enabled + +########## END OF STEPPER CLASS ########## + +class StepperHex: + def __init__(self, container, hexdrive_app, endstop: Pin = None, name: str = "", step_size: int = 1, speed_sps: int = _STEPPER_DEFAULT_SPEED, max_sps: int = _STEPPER_MAX_SPEED, max_pos: int = _STEPPER_MAX_POSITION, timer_id: int = 0): + self._container = container + self._name = name + self._hexdrive_app = hexdrive_app + self._phase = 0 + self._calibrated = False + self._timer = Timer(timer_id) + self._timer_is_running = False + self._timer_mode = 0 + self._free_run_mode = 0 # direction of free run mode + self._enabled = False + self._target_pos = 0 + self._pos = 0 # current position in half steps + self._max_sps = int(max_sps) # max speed in full steps per second + self._steps_per_sec = int(speed_sps) # current speed in full steps per second + self._max_pos = 2*int(max_pos) # max position stored in half steps + self._freq = 0 + self._min_period = 0 + self._step_size = int(step_size) # 1 = half steps, 2 = full steps + self._last_step_time = 0 + self.track_target() + + # End stop - needs to be a HS pin to support interrupts + if endstop is not None: + endstop.init(mode=Pin.IN, pull=Pin.PULL_UP) + endstop.irq(trigger=Pin.IRQ_FALLING, handler=self._hit_endstop) + + def step_size(self,sz=1): + if sz < 1: + sz = 1 + elif sz > 2: + sz = 2 + self._step_size = int(sz) + + def speed(self,sps): # speed in FULL steps per second + if self._free_run_mode == 1 and sps < 0: + self._free_run_mode = -1 + elif self._free_run_mode == -1 and sps > 0: + self._free_run_mode = 1 + if sps > self._max_sps: + sps = self._max_sps + elif sps < -self._max_sps: + sps = -self._max_sps + self._steps_per_sec = int(sps) + self._update_timer((2//self._step_size)*abs(self._steps_per_sec)) # steps per second + + def get_speed(self) -> int: + return self._steps_per_sec + + def target(self,t): + if self._calibrated and t < 0: + # when already calibrated limit to 0 + self._target_pos = 0 + elif self._calibrated and (2*int(t)) > self._max_pos: + # when already calibrated limit to max + self._target_pos = self._max_pos + else: + self._target_pos = 2*int(t) + + def get_pos(self) -> int: + return (self._pos//2) # convert half steps to full steps + + def overwrite_pos(self,p=0): + self._pos = 2*int(p) # convert full steps to half steps + + def step(self,d=0): + cur_time = time.ticks_ms() + if time.ticks_diff(cur_time, self._last_step_time) < self._min_period: + # avoid stepping too quickly as this causes skipped steps + return + self._last_step_time = cur_time + if d>0: + self._pos+=self._step_size + self._phase = (self._phase+self._step_size)%_STEPPER_NUM_PHASES + elif d<0: + self._pos-=self._step_size + self._phase = (self._phase-self._step_size)%_STEPPER_NUM_PHASES + # Check position limits + if self._calibrated and self._pos < 0: + print(f"{self._name} s/w min endstop") + self._pos = 0 + self.speed(0) + return + elif self._calibrated and self._pos > self._max_pos: + print(f"{self._name} s/w max endstop") + self._pos = self._max_pos + self.speed(0) + return + try: + #print(f"s{self._phase}") + self._hexdrive_app.motor_step(self._phase) + except Exception as e: + print(f"{self._name} step phase {self._phase} failed:{e}") + + + def _hit_endstop(self, pin: Pin): + # double check the endstop is hit + # if not, ignore the interrupt + if pin.value() == 0: + print(f"{self._name} Endstop - hit") + if not self._calibrated: + self._calibrated = True + # if we were moving towards the endstop, stop + if self._free_run_mode < 0: + self.speed(0) + # set this as the new zero position + self.overwrite_pos(0) + elif self._free_run_mode == 0 and self._target_pos < self._pos: + self.speed(0) + else: + print(f"{self._name} Endstop - false alarm") + + def _timer_callback_fwd(self,t): + self.step(1) + + def _timer_callback_rev(self,t): + self.step(-1) + + def _timer_callback(self,t): + if self._target_pos>self._pos: + self.step(1) + elif self._target_pos0: + self._timer.init(freq=freq,callback=self._timer_callback_fwd) + elif self._free_run_mode<0: + self._timer.init(freq=freq,callback=self._timer_callback_rev) + else: + self._timer.init(freq=freq,callback=self._timer_callback) + self._freq = freq + self._min_period = (1000//freq) - 1 + self._timer_is_running=True + self._timer_mode = self._free_run_mode + except Exception as e: + print(f"{self._name} update_timer failed:{e}") + elif freq == 0: + print(f"{self._name} Timer: 0Hz") + + def stop(self): + self._update_timer(0) + try: + self._hexdrive_app.motor_release() + except Exception as e: + print(f"{self._name} stop failed:{e}") + + def enable(self,e = True): + self._enabled=e + try: + if e: + if self._free_run_mode!=0: + self._update_timer((2//self._step_size)*abs(self._steps_per_sec)) # half steps per second + self._hexdrive_app.motor_step(self._phase) + else: + self._update_timer(0) + self._hexdrive_app.motor_release() + self._hexdrive_app.set_power(e) + except Exception as e: + print(f"{self._name} enable failed:{e}") + + def is_enabled(self) -> bool: + return self._enabled + + +class HexDriveType: + def __init__(self, pid, vid = 0xCAFE, motors = 0, steppers = 0, servos = 0, name ="Unknown"): + self.vid = vid + self.pid = pid + self.name = name + self.motors = motors + self.servos = servos + self.steppers = steppers + + +class MySetting: + def __init__(self, container, default, minimum, maximum): + self._container = container + self.d = default + self.v = default + self._min = minimum + self._max = maximum + + + def __str__(self): + return str(self.v) + + + def _index(self): + for k,v in self._container.items(): + if v == self: + return k + return None + + + # This returns an increase in the value passed in - subject to max and with scale of increase depending on level + # based on the type of the setting + # it does not affect the current value of the setting + def inc(self, v, l=0): + 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 # round up to the next multiple of 10^l, being very careful not to cause big jumps when value was nearly at the next multiple + + if v > self._max: + v = self._max + elif isinstance(self.v, float): + # only float at present is brightness from 0.0 to 1.0 + v += 0.1 + if v > self._max: + v = self._max + elif self._container['logging'].v: + print(f"H:inc type: {type(self.v)}") + return v + + # This returns a decrease in the value passed in - subject to min and with scale of increase depending on level + # based on the type of the setting + # it does not affect the current value of the setting + def dec(self, v, l=0): + 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 # round down to the next multiple of 10^l + + if v < self._min: + v = self._min + elif isinstance(self.v, float): + # only float at present is brightness from 0.0 to 1.0 + v -= 0.1 + if v < self._min: + v = self._min + elif self._container['logging'].v: + print(f"H: dec type: {type(self.v)}") + return v + + + def persist(self): + # only save non-default settings to the settings store + try: + if self.v != self.d: + settings.set(f"xystage.{self._index()}", self.v) + else: + settings.set(f"xystage.{self._index()}", None) + except Exception as e: + print(f"H:Failed to persist setting {self._index()}: {e}") + +__app_export__ = XYStageApp From f2e8ea50320197bce456fbb1e21d407021679fac Mon Sep 17 00:00:00 2001 From: robotmad Date: Wed, 2 Oct 2024 00:17:14 +0100 Subject: [PATCH 10/16] working xystage --- xystage.py | 237 +++++++++++++++++------------------------------------ 1 file changed, 77 insertions(+), 160 deletions(-) diff --git a/xystage.py b/xystage.py index 5ec147f..e26d9ec 100644 --- a/xystage.py +++ b/xystage.py @@ -12,18 +12,14 @@ from app_components import Menu from events.input import BUTTON_TYPES, Button, Buttons, ButtonUpEvent from frontboards.twentyfour import BUTTONS -from machine import I2C, Timer, Pin +from machine import PWM, Timer, Pin from system.eventbus import eventbus from system.hexpansion.events import (HexpansionInsertionEvent, HexpansionRemovalEvent) -from system.hexpansion.header import HexpansionHeader, write_header, read_header -from system.hexpansion.util import get_hexpansion_block_devices +from system.hexpansion.header import read_header from system.hexpansion.config import HexpansionConfig -from system.patterndisplay.events import PatternDisable, PatternEnable from system.scheduler import scheduler -from system.scheduler.events import (RequestForegroundPopEvent, - RequestForegroundPushEvent, - RequestStopAppEvent) +from system.scheduler.events import (RequestStopAppEvent) from tildagonos import tildagonos @@ -51,11 +47,11 @@ # Stepper Tester - Defaults -_STEPPER_MAX_SPEED = 200 # full steps per second +_STEPPER_MAX_SPEED = 5000 # full steps per second (X) +_STEPPER_MIN_SPEED = 200 # full steps per second (X) _STEPPER_MAX_POSITION = 3100 # full steps from h/w endstop to s/w endstop at the other end -_STEPPER_DEFAULT_SPEED = 50 # full steps per second +_STEPPER_DEFAULT_SPEED = 200 # full steps per second (Y) _STEPPER_NUM_PHASES = 8 # half steps -_STEPPER_DEFAULT_STEP = 1 # half steps, (2 = full steps) # Timings _AUTO_REPEAT_MS = 200 # Time between auto-repeats, in ms @@ -77,10 +73,8 @@ _MINIMISE_VALID_STATES = [0, 1, 3, 4, 5] # HexDrive Hexpansion constants -_EEPROM_ADDR = 0x50 _EEPROM_NUM_ADDRESS_BYTES = 2 -_EEPROM_PAGE_SIZE = 32 -_EEPROM_TOTAL_SIZE = 64 * 1024 // 8 + # HexDrive used for Y - Drive (autodetected) @@ -95,9 +89,9 @@ _USABLE_X_PIXELS = 200 _USABLE_Y_PIXELS = 150 -_WIDTH_DEFAULT = 2000 +_WIDTH_DEFAULT = (1000*32) _HEIGHT_DEFAULT = 3000 -_XRANGE_DEFAULT = 1940 # Driver configured for half steps +_XRANGE_DEFAULT = (970*32) # Driver configured for 1/32 steps _YRANGE_DEFAULT = 3100 @@ -129,10 +123,12 @@ def __init__(self): # Settings self._settings = {} self._settings['logging'] = MySetting(self._settings, _LOGGING, False, True) - self._settings['width'] = MySetting(self._settings, _WIDTH_DEFAULT, 100, 10000) - self._settings['height'] = MySetting(self._settings, _HEIGHT_DEFAULT, 100, 10000) - self._settings['XRange'] = MySetting(self._settings, _XRANGE_DEFAULT, 100, 10000) - self._settings['YRange'] = MySetting(self._settings, _YRANGE_DEFAULT, 100, 10000) + self._settings['width'] = MySetting(self._settings, _WIDTH_DEFAULT, 10, 100000) + self._settings['height'] = MySetting(self._settings, _HEIGHT_DEFAULT, 10, 100000) + self._settings['XRange'] = MySetting(self._settings, _XRANGE_DEFAULT, 10, 100000) + self._settings['YRange'] = MySetting(self._settings, _YRANGE_DEFAULT, 10, 100000) + self._settings['min_speed'] = MySetting(self._settings, _STEPPER_MIN_SPEED, 10, 10000) + self._settings['max_speed'] = MySetting(self._settings, _STEPPER_MAX_SPEED, 10, 100000) self._edit_setting: int = None self._edit_setting_value = None @@ -262,7 +258,6 @@ def update(self, delta: int): self.hexdrive_app = self.find_hexdrive_app(self.hexdrive_port) self.current_state = STATE_MENU - # manage PatternEnable/Disable for all states self._update_main_application(delta) if self.current_state != self.previous_state: @@ -299,26 +294,24 @@ def _update_main_application(self, delta: int): # Stepper Tester: def _update_state_xystage(self, delta: int): + self.xystage['x'] = self._stepperX.get_pos(delta) - self._settings['XRange'].v//2 # Left/Right to adjust position pressed = False if self.button_states.get(BUTTON_TYPES["RIGHT"]): pressed = True - if self._auto_repeat_check(delta, True): - pos = self._stepperX.get_pos() - pos = self._inc(pos, self._auto_repeat_level + 1) - # Limit to the range of the stepper - if pos <= self._settings['XRange'].v: - self._stepperX.target(pos) - self._refresh = True + if self._auto_repeat_check(delta, False): + speed = abs(self._stepperX.get_speed()) + # estimate the amount of movement based on the speed and time since last update + speed = max(self._settings['min_speed'].v, self._inc(speed, 1 + self._auto_repeat_level)) + self._stepperX.speed(speed) + self._refresh = True elif self.button_states.get(BUTTON_TYPES["LEFT"]): pressed = True - if self._auto_repeat_check(delta, True): - pos = self._stepperX.get_pos() - pos = self._dec(pos, self._auto_repeat_level + 1) - # Limit to the range of the stepper - if pos >= 0: - self._stepperX.target(pos) - self._refresh = True + if self._auto_repeat_check(delta, False): + speed = abs(self._stepperX.get_speed()) + speed = max(self._settings['min_speed'].v, self._inc(speed, 1 + self._auto_repeat_level)) + self._stepperX.speed(-speed) + self._refresh = True if self.button_states.get(BUTTON_TYPES["UP"]): pressed = True if self._auto_repeat_check(delta, True): @@ -341,6 +334,7 @@ def _update_state_xystage(self, delta: int): self._time_since_last_input = 0 else: self._auto_repeat_clear() + self._stepperX.speed(0) # non auto-repeating buttons if self.button_states.get(BUTTON_TYPES["CANCEL"]): self.button_states.clear() @@ -362,9 +356,6 @@ def _update_state_xystage(self, delta: int): print("Stepper:Timeout") # if the stepperX or stepperY position has changed then update the display by setting refresh to True - if (self._stepperX.get_pos() - self._settings['XRange'].v//2) != self.xystage['x']: - self._refresh = True - self.xystage['x'] = self._stepperX.get_pos() - self._settings['XRange'].v//2 if (self._stepperY.get_pos() - self._settings['YRange'].v//2) != self.xystage['y']: self._refresh = True self.xystage['y'] = self._stepperY.get_pos() - self._settings['YRange'].v//2 @@ -564,12 +555,8 @@ def _main_menu_select_handler(self, item: str, idx: int): try: # End stop - needs to be a HS pin to support interrupts endstop_pin = HexpansionConfig(XYSTAGE_HEXPANSION).pin[X_ENDSTOP] - self._stepperX = Stepper(self, endstop = endstop_pin, name = "X", step_size=1, timer_id=i, max_pos=self._settings['XRange'].v) + self._stepperX = Stepper(self, endstop = endstop_pin, name = "X", max_sps = self._settings['max_speed'].v, max_pos=self._settings['XRange'].v) print(f"StepperX:Init {i}") - # Start off assuming stage is in last known position - self._stepperX.overwrite_pos(self.xystage['x'] + self._settings['XRange'].v//2) - pos = self._stepperX.get_pos() - self._stepperX.target(pos) continue except: print(f"StepperX:Init {i} Failed") @@ -599,7 +586,7 @@ def _main_menu_select_handler(self, item: str, idx: int): self._refresh = True self._auto_repeat_clear() self._stepperX.enable(True) - self._stepperX.speed(_STEPPER_DEFAULT_SPEED) + #self._stepperX.speed(_STEPPER_DEFAULT_SPEED) self._stepperY.enable(True) self._stepperY.speed(_STEPPER_DEFAULT_SPEED) self._time_since_last_input = 0 @@ -690,55 +677,47 @@ def _auto_repeat_clear(self): ######## STEPPER MOTOR CLASS ######## -class Stepper: - def __init__(self, container, endstop: Pin = None, name: str = "", step_size: int = 1, speed_sps: int = _STEPPER_DEFAULT_SPEED, max_sps: int = _STEPPER_MAX_SPEED, max_pos: int = _STEPPER_MAX_POSITION, timer_id: int = 0): +class Stepper: # External Driver, Speed Control Only via PWM + def __init__(self, container, endstop: Pin = None, name: str = "", speed_sps: int = _STEPPER_DEFAULT_SPEED, max_sps: int = _STEPPER_MAX_SPEED, max_pos: int = _STEPPER_MAX_POSITION): self._container = container self._name = name self._calibrated = False - self._timer = Timer(timer_id) + #self._timer = Timer(timer_id) self._timer_is_running = False self._timer_mode = 0 - self._free_run_mode = 0 # direction of free run mode + self._pos = 0 # current position in steps + self._free_run_mode = 1 # direction of free run mode self._enabled = False - self._target_pos = 0 - self._pos = 0 # current position in half steps - self._max_sps = int(max_sps) # max speed in full steps per second - self._steps_per_sec = int(speed_sps) # current speed in full steps per second - self._max_pos = 2*int(max_pos) # max position stored in half steps + self._max_sps = int(max_sps) # max speed in steps per second + self._steps_per_sec = int(speed_sps) # current speed in steps per second + self._max_pos = int(max_pos) # max position stored in half steps self._freq = 0 - self._min_period = 0 - self._step_size = int(step_size) # 1 = half steps, 2 = full steps - self._last_step_time = 0 - self.track_target() - # LS Pins for external stepper driver + # Pins for external stepper driver self._hexpansion_config = HexpansionConfig(XYSTAGE_HEXPANSION) self._pins = {} # Direction Pin self._pins["x_dir"] = self._hexpansion_config.ls_pin[X_DIR] self._pins["x_dir"].init(mode=Pin.OUT) self._pins["x_dir"].off() - # Enable Pin + # Enable Pin - active low self._pins["x_en"] = self._hexpansion_config.ls_pin[X_ENABLE] self._pins["x_en"].init(mode=Pin.OUT) self._pins["x_en"].on() - # Step Pin + # Step Pin - needs to be a HS pin to support PWM self._pins["x_step"] = self._hexpansion_config.pin[X_STEP] - self._pins["x_step"].init(mode=Pin.OUT) - self._pins["x_step"].off() + #self._pins["x_step"].init(mode=Pin.OUT) + #self._pins["x_step"].off() + # Setup PWM output on the step pin + try: + self._pwm = PWM(self._pins["x_step"], freq=10, duty_ns=2000) # 0 Hz is invalid but 0 duty is allowed + except Exception as e: + print(f"{self._name} PWM failed:{e}") # End stop - needs to be a HS pin to support interrupts if endstop is not None: endstop.init(mode=Pin.IN, pull=Pin.PULL_UP) endstop.irq(trigger=Pin.IRQ_FALLING, handler=self._hit_endstop) - - def step_size(self,sz=1): - if sz < 1: - sz = 1 - elif sz > 2: - sz = 2 - self._step_size = int(sz) - def speed(self,sps): # speed in FULL steps per second if self._free_run_mode == 1 and sps < 0: self._free_run_mode = -1 @@ -749,58 +728,26 @@ def speed(self,sps): # speed in FULL steps per second elif sps < -self._max_sps: sps = -self._max_sps self._steps_per_sec = int(sps) - self._update_timer((2//self._step_size)*abs(self._steps_per_sec)) # steps per second + self._update_timer(abs(self._steps_per_sec)) # steps per second def get_speed(self) -> int: return self._steps_per_sec - def target(self,t): - if self._calibrated and t < 0: - # when already calibrated limit to 0 - self._target_pos = 0 - elif self._calibrated and (2*int(t)) > self._max_pos: - # when already calibrated limit to max - self._target_pos = self._max_pos - else: - self._target_pos = 2*int(t) - - def get_pos(self) -> int: - return (self._pos//2) # convert half steps to full steps - - def overwrite_pos(self,p=0): - self._pos = 2*int(p) # convert full steps to half steps - - def step(self,d=0): - cur_time = time.ticks_ms() - if time.ticks_diff(cur_time, self._last_step_time) < self._min_period: - # avoid stepping too quickly as this causes skipped steps - return - self._last_step_time = cur_time - if d>0: - self._pins["x_en"].off() # active low enable - self._pins["x_dir"].off() - self._pos+=self._step_size - elif d<0: - self._pins["x_en"].off() # active low enable - self._pins["x_dir"].on() - self._pos-=self._step_size - # Check position limits + # function to estimate the current position based on the speed and time since last update + def get_pos(self, delta) -> int: + steps = (self._steps_per_sec * delta) // 1000 + self._pos += steps + # Check if we have hit the end stop if self._calibrated and self._pos < 0: print(f"{self._name} s/w min endstop") - self._pos = 0 + #self._pos = 0 self.speed(0) - return elif self._calibrated and self._pos > self._max_pos: print(f"{self._name} s/w max endstop") - self._pos = self._max_pos - self.speed(0) - return - try: - self._pins["x_step"].on() - self._pins["x_step"].off() - except Exception as e: - print(f"{self._name} step failed:{e}") - + #self._pos = self._max_pos + self.speed(0) + return self._pos + def _hit_endstop(self, pin: Pin): # double check the endstop is hit # if not, ignore the interrupt @@ -808,80 +755,50 @@ def _hit_endstop(self, pin: Pin): print(f"{self._name} Endstop - hit") if not self._calibrated: self._calibrated = True - # if we were moving towards the endstop, stop - if self._free_run_mode < 0: - self.speed(0) - # set this as the new zero position - self.overwrite_pos(0) - elif self._free_run_mode == 0 and self._target_pos < self._pos: - self.speed(0) + self._pos = 0 + self.speed(0) else: print(f"{self._name} Endstop - false alarm") - def _timer_callback_fwd(self,t): - self.step(1) - - def _timer_callback_rev(self,t): - self.step(-1) - - def _timer_callback(self,t): - if self._target_pos>self._pos: - self.step(1) - elif self._target_pos0: - self._timer.init(freq=freq,callback=self._timer_callback_fwd) self._pins["x_dir"].off() + self._pwm.freq(int(freq)) + self._pwm.duty_ns(12000) # minimum 1.9uS STEP pulse width for DRV8825 + self._pins["x_en"].off() # enable active low elif self._free_run_mode<0: - self._timer.init(freq=freq,callback=self._timer_callback_rev) self._pins["x_dir"].on() + self._pwm.freq(int(freq)) + self._pwm.duty_ns(12000) # minimum 1.9uS STEP pulse width for DRV8825 + self._pins["x_en"].off() # enable active low else: - self._timer.init(freq=freq,callback=self._timer_callback) + self._pins["x_en"].on() + self._pwm.duty_ns(0) # stop the PWM (frequency of 0 is not allowed) self._freq = freq - self._min_period = (1000//freq) - 1 self._timer_is_running=True self._timer_mode = self._free_run_mode except Exception as e: print(f"{self._name} update_timer failed:{e}") - elif freq == 0: - print(f"{self._name} Timer: 0Hz") - self._pins["x_en"].on() + def stop(self): self._update_timer(0) - def enable(self,e = True): self._enabled=e self._pins["x_en"].value(not e) try: if e: if self._free_run_mode!=0: - self._update_timer((2//self._step_size)*abs(self._steps_per_sec)) # half steps per second + self._update_timer(abs(self._steps_per_sec)) # steps per second else: self._update_timer(0) except Exception as e: From 27eb4b5c9f00fd160558b6ce3af03777dcfa192e Mon Sep 17 00:00:00 2001 From: robotmad Date: Fri, 4 Oct 2024 23:30:00 +0100 Subject: [PATCH 11/16] workign XYStage with two DRV8825 drivers --- app.py | 34 +++- xystage.py | 527 +++++++++++++++++++---------------------------------- 2 files changed, 218 insertions(+), 343 deletions(-) diff --git a/app.py b/app.py index c44cf5d..e23908c 100644 --- a/app.py +++ b/app.py @@ -1,4 +1,6 @@ import asyncio +import aioble +import bluetooth import os import time from math import cos, pi @@ -28,12 +30,23 @@ from .utils import chain, draw_logo_animated, parse_version + + +# See the following for generating UUIDs: +# https://www.uuidgenerator.net/ +_BLE_SERVICE_UUID = bluetooth.UUID('19b10000-e8f2-537e-4f6c-d104768a1214') +_BLE_SENSOR_CHAR_UUID = bluetooth.UUID('19b10001-e8f2-537e-4f6c-d104768a1214') +_BLE_LED_UUID = bluetooth.UUID('19b10002-e8f2-537e-4f6c-d104768a1214') +# How frequently to send advertising beacons. +_ADV_INTERVAL_MS = 250_000 + + # Hard coded to talk to EEPROMs on address 0x50 - because we know that is what is on the HexDrive Hexpansion # makes it a lot more efficient than scanning the I2C bus for devices and working out what they are -CURRENT_APP_VERSION = 5 # HEXDRIVE.PY Integer Version Number - checked against the EEPROM app.py version to determine if it needs updating +CURRENT_APP_VERSION = 6 # HEXDRIVE.PY Integer Version Number - checked against the EEPROM app.py version to determine if it needs updating -_APP_VERSION = "1.3" # BadgeBot App Version Number +_APP_VERSION = "1.4" # BadgeBot App Version Number # If you change the URL then you will need to regenerate the QR code _QR_CODE = [0x1fcf67f, @@ -135,6 +148,11 @@ _EEPROM_PAGE_SIZE = 32 _EEPROM_TOTAL_SIZE = 64 * 1024 // 8 +X_DIR = 0 +X_STEP = 1 +Y_DIR = 2 +Y_STEP = 3 + #Misceallaneous Settings _LOGGING = False @@ -300,6 +318,14 @@ def __init__(self): eventbus.on_async(RequestForegroundPushEvent, self._gain_focus, self) eventbus.on_async(RequestForegroundPopEvent, self._lose_focus, self) + ### Bluetooth ### + # Register GATT server, the service and characteristics + self.ble_service = aioble.Service(_BLE_SERVICE_UUID) + self.sensor_characteristic = aioble.Characteristic(self.ble_service, _BLE_SENSOR_CHAR_UUID, read=True, notify=True) + self.led_characteristic = aioble.Characteristic(self.ble_service, _BLE_LED_UUID, read=True, write=True, notify=True, capture=True) + # Register service(s) + #aioble.register_services(self.ble_service) + # We start with focus on launch, without an event emmited self._gain_focus(RequestForegroundPushEvent(self)) @@ -2003,9 +2029,7 @@ def step(self,d=0): self.speed(0) return try: - if not self._hexdrive_app.motor_step(self._phase): - # we have reached the endstop - self._hit_endstop() + self._hexdrive_app.motor_step(self._phase) except Exception as e: print(f"step phase {self._phase} failed:{e}") diff --git a/xystage.py b/xystage.py index e26d9ec..5865f31 100644 --- a/xystage.py +++ b/xystage.py @@ -29,15 +29,6 @@ -# See the following for generating UUIDs: -# https://www.uuidgenerator.net/ -_BLE_SERVICE_UUID = bluetooth.UUID('19b10000-e8f2-537e-4f6c-d104768a1214') -_BLE_SENSOR_CHAR_UUID = bluetooth.UUID('19b10001-e8f2-537e-4f6c-d104768a1214') -_BLE_LED_UUID = bluetooth.UUID('19b10002-e8f2-537e-4f6c-d104768a1214') -# How frequently to send advertising beacons. -_ADV_INTERVAL_MS = 250_000 - - # Hard coded to talk to EEPROMs on address 0x50 - because we know that is what is on the HexDrive Hexpansion # makes it a lot more efficient than scanning the I2C bus for devices and working out what they are @@ -47,11 +38,9 @@ # Stepper Tester - Defaults -_STEPPER_MAX_SPEED = 5000 # full steps per second (X) -_STEPPER_MIN_SPEED = 200 # full steps per second (X) +_STEPPER_MAX_SPEED = 1100*32 # full steps per second +_STEPPER_MIN_SPEED = 20*32 # full steps per second _STEPPER_MAX_POSITION = 3100 # full steps from h/w endstop to s/w endstop at the other end -_STEPPER_DEFAULT_SPEED = 200 # full steps per second (Y) -_STEPPER_NUM_PHASES = 8 # half steps # Timings _AUTO_REPEAT_MS = 200 # Time between auto-repeats, in ms @@ -75,25 +64,24 @@ # HexDrive Hexpansion constants _EEPROM_NUM_ADDRESS_BYTES = 2 - -# HexDrive used for Y - Drive (autodetected) - XYSTAGE_HEXPANSION = 1 # Hexpansion slot for XYStage - X Driver # Dedicated Pins - to drive an external stepper driver -X_DIR = 0 # ls pin (LSA) -X_ENABLE = 1 # ls pin (LSB) - active low -Y_ENDSTOP = 0 # hs pin (HSF) - switch to ground -Y_STEP = 1 # hs pin (HSG) NOT USED -X_ENDSTOP = 2 # hs pin (HSH) - switch to ground -X_STEP = 3 # hs pin (HSI) +X_DIR = 1 # ls pin (LSB) +X_ENABLE = 0 # ls pin (LSA) - active low +X_ENDSTOP = 3 # hs pin (HSG) - switch to ground +X_STEP = 0 # hs pin (HSF) +Y_DIR = 3 # ls pin (LSD) +Y_ENABLE = 2 # ls pin (LSC) - active low +Y_ENDSTOP = 1 # hs pin (HSI) - switch to ground +Y_STEP = 2 # hs pin (HSH) _USABLE_X_PIXELS = 200 -_USABLE_Y_PIXELS = 150 -_WIDTH_DEFAULT = (1000*32) -_HEIGHT_DEFAULT = 3000 -_XRANGE_DEFAULT = (970*32) # Driver configured for 1/32 steps -_YRANGE_DEFAULT = 3100 - +_USABLE_Y_PIXELS = 140 +_WIDTH_DEFAULT = (2000*32) +_HEIGHT_DEFAULT = (2000*32) +_XRANGE_DEFAULT = (2200*32) # Driver configured for 1/32 steps +_YRANGE_DEFAULT = (2000*32) # Driver configured for 1/32 steps +POSITION_MATCH_TOLERANCE = 20 #Misceallaneous Settings _LOGGING = True @@ -159,15 +147,15 @@ def __init__(self): eventbus.on_async(HexpansionRemovalEvent, self._handle_hexpansion_removal, self) # Motor Driver - self.num_steppers: int = 1 # Default assumed for a single HexDrive + self._hexpansion_config = HexpansionConfig(XYSTAGE_HEXPANSION) # There is no EEPROM on the XYStage Hexpansion + self.num_steppers: int = 2 # Default assumed for dedicated hardware self._stepperX: Stepper = None self._stepperY: Stepper = None self.xystage = {} self.xystage['x'] = 0 self.xystage['y'] = 0 - self._time_since_last_update: int = 0 self._keep_alive_period: int = 500 # ms (half the value used in hexdrive.py) - self._timeout_period: int = 120000 # ms (2 minutes - without any user input) + self._timeout_period: int = 60*60000 # ms (60 minutes) # Overall app state (controls what is displayed and what user inputs are accepted) self.current_state = STATE_INIT @@ -257,6 +245,7 @@ def update(self, delta: int): self.hexdrive_port = list(self.ports_with_hexdrive)[0] self.hexdrive_app = self.find_hexdrive_app(self.hexdrive_port) self.current_state = STATE_MENU + self.current_state = STATE_MENU # NO HEXDRIVE REQUIRED FOR XYSTAGE self._update_main_application(delta) @@ -292,82 +281,99 @@ def _update_main_application(self, delta: int): self._update_state_settings(delta) ### End of Update ### + def _get_speed_from_disance(self, distance: int) -> int: + # calculate the speed required to move the distance in 2 second + # subject to obeying the min and max speed limits + speed = int((distance) // 2) + return max(self._settings['min_speed'].v, min(speed, self._settings['max_speed'].v)) + + # Stepper Tester: def _update_state_xystage(self, delta: int): self.xystage['x'] = self._stepperX.get_pos(delta) - self._settings['XRange'].v//2 + self.xystage['y'] = self._stepperY.get_pos(delta) - self._settings['YRange'].v//2 # Left/Right to adjust position pressed = False - if self.button_states.get(BUTTON_TYPES["RIGHT"]): - pressed = True - if self._auto_repeat_check(delta, False): - speed = abs(self._stepperX.get_speed()) - # estimate the amount of movement based on the speed and time since last update - speed = max(self._settings['min_speed'].v, self._inc(speed, 1 + self._auto_repeat_level)) - self._stepperX.speed(speed) - self._refresh = True - elif self.button_states.get(BUTTON_TYPES["LEFT"]): - pressed = True - if self._auto_repeat_check(delta, False): - speed = abs(self._stepperX.get_speed()) - speed = max(self._settings['min_speed'].v, self._inc(speed, 1 + self._auto_repeat_level)) - self._stepperX.speed(-speed) - self._refresh = True - if self.button_states.get(BUTTON_TYPES["UP"]): + if self.button_states.get(BUTTON_TYPES["CONFIRM"]): + # if CONFIRM pressed then go to position 0,0 pressed = True - if self._auto_repeat_check(delta, True): - pos = self._stepperY.get_pos() - pos = self._inc(pos, self._auto_repeat_level + 1) - # Limit to the range of the stepper - if pos >= 0: - self._stepperY.target(pos) + # if current position is not close to 0,0 then go to 0,0 + # check each of X & Y independently + # if value is too high then apply -ve speed + # if value is too low then apply +ve speed + # subject to the min and max speed limits + if self.xystage['x'] > (0 + POSITION_MATCH_TOLERANCE): + self._stepperX.speed(-self._get_speed_from_disance(abs(self.xystage['x']))) + elif self.xystage['x'] < (0 - POSITION_MATCH_TOLERANCE): + self._stepperX.speed(self._get_speed_from_disance(abs(self.xystage['x']))) + else: + self._stepperX.speed(0) + if self.xystage['y'] > (0 + POSITION_MATCH_TOLERANCE): + self._stepperY.speed(-self._get_speed_from_disance(abs(self.xystage['y']))) + elif self.xystage['y'] < (0 - POSITION_MATCH_TOLERANCE): + self._stepperY.speed(self._get_speed_from_disance(abs(self.xystage['y']))) + else: + self._stepperY.speed(0) + self._refresh = True + else: + if self.button_states.get(BUTTON_TYPES["RIGHT"]): + pressed = True + if self._auto_repeat_check(delta, False): + speed = abs(self._stepperX.get_speed()) + # estimate the amount of movement based on the speed and time since last update + speed = max(self._settings['min_speed'].v, self._inc(speed, 1 + self._auto_repeat_level)) + self._stepperX.speed(speed) + self._refresh = True + elif self.button_states.get(BUTTON_TYPES["LEFT"]): + pressed = True + if self._auto_repeat_check(delta, False): + speed = abs(self._stepperX.get_speed()) + speed = max(self._settings['min_speed'].v, self._inc(speed, 1 + self._auto_repeat_level)) + self._stepperX.speed(-speed) + self._refresh = True + if self.button_states.get(BUTTON_TYPES["UP"]): + pressed = True + if self._auto_repeat_check(delta, False): + speed = abs(self._stepperY.get_speed()) + speed = max(self._settings['min_speed'].v, self._inc(speed, 1 + self._auto_repeat_level)) + self._stepperY.speed(speed) + self._refresh = True + elif self.button_states.get(BUTTON_TYPES["DOWN"]): + pressed = True + if self._auto_repeat_check(delta, True): + speed = abs(self._stepperY.get_speed()) + speed = max(self._settings['min_speed'].v, self._inc(speed, 1 + self._auto_repeat_level)) + self._stepperY.speed(-speed) self._refresh = True - elif self.button_states.get(BUTTON_TYPES["DOWN"]): - pressed = True - if self._auto_repeat_check(delta, True): - pos = self._stepperY.get_pos() - pos = self._dec(pos, self._auto_repeat_level + 1) - # Limit to the range of the stepper - if pos <= self._settings['YRange'].v: - self._stepperY.target(pos) - self._refresh = True if pressed: self._time_since_last_input = 0 else: self._auto_repeat_clear() - self._stepperX.speed(0) # non auto-repeating buttons if self.button_states.get(BUTTON_TYPES["CANCEL"]): self.button_states.clear() - if self.hexdrive_app is not None: - self._stepperX.enable(False) - self._stepperY.enable(False) - self.current_state = STATE_MENU - return - self._time_since_last_input += delta - if self._time_since_last_input > self._timeout_period: - self._stepperX.stop() - self._stepperX.speed(0) self._stepperX.enable(False) - self._stepperY.stop() - self._stepperY.speed(0) - self._stepperY.enable(False) + self._stepperY.enable(False) self.current_state = STATE_MENU - self.notification = Notification(" Stepper:\n Timeout") - print("Stepper:Timeout") + return + if self._stepperX.speed(0) or self._stepperY.speed(0) or self._time_since_last_input == 0: + self._refresh = True + else: + self._time_since_last_input += delta + if self._time_since_last_input > self._timeout_period: + self._stepperX.stop() + self._stepperX.speed(0) + self._stepperX.enable(False) + self._stepperY.stop() + self._stepperY.speed(0) + self._stepperY.enable(False) + self.current_state = STATE_MENU + self.notification = Notification(" Stepper:\n Timeout") + print("Stepper:Timeout") - # if the stepperX or stepperY position has changed then update the display by setting refresh to True - if (self._stepperY.get_pos() - self._settings['YRange'].v//2) != self.xystage['y']: - self._refresh = True - self.xystage['y'] = self._stepperY.get_pos() - self._settings['YRange'].v//2 if self._refresh: print(f"X:{self.xystage['x']} Y:{self.xystage['y']}") - # Check if we need to keep the stepper alive - self._time_since_last_update += delta - #if self._time_since_last_update > self._keep_alive_period: - # #self._stepperX.step() - # #self._stepperY.step() - # self._time_since_last_update = 0 def _update_state_settings(self, delta: int): if self.button_states.get(BUTTON_TYPES["UP"]): @@ -425,7 +431,7 @@ def draw(self, ctx): ctx.rgb(0,0,0).rectangle(-120,-120,240,240).fill() # Main screen content if self.current_state == STATE_WARNING: - self.draw_message(ctx, ["StepperXY requires","HexDrive hexpansion","from RobotMad","github.com","/TeamRobotmad","/BadgeBot"], [(1,1,1),(1,1,0),(1,1,0),(1,1,1),(1,1,1),(1,1,1)], label_font_size) + self.draw_message(ctx, ["XYStage requires","HexDrive hexpansion","from RobotMad","github.com","/TeamRobotmad","/XYStage"], [(1,1,1),(1,1,0),(1,1,0),(1,1,1),(1,1,1),(1,1,1)], label_font_size) elif self.current_state == STATE_ERROR: self.draw_message(ctx, self.error_message, [(1,0,0)]*len(self.error_message), label_font_size) elif self.current_state == STATE_MESSAGE: @@ -446,15 +452,18 @@ def draw(self, ctx): self.notification.draw(ctx) def _draw_state_xystage(self, ctx): + ctx.rgb(1,1,1).move_to(-80, -100).text("XY Stage") # Draw outer rectangle for the XYStage based on the largest that can fit on the screen # top left of the rectangle is at -100,-100 i.e. Y is inverted ctx.rgb(0.3,0.3,0.3).rectangle(-_USABLE_X_PIXELS//2,-_USABLE_Y_PIXELS//2,_USABLE_X_PIXELS,_USABLE_Y_PIXELS).stroke() - x,y = self._scale_xystage(self.xystage['x']-(self._settings['width'].v//2),-self.xystage['y']-(self._settings['height'].v//2)) + x,y = self._scale_xystage(self.xystage['x'],-self.xystage['y']) sx,sy = self._scale_xystage(self._settings['width'].v,self._settings['height'].v) - ctx.rgb(0.0,1.0,0.5).rectangle(x,y,sx,sy).fill() - #stepper_text = "XYStage" - #stepper_text_colours[0] = (1,1,1) # Title - White - #self.draw_message(ctx, stepper_text, stepper_text_colours, label_font_size) + ctx.rgb(0.0,1.0,0.2).rectangle(x-(sx//2),y-(sy//2),sx,sy).fill() + # Draw a small black cross hair at the 'x','y' position + ctx.rgb(0,0,0).move_to(x-10,y).line_to(x+10,y).stroke() + ctx.rgb(0,0,0).move_to(x,y-10).line_to(x,y+10).stroke() + # Display the x,y position of the stage in text underneath the stage + ctx.rgb(1,1,1).move_to(-70, 100).text(f"{self.xystage['x']//32:5d}, {self.xystage['y']//32:5d}") #button_labels(ctx, confirm_label="Stop", cancel_label="Exit", left_label="<--", right_label="-->") def _scale_xystage(self, x: int, y: int) -> (int, int): @@ -545,17 +554,21 @@ def _main_menu_select_handler(self, item: str, idx: int): print(f"H:Main Menu {item} at index {idx}") if item == _main_menu_items[0]: # XYStage if self.num_steppers == 0: - self.notification = Notification("No Steppers") - print("No Steppers") + self.notification = Notification("Hexpansion Missing") + print("No Hexpansion") else: if self._stepperX is None or self._stepperY is None: # try timer IDs 0-3 until one is free for i in range(4): if self._stepperX is None: try: - # End stop - needs to be a HS pin to support interrupts - endstop_pin = HexpansionConfig(XYSTAGE_HEXPANSION).pin[X_ENDSTOP] - self._stepperX = Stepper(self, endstop = endstop_pin, name = "X", max_sps = self._settings['max_speed'].v, max_pos=self._settings['XRange'].v) + # Pins + pins = {} + pins["dir"] = self._hexpansion_config.ls_pin[X_DIR] + pins["en"] = self._hexpansion_config.ls_pin[X_ENABLE] + pins["step"] = self._hexpansion_config.pin[X_STEP] + pins["stop"] = self._hexpansion_config.pin[X_ENDSTOP] + self._stepperX = Stepper(self, pins, reverse = True, name = "X", max_sps = self._settings['max_speed'].v, max_pos=self._settings['XRange'].v) print(f"StepperX:Init {i}") continue except: @@ -563,14 +576,15 @@ def _main_menu_select_handler(self, item: str, idx: int): pass elif self._stepperY is None: try: - # End stop - needs to be a HS pin to support interrupts - endstop_pin = HexpansionConfig(XYSTAGE_HEXPANSION).pin[Y_ENDSTOP] - self._stepperY = StepperHex(self, self.hexdrive_app, endstop = endstop_pin, name = "Y", step_size=1, timer_id=i, max_pos=self._settings['YRange'].v) + # Pins + pins = {} + pins["dir"] = self._hexpansion_config.ls_pin[Y_DIR] + pins["en"] = self._hexpansion_config.ls_pin[Y_ENABLE] + pins["step"] = self._hexpansion_config.pin[Y_STEP] + pins["stop"] = self._hexpansion_config.pin[Y_ENDSTOP] + self._stepperY = Stepper(self, pins, name = "Y", max_sps = self._settings['max_speed'].v, max_pos=self._settings['YRange'].v) print(f"StepperY:Init {i}") # Start off assuming stage is in last known position - self._stepperY.overwrite_pos(self.xystage['y'] + self._settings['YRange'].v//2) - pos = self._stepperY.get_pos() - self._stepperY.target(pos) continue except: print(f"StepperY:Init {i} Failed") @@ -588,7 +602,7 @@ def _main_menu_select_handler(self, item: str, idx: int): self._stepperX.enable(True) #self._stepperX.speed(_STEPPER_DEFAULT_SPEED) self._stepperY.enable(True) - self._stepperY.speed(_STEPPER_DEFAULT_SPEED) + #self._stepperY.speed(_STEPPER_DEFAULT_SPEED) self._time_since_last_input = 0 elif item == _main_menu_items[1]: # Settings self.set_menu(_main_menu_items[3]) @@ -678,7 +692,7 @@ def _auto_repeat_clear(self): ######## STEPPER MOTOR CLASS ######## class Stepper: # External Driver, Speed Control Only via PWM - def __init__(self, container, endstop: Pin = None, name: str = "", speed_sps: int = _STEPPER_DEFAULT_SPEED, max_sps: int = _STEPPER_MAX_SPEED, max_pos: int = _STEPPER_MAX_POSITION): + def __init__(self, container, pins, reverse = False, name: str = "", max_sps: int = _STEPPER_MAX_SPEED, max_pos: int = _STEPPER_MAX_POSITION): self._container = container self._name = name self._calibrated = False @@ -688,47 +702,71 @@ def __init__(self, container, endstop: Pin = None, name: str = "", speed_sps: in self._pos = 0 # current position in steps self._free_run_mode = 1 # direction of free run mode self._enabled = False + self._reverse = reverse + self._max_sps_change = int(max_sps/10) # max change in speed in steps per second per update self._max_sps = int(max_sps) # max speed in steps per second - self._steps_per_sec = int(speed_sps) # current speed in steps per second + self._steps_per_sec = 0 # current speed in steps per second self._max_pos = int(max_pos) # max position stored in half steps self._freq = 0 # Pins for external stepper driver - self._hexpansion_config = HexpansionConfig(XYSTAGE_HEXPANSION) - self._pins = {} - # Direction Pin - self._pins["x_dir"] = self._hexpansion_config.ls_pin[X_DIR] - self._pins["x_dir"].init(mode=Pin.OUT) - self._pins["x_dir"].off() - # Enable Pin - active low - self._pins["x_en"] = self._hexpansion_config.ls_pin[X_ENABLE] - self._pins["x_en"].init(mode=Pin.OUT) - self._pins["x_en"].on() - # Step Pin - needs to be a HS pin to support PWM - self._pins["x_step"] = self._hexpansion_config.pin[X_STEP] - #self._pins["x_step"].init(mode=Pin.OUT) - #self._pins["x_step"].off() + self._pins = pins + self._pins["en"].init(mode=Pin.OUT) + self._pins["en"].on() # active low + self._pins["dir"].init(mode=Pin.OUT) + self._pins["dir"].off() + self._pins["step"].init(mode=Pin.OUT) + self._pins["step"].off() + self._pins["stop"].init(mode=Pin.IN, pull=Pin.PULL_UP) + self._pins["stop"].irq(trigger=Pin.IRQ_FALLING, handler=self._hit_endstop) + # Setup PWM output on the step pin try: - self._pwm = PWM(self._pins["x_step"], freq=10, duty_ns=2000) # 0 Hz is invalid but 0 duty is allowed + self._pwm = PWM(self._pins["step"], freq=10, duty_ns=2000) # 0 Hz is invalid but 0 duty is allowed except Exception as e: print(f"{self._name} PWM failed:{e}") - # End stop - needs to be a HS pin to support interrupts - if endstop is not None: - endstop.init(mode=Pin.IN, pull=Pin.PULL_UP) - endstop.irq(trigger=Pin.IRQ_FALLING, handler=self._hit_endstop) - def speed(self,sps): # speed in FULL steps per second + def speed(self,sps) -> bool: # speed in FULL steps per second if self._free_run_mode == 1 and sps < 0: self._free_run_mode = -1 elif self._free_run_mode == -1 and sps > 0: self._free_run_mode = 1 - if sps > self._max_sps: - sps = self._max_sps - elif sps < -self._max_sps: - sps = -self._max_sps + speed_change_limited = False + if sps > 0: + if self._calibrated and self._pos >= self._max_pos: + # endstop reached + sps = 0 + else: + if sps > self._max_sps: + # limit speed + sps = self._max_sps + # limit acceleration by comparing the change in speed to the max acceleration + # if the change is greater than the max acceleration, limit the change to the max acceleration + if sps - self._steps_per_sec > self._max_sps_change: + sps = self._steps_per_sec + self._max_sps_change + speed_change_limited = True + elif sps - self._steps_per_sec < -self._max_sps_change: + sps = self._steps_per_sec - self._max_sps_change + speed_change_limited = True + else: + if self._pins["stop"].value() == 0 or (self._calibrated and self._pos <= 0): + # endstop reached + sps = 0 + else: + if sps < -self._max_sps: + # limit speed + sps = -self._max_sps + # limit acceleration by comparing the change in speed to the max acceleration + # if the change is greater than the max acceleration, limit the change to the max acceleration + if sps - self._steps_per_sec > self._max_sps_change: + sps = self._steps_per_sec + self._max_sps_change + speed_change_limited = True + elif sps - self._steps_per_sec < -self._max_sps_change: + sps = self._steps_per_sec - self._max_sps_change + speed_change_limited = True self._steps_per_sec = int(sps) self._update_timer(abs(self._steps_per_sec)) # steps per second + return speed_change_limited def get_speed(self) -> int: return self._steps_per_sec @@ -738,14 +776,13 @@ def get_pos(self, delta) -> int: steps = (self._steps_per_sec * delta) // 1000 self._pos += steps # Check if we have hit the end stop - if self._calibrated and self._pos < 0: - print(f"{self._name} s/w min endstop") - #self._pos = 0 - self.speed(0) - elif self._calibrated and self._pos > self._max_pos: - print(f"{self._name} s/w max endstop") - #self._pos = self._max_pos - self.speed(0) + if self._calibrated: + if self._pos < 0 and self._steps_per_sec < 0: + print(f"{self._name} s/w min endstop") + self.speed(0) + elif self._pos > self._max_pos and self._steps_per_sec > 0: + print(f"{self._name} s/w max endstop") + self.speed(0) return self._pos def _hit_endstop(self, pin: Pin): @@ -756,31 +793,33 @@ def _hit_endstop(self, pin: Pin): if not self._calibrated: self._calibrated = True self._pos = 0 - self.speed(0) + # if we are still trying to move TOWARDS the endstop + if self._steps_per_sec < 0: + self.speed(0) else: print(f"{self._name} Endstop - false alarm") def _update_timer(self,freq): if freq == 0: - print(f"{self._name} Timer: 0Hz") - self._pins["x_en"].on() # disable the stepper + #print(f"{self._name} Timer: 0Hz") + self._pins["en"].on() # disable the stepper self._pwm.duty_ns(0) # stop the PWM (frequency of 0 is not allowed) self._freq = 0 elif freq != self._freq or self._free_run_mode != self._timer_mode: try: print(f"{self._name} Timer:{self._free_run_mode} {freq}Hz") if self._free_run_mode>0: - self._pins["x_dir"].off() + self._pins["dir"].value(1 if self._reverse else 0) self._pwm.freq(int(freq)) - self._pwm.duty_ns(12000) # minimum 1.9uS STEP pulse width for DRV8825 - self._pins["x_en"].off() # enable active low + self._pwm.duty_ns(2000) # minimum 1.9uS STEP pulse width for DRV8825 + self._pins["en"].off() # enable active low elif self._free_run_mode<0: - self._pins["x_dir"].on() + self._pins["dir"].value(0 if self._reverse else 1) self._pwm.freq(int(freq)) - self._pwm.duty_ns(12000) # minimum 1.9uS STEP pulse width for DRV8825 - self._pins["x_en"].off() # enable active low + self._pwm.duty_ns(2000) # minimum 1.9uS STEP pulse width for DRV8825 + self._pins["en"].off() # enable active low else: - self._pins["x_en"].on() + self._pins["en"].on() self._pwm.duty_ns(0) # stop the PWM (frequency of 0 is not allowed) self._freq = freq self._timer_is_running=True @@ -794,7 +833,7 @@ def stop(self): def enable(self,e = True): self._enabled=e - self._pins["x_en"].value(not e) + self._pins["en"].value(not e) try: if e: if self._free_run_mode!=0: @@ -809,194 +848,6 @@ def is_enabled(self) -> bool: ########## END OF STEPPER CLASS ########## -class StepperHex: - def __init__(self, container, hexdrive_app, endstop: Pin = None, name: str = "", step_size: int = 1, speed_sps: int = _STEPPER_DEFAULT_SPEED, max_sps: int = _STEPPER_MAX_SPEED, max_pos: int = _STEPPER_MAX_POSITION, timer_id: int = 0): - self._container = container - self._name = name - self._hexdrive_app = hexdrive_app - self._phase = 0 - self._calibrated = False - self._timer = Timer(timer_id) - self._timer_is_running = False - self._timer_mode = 0 - self._free_run_mode = 0 # direction of free run mode - self._enabled = False - self._target_pos = 0 - self._pos = 0 # current position in half steps - self._max_sps = int(max_sps) # max speed in full steps per second - self._steps_per_sec = int(speed_sps) # current speed in full steps per second - self._max_pos = 2*int(max_pos) # max position stored in half steps - self._freq = 0 - self._min_period = 0 - self._step_size = int(step_size) # 1 = half steps, 2 = full steps - self._last_step_time = 0 - self.track_target() - - # End stop - needs to be a HS pin to support interrupts - if endstop is not None: - endstop.init(mode=Pin.IN, pull=Pin.PULL_UP) - endstop.irq(trigger=Pin.IRQ_FALLING, handler=self._hit_endstop) - - def step_size(self,sz=1): - if sz < 1: - sz = 1 - elif sz > 2: - sz = 2 - self._step_size = int(sz) - - def speed(self,sps): # speed in FULL steps per second - if self._free_run_mode == 1 and sps < 0: - self._free_run_mode = -1 - elif self._free_run_mode == -1 and sps > 0: - self._free_run_mode = 1 - if sps > self._max_sps: - sps = self._max_sps - elif sps < -self._max_sps: - sps = -self._max_sps - self._steps_per_sec = int(sps) - self._update_timer((2//self._step_size)*abs(self._steps_per_sec)) # steps per second - - def get_speed(self) -> int: - return self._steps_per_sec - - def target(self,t): - if self._calibrated and t < 0: - # when already calibrated limit to 0 - self._target_pos = 0 - elif self._calibrated and (2*int(t)) > self._max_pos: - # when already calibrated limit to max - self._target_pos = self._max_pos - else: - self._target_pos = 2*int(t) - - def get_pos(self) -> int: - return (self._pos//2) # convert half steps to full steps - - def overwrite_pos(self,p=0): - self._pos = 2*int(p) # convert full steps to half steps - - def step(self,d=0): - cur_time = time.ticks_ms() - if time.ticks_diff(cur_time, self._last_step_time) < self._min_period: - # avoid stepping too quickly as this causes skipped steps - return - self._last_step_time = cur_time - if d>0: - self._pos+=self._step_size - self._phase = (self._phase+self._step_size)%_STEPPER_NUM_PHASES - elif d<0: - self._pos-=self._step_size - self._phase = (self._phase-self._step_size)%_STEPPER_NUM_PHASES - # Check position limits - if self._calibrated and self._pos < 0: - print(f"{self._name} s/w min endstop") - self._pos = 0 - self.speed(0) - return - elif self._calibrated and self._pos > self._max_pos: - print(f"{self._name} s/w max endstop") - self._pos = self._max_pos - self.speed(0) - return - try: - #print(f"s{self._phase}") - self._hexdrive_app.motor_step(self._phase) - except Exception as e: - print(f"{self._name} step phase {self._phase} failed:{e}") - - - def _hit_endstop(self, pin: Pin): - # double check the endstop is hit - # if not, ignore the interrupt - if pin.value() == 0: - print(f"{self._name} Endstop - hit") - if not self._calibrated: - self._calibrated = True - # if we were moving towards the endstop, stop - if self._free_run_mode < 0: - self.speed(0) - # set this as the new zero position - self.overwrite_pos(0) - elif self._free_run_mode == 0 and self._target_pos < self._pos: - self.speed(0) - else: - print(f"{self._name} Endstop - false alarm") - - def _timer_callback_fwd(self,t): - self.step(1) - - def _timer_callback_rev(self,t): - self.step(-1) - - def _timer_callback(self,t): - if self._target_pos>self._pos: - self.step(1) - elif self._target_pos0: - self._timer.init(freq=freq,callback=self._timer_callback_fwd) - elif self._free_run_mode<0: - self._timer.init(freq=freq,callback=self._timer_callback_rev) - else: - self._timer.init(freq=freq,callback=self._timer_callback) - self._freq = freq - self._min_period = (1000//freq) - 1 - self._timer_is_running=True - self._timer_mode = self._free_run_mode - except Exception as e: - print(f"{self._name} update_timer failed:{e}") - elif freq == 0: - print(f"{self._name} Timer: 0Hz") - - def stop(self): - self._update_timer(0) - try: - self._hexdrive_app.motor_release() - except Exception as e: - print(f"{self._name} stop failed:{e}") - - def enable(self,e = True): - self._enabled=e - try: - if e: - if self._free_run_mode!=0: - self._update_timer((2//self._step_size)*abs(self._steps_per_sec)) # half steps per second - self._hexdrive_app.motor_step(self._phase) - else: - self._update_timer(0) - self._hexdrive_app.motor_release() - self._hexdrive_app.set_power(e) - except Exception as e: - print(f"{self._name} enable failed:{e}") - - def is_enabled(self) -> bool: - return self._enabled - - class HexDriveType: def __init__(self, pid, vid = 0xCAFE, motors = 0, steppers = 0, servos = 0, name ="Unknown"): self.vid = vid From c72a9304e0ba262408ab528bccb9ea0f22e8450f Mon Sep 17 00:00:00 2001 From: robotmad Date: Sat, 5 Oct 2024 13:38:43 +0100 Subject: [PATCH 12/16] tidy up to split XYStage into its own repo --- app.py | 12 +- xystage.py | 940 ----------------------------------------------------- 2 files changed, 4 insertions(+), 948 deletions(-) delete mode 100644 xystage.py diff --git a/app.py b/app.py index e23908c..6044121 100644 --- a/app.py +++ b/app.py @@ -46,7 +46,7 @@ CURRENT_APP_VERSION = 6 # HEXDRIVE.PY Integer Version Number - checked against the EEPROM app.py version to determine if it needs updating -_APP_VERSION = "1.4" # BadgeBot App Version Number +_APP_VERSION = "1.5" # BadgeBot App Version Number # If you change the URL then you will need to regenerate the QR code _QR_CODE = [0x1fcf67f, @@ -98,7 +98,7 @@ # Stepper Tester - Defaults _STEPPER_MAX_SPEED = 200 # full steps per second -_STEPPER_MAX_POSITION = 3100 # full steps from h/w endstop to s/w endstop at the other end +_STEPPER_MAX_POSITION = 3100 # full steps from one end to the other end _STEPPER_DEFAULT_SPEED = 50 # full steps per second _STEPPER_NUM_PHASES = 8 # half steps _STEPPER_DEFAULT_SPR = 200 # full steps per revolution @@ -148,12 +148,6 @@ _EEPROM_PAGE_SIZE = 32 _EEPROM_TOTAL_SIZE = 64 * 1024 // 8 -X_DIR = 0 -X_STEP = 1 -Y_DIR = 2 -Y_STEP = 3 - - #Misceallaneous Settings _LOGGING = False _ERASE_SLOT = 0 # Slot for user to set if they want to erase EEPROMs on HexDrives @@ -2033,6 +2027,8 @@ def step(self,d=0): except Exception as e: print(f"step phase {self._phase} failed:{e}") + # There is no code to handle the endstop being hit at present - it needs to be specific to the hardware + # i.e. which pin is connected to the endstop. def _hit_endstop(self): print("Endstop - hit") if not self._calibrated: diff --git a/xystage.py b/xystage.py deleted file mode 100644 index 5865f31..0000000 --- a/xystage.py +++ /dev/null @@ -1,940 +0,0 @@ -import asyncio -import aioble -import bluetooth -import os -import time -from math import cos, pi -import ota -import settings -import vfs -from app_components.notification import Notification -from app_components.tokens import label_font_size, twentyfour_pt, clear_background, button_labels -from app_components import Menu -from events.input import BUTTON_TYPES, Button, Buttons, ButtonUpEvent -from frontboards.twentyfour import BUTTONS -from machine import PWM, Timer, Pin -from system.eventbus import eventbus -from system.hexpansion.events import (HexpansionInsertionEvent, - HexpansionRemovalEvent) -from system.hexpansion.header import read_header -from system.hexpansion.config import HexpansionConfig -from system.scheduler import scheduler -from system.scheduler.events import (RequestStopAppEvent) - -from tildagonos import tildagonos - -import app - -from .utils import chain, draw_logo_animated, parse_version - - - -# Hard coded to talk to EEPROMs on address 0x50 - because we know that is what is on the HexDrive Hexpansion -# makes it a lot more efficient than scanning the I2C bus for devices and working out what they are - -CURRENT_APP_VERSION = 5 # HEXDRIVE.PY Integer Version Number - checked against the EEPROM app.py version to determine if it needs updating - -_APP_VERSION = "1.0" # BadgeBot App Version Number - - -# Stepper Tester - Defaults -_STEPPER_MAX_SPEED = 1100*32 # full steps per second -_STEPPER_MIN_SPEED = 20*32 # full steps per second -_STEPPER_MAX_POSITION = 3100 # full steps from h/w endstop to s/w endstop at the other end - -# Timings -_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 - - -# App states -STATE_INIT = -1 -STATE_WARNING = 0 -STATE_MENU = 1 -STATE_XYSTAGE = 2 -STATE_ERROR = 3 # Hexpansion error -STATE_MESSAGE = 4 # Message display -STATE_SETTINGS = 5 # Edit Settings - -# App states where user can minimise app -_MINIMISE_VALID_STATES = [0, 1, 3, 4, 5] - -# HexDrive Hexpansion constants -_EEPROM_NUM_ADDRESS_BYTES = 2 - -XYSTAGE_HEXPANSION = 1 # Hexpansion slot for XYStage - X Driver -# Dedicated Pins - to drive an external stepper driver -X_DIR = 1 # ls pin (LSB) -X_ENABLE = 0 # ls pin (LSA) - active low -X_ENDSTOP = 3 # hs pin (HSG) - switch to ground -X_STEP = 0 # hs pin (HSF) -Y_DIR = 3 # ls pin (LSD) -Y_ENABLE = 2 # ls pin (LSC) - active low -Y_ENDSTOP = 1 # hs pin (HSI) - switch to ground -Y_STEP = 2 # hs pin (HSH) - -_USABLE_X_PIXELS = 200 -_USABLE_Y_PIXELS = 140 -_WIDTH_DEFAULT = (2000*32) -_HEIGHT_DEFAULT = (2000*32) -_XRANGE_DEFAULT = (2200*32) # Driver configured for 1/32 steps -_YRANGE_DEFAULT = (2000*32) # Driver configured for 1/32 steps -POSITION_MATCH_TOLERANCE = 20 - -#Misceallaneous Settings -_LOGGING = True - -# Menu Items -_main_menu_items = ["XYStage", "Settings", "About","Exit"] - -class XYStageApp(app.App): - def __init__(self): - super().__init__() - # UI Button Controls - self.button_states = Buttons(self) - self.last_press: Button = BUTTON_TYPES["CANCEL"] - self.long_press_delta: int = 0 - 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 - - # UI Feature Controls - self._refresh: bool = True - self.notification: Notification = None - self.error_message = [] - self.current_menu: str = None - self.menu: Menu = None - - # Settings - self._settings = {} - self._settings['logging'] = MySetting(self._settings, _LOGGING, False, True) - self._settings['width'] = MySetting(self._settings, _WIDTH_DEFAULT, 10, 100000) - self._settings['height'] = MySetting(self._settings, _HEIGHT_DEFAULT, 10, 100000) - self._settings['XRange'] = MySetting(self._settings, _XRANGE_DEFAULT, 10, 100000) - self._settings['YRange'] = MySetting(self._settings, _YRANGE_DEFAULT, 10, 100000) - self._settings['min_speed'] = MySetting(self._settings, _STEPPER_MIN_SPEED, 10, 10000) - self._settings['max_speed'] = MySetting(self._settings, _STEPPER_MAX_SPEED, 10, 100000) - - self._edit_setting: int = None - self._edit_setting_value = None - self.update_settings() - - # Check what version of the Badge s/w we are running on - try: - ver = parse_version(ota.get_version()) - if ver is not None: - if self._settings['logging'].v: - print(f"XYStage V{ver}") - # Potential to do things differently based on badge s/w version - # e.g. if ver < [1, 9, 0]: - except: - pass - - # Hexpansion related - self._HEXDRIVE_TYPES = [HexDriveType(0xCBCB, motors=2, servos=4), - HexDriveType(0xCBCA, motors=2, name="2 Motor"), - HexDriveType(0xCBCC, servos=4, name="4 Servo"), - HexDriveType(0xCBCD, motors=1, servos=2, name="1 Mot 2 Srvo"), - HexDriveType(0xCBCE, steppers=1, name="Stepper")] - self.hexpansion_slot_type = [None]*6 - self.hexdrive_port: int = None - self.ports_with_hexdrive = set() - self.hexdrive_app = None - eventbus.on_async(HexpansionInsertionEvent, self._handle_hexpansion_insertion, self) - eventbus.on_async(HexpansionRemovalEvent, self._handle_hexpansion_removal, self) - - # Motor Driver - self._hexpansion_config = HexpansionConfig(XYSTAGE_HEXPANSION) # There is no EEPROM on the XYStage Hexpansion - self.num_steppers: int = 2 # Default assumed for dedicated hardware - self._stepperX: Stepper = None - self._stepperY: Stepper = None - self.xystage = {} - self.xystage['x'] = 0 - self.xystage['y'] = 0 - self._keep_alive_period: int = 500 # ms (half the value used in hexdrive.py) - self._timeout_period: int = 60*60000 # ms (60 minutes) - - # Overall app state (controls what is displayed and what user inputs are accepted) - self.current_state = STATE_INIT - self.previous_state = self.current_state - print("XYStageApp:Init") - - - ### ASYNC EVENT HANDLERS ### - - async def _handle_hexpansion_removal(self, event: HexpansionRemovalEvent): - self.hexpansion_slot_type[event.port-1] = None - if event.port in self.ports_with_hexdrive: - self.ports_with_hexdrive.remove(event.port) - if event.port == self.hexdrive_port: - self.hexdrive_port = None - self.hexdrive_app = None - self.current_state = STATE_WARNING - self.notification = Notification("HexDrive Removed") - - async def _handle_hexpansion_insertion(self, event: HexpansionInsertionEvent): - if self.check_port_for_hexdrive(event.port): - pass - ### HEXPANSION FUNCTIONS ### - - # Scan the Hexpansion ports for EEPROMs and HexDrives in case they are already plugged in when we start - def scan_ports(self): - for port in range(1, 7): - self.check_port_for_hexdrive(port) - - - def check_port_for_hexdrive(self, port: int) -> bool: - # we know the EEPROM address so we can just read the header directly - if port not in range(1, 7): - return False - # We want to do this in two parts so that we detect if there is a valid EEPROM or not - try: - hexpansion_header = read_header(port, addr_len=_EEPROM_NUM_ADDRESS_BYTES) - except OSError: - # no EEPROM on this port - return False - except RuntimeError: - # not a valid header - if self._settings['logging'].v: - print(f"H:Found EEPROM on port {port}") - return True - # check is this is a HexDrive header by scanning the _HEXDRIVE_TYPES list - for index, hexpansion_type in enumerate(self._HEXDRIVE_TYPES): - if hexpansion_header.vid == hexpansion_type.vid and hexpansion_header.pid == hexpansion_type.pid: - if self._settings['logging'].v: - print(f"H:Found '{hexpansion_type.name}' HexDrive on port {port}") - if port not in self.ports_with_hexdrive: - self.ports_with_hexdrive.add(port) - self.hexpansion_slot_type[port-1] = index - return True - # we are not interested in this type of hexpansion - return False - - - def find_hexdrive_app(self, port: int) -> app: - for an_app in scheduler.apps: - if type(an_app).__name__ is 'HexDriveApp': - if hasattr(an_app, "config") and hasattr(an_app.config, "port") and an_app.config.port == port: - return an_app - return None - - - def update_settings(self): - for s in self._settings: - self._settings[s].v = settings.get(f"xystage.{s}", self._settings[s].d) - - - ### MAIN APP CONTROL FUNCTIONS ### - - def update(self, delta: int): - if self.notification: - self.notification.update(delta) - - if self.current_state == STATE_INIT: - # One Time initialisation - self.scan_ports() - if (len(self.ports_with_hexdrive) == 0): - # There are currently no possible HexDrives plugged in - self.current_state = STATE_WARNING - else: - # We have a HexDrive so we can start the main app - # remember which port it is on - self.hexdrive_port = list(self.ports_with_hexdrive)[0] - self.hexdrive_app = self.find_hexdrive_app(self.hexdrive_port) - self.current_state = STATE_MENU - self.current_state = STATE_MENU # NO HEXDRIVE REQUIRED FOR XYSTAGE - - self._update_main_application(delta) - - if self.current_state != self.previous_state: - if self._settings['logging'].v: - print(f"State: {self.previous_state} -> {self.current_state}") - self.previous_state = self.current_state - # something has changed - so worth redrawing - self._refresh = True - - - def _update_main_application(self, delta: int): - if self.current_state == STATE_MENU: - if self.current_menu is None: - self.set_menu("main") - self._refresh = True - else: - self.menu.update(delta) - if self.menu.is_animating != "none": - if self._settings['logging'].v: - print("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() - - ### XY Stage Application ### - elif self.current_state == STATE_XYSTAGE: - self._update_state_xystage(delta) - - ### Settings Capability ### - elif self.current_state == STATE_SETTINGS: - self._update_state_settings(delta) - ### End of Update ### - - def _get_speed_from_disance(self, distance: int) -> int: - # calculate the speed required to move the distance in 2 second - # subject to obeying the min and max speed limits - speed = int((distance) // 2) - return max(self._settings['min_speed'].v, min(speed, self._settings['max_speed'].v)) - - - # Stepper Tester: - def _update_state_xystage(self, delta: int): - self.xystage['x'] = self._stepperX.get_pos(delta) - self._settings['XRange'].v//2 - self.xystage['y'] = self._stepperY.get_pos(delta) - self._settings['YRange'].v//2 - # Left/Right to adjust position - pressed = False - if self.button_states.get(BUTTON_TYPES["CONFIRM"]): - # if CONFIRM pressed then go to position 0,0 - pressed = True - # if current position is not close to 0,0 then go to 0,0 - # check each of X & Y independently - # if value is too high then apply -ve speed - # if value is too low then apply +ve speed - # subject to the min and max speed limits - if self.xystage['x'] > (0 + POSITION_MATCH_TOLERANCE): - self._stepperX.speed(-self._get_speed_from_disance(abs(self.xystage['x']))) - elif self.xystage['x'] < (0 - POSITION_MATCH_TOLERANCE): - self._stepperX.speed(self._get_speed_from_disance(abs(self.xystage['x']))) - else: - self._stepperX.speed(0) - if self.xystage['y'] > (0 + POSITION_MATCH_TOLERANCE): - self._stepperY.speed(-self._get_speed_from_disance(abs(self.xystage['y']))) - elif self.xystage['y'] < (0 - POSITION_MATCH_TOLERANCE): - self._stepperY.speed(self._get_speed_from_disance(abs(self.xystage['y']))) - else: - self._stepperY.speed(0) - self._refresh = True - else: - if self.button_states.get(BUTTON_TYPES["RIGHT"]): - pressed = True - if self._auto_repeat_check(delta, False): - speed = abs(self._stepperX.get_speed()) - # estimate the amount of movement based on the speed and time since last update - speed = max(self._settings['min_speed'].v, self._inc(speed, 1 + self._auto_repeat_level)) - self._stepperX.speed(speed) - self._refresh = True - elif self.button_states.get(BUTTON_TYPES["LEFT"]): - pressed = True - if self._auto_repeat_check(delta, False): - speed = abs(self._stepperX.get_speed()) - speed = max(self._settings['min_speed'].v, self._inc(speed, 1 + self._auto_repeat_level)) - self._stepperX.speed(-speed) - self._refresh = True - if self.button_states.get(BUTTON_TYPES["UP"]): - pressed = True - if self._auto_repeat_check(delta, False): - speed = abs(self._stepperY.get_speed()) - speed = max(self._settings['min_speed'].v, self._inc(speed, 1 + self._auto_repeat_level)) - self._stepperY.speed(speed) - self._refresh = True - elif self.button_states.get(BUTTON_TYPES["DOWN"]): - pressed = True - if self._auto_repeat_check(delta, True): - speed = abs(self._stepperY.get_speed()) - speed = max(self._settings['min_speed'].v, self._inc(speed, 1 + self._auto_repeat_level)) - self._stepperY.speed(-speed) - self._refresh = True - if pressed: - self._time_since_last_input = 0 - else: - self._auto_repeat_clear() - # non auto-repeating buttons - if self.button_states.get(BUTTON_TYPES["CANCEL"]): - self.button_states.clear() - self._stepperX.enable(False) - self._stepperY.enable(False) - self.current_state = STATE_MENU - return - if self._stepperX.speed(0) or self._stepperY.speed(0) or self._time_since_last_input == 0: - self._refresh = True - else: - self._time_since_last_input += delta - if self._time_since_last_input > self._timeout_period: - self._stepperX.stop() - self._stepperX.speed(0) - self._stepperX.enable(False) - self._stepperY.stop() - self._stepperY.speed(0) - self._stepperY.enable(False) - self.current_state = STATE_MENU - self.notification = Notification(" Stepper:\n Timeout") - print("Stepper:Timeout") - - if self._refresh: - print(f"X:{self.xystage['x']} Y:{self.xystage['y']}") - - - def _update_state_settings(self, delta: int): - 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._settings['logging'].v: - print(f"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._settings['logging'].v: - print(f"Setting: {self._edit_setting} (-) Value: {self._edit_setting_value}") - self._refresh = True - else: - # non auto-repeating buttons - self._auto_repeat_clear() - if self.button_states.get(BUTTON_TYPES["RIGHT"]) or self.button_states.get(BUTTON_TYPES["LEFT"]): - self.button_states.clear() - # Force default value - self._edit_setting_value = self._settings[self._edit_setting].d - if self._settings['logging'].v: - print(f"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() - # leave setting unchanged - if self._settings['logging'].v: - print(f"Setting: {self._edit_setting} Cancelled") - self.set_menu(_main_menu_items[3]) - self.current_state = STATE_MENU - elif self.button_states.get(BUTTON_TYPES["CONFIRM"]): - self.button_states.clear() - # set setting - if self._settings['logging'].v: - print(f"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.set_menu(_main_menu_items[3]) - self.current_state = STATE_MENU - - - def draw(self, ctx): - if self._refresh or self.notification is not None: - self._refresh = False - clear_background(ctx) - ctx.save() - 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 - ctx.rgb(0,0,0).rectangle(-120,-120,240,240).fill() - # Main screen content - if self.current_state == STATE_WARNING: - self.draw_message(ctx, ["XYStage requires","HexDrive hexpansion","from RobotMad","github.com","/TeamRobotmad","/XYStage"], [(1,1,1),(1,1,0),(1,1,0),(1,1,1),(1,1,1),(1,1,1)], label_font_size) - elif self.current_state == STATE_ERROR: - self.draw_message(ctx, self.error_message, [(1,0,0)]*len(self.error_message), label_font_size) - elif self.current_state == STATE_MESSAGE: - self.draw_message(ctx, self.error_message, [(0,1,0)]*len(self.error_message), label_font_size) - elif self.current_state == STATE_XYSTAGE: - self._draw_state_xystage(ctx) - elif self.current_state == STATE_SETTINGS: - self.draw_message(ctx, ["Edit Setting",f"{self._edit_setting}:",f"{self._edit_setting_value}"], [(1,1,1),(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") - ctx.restore() - - # These need to be drawn every frame as they contain animations - if self.current_state == STATE_MENU: - clear_background(ctx) - self.menu.draw(ctx) - - if self.notification: - self.notification.draw(ctx) - - def _draw_state_xystage(self, ctx): - ctx.rgb(1,1,1).move_to(-80, -100).text("XY Stage") - # Draw outer rectangle for the XYStage based on the largest that can fit on the screen - # top left of the rectangle is at -100,-100 i.e. Y is inverted - ctx.rgb(0.3,0.3,0.3).rectangle(-_USABLE_X_PIXELS//2,-_USABLE_Y_PIXELS//2,_USABLE_X_PIXELS,_USABLE_Y_PIXELS).stroke() - x,y = self._scale_xystage(self.xystage['x'],-self.xystage['y']) - sx,sy = self._scale_xystage(self._settings['width'].v,self._settings['height'].v) - ctx.rgb(0.0,1.0,0.2).rectangle(x-(sx//2),y-(sy//2),sx,sy).fill() - # Draw a small black cross hair at the 'x','y' position - ctx.rgb(0,0,0).move_to(x-10,y).line_to(x+10,y).stroke() - ctx.rgb(0,0,0).move_to(x,y-10).line_to(x,y+10).stroke() - # Display the x,y position of the stage in text underneath the stage - ctx.rgb(1,1,1).move_to(-70, 100).text(f"{self.xystage['x']//32:5d}, {self.xystage['y']//32:5d}") - #button_labels(ctx, confirm_label="Stop", cancel_label="Exit", left_label="<--", right_label="-->") - - def _scale_xystage(self, x: int, y: int) -> (int, int): - # scale x,y to the canvas range: - # x,y are in the range -'XRange'/2 to 'XRange'/2 and -'YRange'/2 to 'YRange'/2 - x = int(_USABLE_X_PIXELS*x/(self._settings['XRange'].v + self._settings['width'].v)) - y = int(_USABLE_Y_PIXELS*y/(self._settings['YRange'].v + self._settings['height'].v)) - return x, y - - # Value increment/decrement functions for positive integers only - def _inc(self, v: int, l: int): - if l==0: - return v+1 - else: - d = 10**l - v = ((v // d) + 1) * d # round up to the next multiple of 10^l - return v - - def _dec(self, v: int, l: int): - if l==0: - return v-1 - else: - d = 10**l - v = (((v+(9*(10**(l-1)))) // d) - 1) * d # round down to the next multiple of 10^l - return v - - - def draw_message(self, ctx, message, colours, size=label_font_size): - 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) - -### MENU FUNCTIONALITY ### - - - def set_menu(self, menu_name = "main"): #: Literal["main"]): does it work without the type hint? - if self._settings['logging'].v: - print(f"H:Set Menu {menu_name}") - if self.menu is not None: - try: - self.menu._cleanup() - 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() - if self.num_steppers == 0: - menu_items.remove(_main_menu_items[0]) - self.menu = Menu( - self, - menu_items, - select_handler=self._main_menu_select_handler, - back_handler=self._menu_back_handler, - ) - elif menu_name == "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._settings['logging'].v: - print(f"H:Main Menu {item} at index {idx}") - if item == _main_menu_items[0]: # XYStage - if self.num_steppers == 0: - self.notification = Notification("Hexpansion Missing") - print("No Hexpansion") - else: - if self._stepperX is None or self._stepperY is None: - # try timer IDs 0-3 until one is free - for i in range(4): - if self._stepperX is None: - try: - # Pins - pins = {} - pins["dir"] = self._hexpansion_config.ls_pin[X_DIR] - pins["en"] = self._hexpansion_config.ls_pin[X_ENABLE] - pins["step"] = self._hexpansion_config.pin[X_STEP] - pins["stop"] = self._hexpansion_config.pin[X_ENDSTOP] - self._stepperX = Stepper(self, pins, reverse = True, name = "X", max_sps = self._settings['max_speed'].v, max_pos=self._settings['XRange'].v) - print(f"StepperX:Init {i}") - continue - except: - print(f"StepperX:Init {i} Failed") - pass - elif self._stepperY is None: - try: - # Pins - pins = {} - pins["dir"] = self._hexpansion_config.ls_pin[Y_DIR] - pins["en"] = self._hexpansion_config.ls_pin[Y_ENABLE] - pins["step"] = self._hexpansion_config.pin[Y_STEP] - pins["stop"] = self._hexpansion_config.pin[Y_ENDSTOP] - self._stepperY = Stepper(self, pins, name = "Y", max_sps = self._settings['max_speed'].v, max_pos=self._settings['YRange'].v) - print(f"StepperY:Init {i}") - # Start off assuming stage is in last known position - continue - except: - print(f"StepperY:Init {i} Failed") - pass - else: - break - if self._stepperX is None or self._stepperY is None: - self.notification = Notification("No Free Timers") - else: - self.set_menu(None) - self.button_states.clear() - self.current_state = STATE_XYSTAGE - self._refresh = True - self._auto_repeat_clear() - self._stepperX.enable(True) - #self._stepperX.speed(_STEPPER_DEFAULT_SPEED) - self._stepperY.enable(True) - #self._stepperY.speed(_STEPPER_DEFAULT_SPEED) - self._time_since_last_input = 0 - elif item == _main_menu_items[1]: # Settings - self.set_menu(_main_menu_items[3]) - elif item == _main_menu_items[2]: # About - self.set_menu(None) - self.button_states.clear() - self.error_message = ["XYStage","Version: 1.0"] - self.current_state = STATE_MESSAGE - self._refresh = True - elif item == _main_menu_items[3]: # Exit - eventbus.remove(HexpansionInsertionEvent, self._handle_hexpansion_insertion, self) - eventbus.remove(HexpansionRemovalEvent, self._handle_hexpansion_removal, self) - eventbus.emit(RequestStopAppEvent(self)) - - def _settings_menu_select_handler(self, item: str, idx: int): - if self._settings['logging'].v: - print(f"H:Setting {item} @ {idx}") - if idx == 0: #Save - if self._settings['logging'].v: - print("H:Settings Save All") - settings.save() - self.notification = Notification(" Settings Saved") - self.set_menu("main") - elif idx == 1: #Default - if self._settings['logging'].v: - print("H: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("main") - else: - self.set_menu(None) - self.button_states.clear() - self.current_state = STATE_SETTINGS - self._refresh = True - self._auto_repeat_clear() - self._edit_setting = item - self._edit_setting_value = self._settings[item].v - - - def _menu_back_handler(self): - if self.current_menu == "main": - self.minimise() - # There are only two menus so this is the only other option - self.set_menu("main") - - -### BADGEBOT DEMO FUNCTIONALITY ### - - # multi level auto repeat - # if speed_up is True, the auto repeat gets faster the longer the button is held - # otherwise it is a fixed rate, but the level is used to determine the scale of the increase in the setttings inc() and dec() functions - def _auto_repeat_check(self, delta: int, speed_up: bool = True) -> bool: - 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._settings['logging'].v: - print(f"Auto Repeat Level: {self._auto_repeat_level}") - - return True - return False - - - def _auto_repeat_clear(self): - 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 - - - - - - - - - -######## STEPPER MOTOR CLASS ######## - -class Stepper: # External Driver, Speed Control Only via PWM - def __init__(self, container, pins, reverse = False, name: str = "", max_sps: int = _STEPPER_MAX_SPEED, max_pos: int = _STEPPER_MAX_POSITION): - self._container = container - self._name = name - self._calibrated = False - #self._timer = Timer(timer_id) - self._timer_is_running = False - self._timer_mode = 0 - self._pos = 0 # current position in steps - self._free_run_mode = 1 # direction of free run mode - self._enabled = False - self._reverse = reverse - self._max_sps_change = int(max_sps/10) # max change in speed in steps per second per update - self._max_sps = int(max_sps) # max speed in steps per second - self._steps_per_sec = 0 # current speed in steps per second - self._max_pos = int(max_pos) # max position stored in half steps - self._freq = 0 - - # Pins for external stepper driver - self._pins = pins - self._pins["en"].init(mode=Pin.OUT) - self._pins["en"].on() # active low - self._pins["dir"].init(mode=Pin.OUT) - self._pins["dir"].off() - self._pins["step"].init(mode=Pin.OUT) - self._pins["step"].off() - self._pins["stop"].init(mode=Pin.IN, pull=Pin.PULL_UP) - self._pins["stop"].irq(trigger=Pin.IRQ_FALLING, handler=self._hit_endstop) - - # Setup PWM output on the step pin - try: - self._pwm = PWM(self._pins["step"], freq=10, duty_ns=2000) # 0 Hz is invalid but 0 duty is allowed - except Exception as e: - print(f"{self._name} PWM failed:{e}") - - def speed(self,sps) -> bool: # speed in FULL steps per second - if self._free_run_mode == 1 and sps < 0: - self._free_run_mode = -1 - elif self._free_run_mode == -1 and sps > 0: - self._free_run_mode = 1 - speed_change_limited = False - if sps > 0: - if self._calibrated and self._pos >= self._max_pos: - # endstop reached - sps = 0 - else: - if sps > self._max_sps: - # limit speed - sps = self._max_sps - # limit acceleration by comparing the change in speed to the max acceleration - # if the change is greater than the max acceleration, limit the change to the max acceleration - if sps - self._steps_per_sec > self._max_sps_change: - sps = self._steps_per_sec + self._max_sps_change - speed_change_limited = True - elif sps - self._steps_per_sec < -self._max_sps_change: - sps = self._steps_per_sec - self._max_sps_change - speed_change_limited = True - else: - if self._pins["stop"].value() == 0 or (self._calibrated and self._pos <= 0): - # endstop reached - sps = 0 - else: - if sps < -self._max_sps: - # limit speed - sps = -self._max_sps - # limit acceleration by comparing the change in speed to the max acceleration - # if the change is greater than the max acceleration, limit the change to the max acceleration - if sps - self._steps_per_sec > self._max_sps_change: - sps = self._steps_per_sec + self._max_sps_change - speed_change_limited = True - elif sps - self._steps_per_sec < -self._max_sps_change: - sps = self._steps_per_sec - self._max_sps_change - speed_change_limited = True - self._steps_per_sec = int(sps) - self._update_timer(abs(self._steps_per_sec)) # steps per second - return speed_change_limited - - def get_speed(self) -> int: - return self._steps_per_sec - - # function to estimate the current position based on the speed and time since last update - def get_pos(self, delta) -> int: - steps = (self._steps_per_sec * delta) // 1000 - self._pos += steps - # Check if we have hit the end stop - if self._calibrated: - if self._pos < 0 and self._steps_per_sec < 0: - print(f"{self._name} s/w min endstop") - self.speed(0) - elif self._pos > self._max_pos and self._steps_per_sec > 0: - print(f"{self._name} s/w max endstop") - self.speed(0) - return self._pos - - def _hit_endstop(self, pin: Pin): - # double check the endstop is hit - # if not, ignore the interrupt - if pin.value() == 0: - print(f"{self._name} Endstop - hit") - if not self._calibrated: - self._calibrated = True - self._pos = 0 - # if we are still trying to move TOWARDS the endstop - if self._steps_per_sec < 0: - self.speed(0) - else: - print(f"{self._name} Endstop - false alarm") - - def _update_timer(self,freq): - if freq == 0: - #print(f"{self._name} Timer: 0Hz") - self._pins["en"].on() # disable the stepper - self._pwm.duty_ns(0) # stop the PWM (frequency of 0 is not allowed) - self._freq = 0 - elif freq != self._freq or self._free_run_mode != self._timer_mode: - try: - print(f"{self._name} Timer:{self._free_run_mode} {freq}Hz") - if self._free_run_mode>0: - self._pins["dir"].value(1 if self._reverse else 0) - self._pwm.freq(int(freq)) - self._pwm.duty_ns(2000) # minimum 1.9uS STEP pulse width for DRV8825 - self._pins["en"].off() # enable active low - elif self._free_run_mode<0: - self._pins["dir"].value(0 if self._reverse else 1) - self._pwm.freq(int(freq)) - self._pwm.duty_ns(2000) # minimum 1.9uS STEP pulse width for DRV8825 - self._pins["en"].off() # enable active low - else: - self._pins["en"].on() - self._pwm.duty_ns(0) # stop the PWM (frequency of 0 is not allowed) - self._freq = freq - self._timer_is_running=True - self._timer_mode = self._free_run_mode - except Exception as e: - print(f"{self._name} update_timer failed:{e}") - - - def stop(self): - self._update_timer(0) - - def enable(self,e = True): - self._enabled=e - self._pins["en"].value(not e) - try: - if e: - if self._free_run_mode!=0: - self._update_timer(abs(self._steps_per_sec)) # steps per second - else: - self._update_timer(0) - except Exception as e: - print(f"{self._name} enable failed:{e}") - - def is_enabled(self) -> bool: - return self._enabled - -########## END OF STEPPER CLASS ########## - -class HexDriveType: - def __init__(self, pid, vid = 0xCAFE, motors = 0, steppers = 0, servos = 0, name ="Unknown"): - self.vid = vid - self.pid = pid - self.name = name - self.motors = motors - self.servos = servos - self.steppers = steppers - - -class MySetting: - def __init__(self, container, default, minimum, maximum): - self._container = container - self.d = default - self.v = default - self._min = minimum - self._max = maximum - - - def __str__(self): - return str(self.v) - - - def _index(self): - for k,v in self._container.items(): - if v == self: - return k - return None - - - # This returns an increase in the value passed in - subject to max and with scale of increase depending on level - # based on the type of the setting - # it does not affect the current value of the setting - def inc(self, v, l=0): - 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 # round up to the next multiple of 10^l, being very careful not to cause big jumps when value was nearly at the next multiple - - if v > self._max: - v = self._max - elif isinstance(self.v, float): - # only float at present is brightness from 0.0 to 1.0 - v += 0.1 - if v > self._max: - v = self._max - elif self._container['logging'].v: - print(f"H:inc type: {type(self.v)}") - return v - - # This returns a decrease in the value passed in - subject to min and with scale of increase depending on level - # based on the type of the setting - # it does not affect the current value of the setting - def dec(self, v, l=0): - 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 # round down to the next multiple of 10^l - - if v < self._min: - v = self._min - elif isinstance(self.v, float): - # only float at present is brightness from 0.0 to 1.0 - v -= 0.1 - if v < self._min: - v = self._min - elif self._container['logging'].v: - print(f"H: dec type: {type(self.v)}") - return v - - - def persist(self): - # only save non-default settings to the settings store - try: - if self.v != self.d: - settings.set(f"xystage.{self._index()}", self.v) - else: - settings.set(f"xystage.{self._index()}", None) - except Exception as e: - print(f"H:Failed to persist setting {self._index()}: {e}") - -__app_export__ = XYStageApp From 7f3c31ae19707a80bc0f96efd18f690c4f7fd9a1 Mon Sep 17 00:00:00 2001 From: robotmad Date: Sat, 5 Oct 2024 13:42:19 +0100 Subject: [PATCH 13/16] coment out BLE --- app.py | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/app.py b/app.py index 6044121..8867065 100644 --- a/app.py +++ b/app.py @@ -1,6 +1,6 @@ import asyncio -import aioble -import bluetooth +#import aioble +#import bluetooth import os import time from math import cos, pi @@ -34,11 +34,11 @@ # See the following for generating UUIDs: # https://www.uuidgenerator.net/ -_BLE_SERVICE_UUID = bluetooth.UUID('19b10000-e8f2-537e-4f6c-d104768a1214') -_BLE_SENSOR_CHAR_UUID = bluetooth.UUID('19b10001-e8f2-537e-4f6c-d104768a1214') -_BLE_LED_UUID = bluetooth.UUID('19b10002-e8f2-537e-4f6c-d104768a1214') +#_BLE_SERVICE_UUID = bluetooth.UUID('19b10000-e8f2-537e-4f6c-d104768a1214') +#_BLE_SENSOR_CHAR_UUID = bluetooth.UUID('19b10001-e8f2-537e-4f6c-d104768a1214') +#_BLE_LED_UUID = bluetooth.UUID('19b10002-e8f2-537e-4f6c-d104768a1214') # How frequently to send advertising beacons. -_ADV_INTERVAL_MS = 250_000 +#_ADV_INTERVAL_MS = 250_000 # Hard coded to talk to EEPROMs on address 0x50 - because we know that is what is on the HexDrive Hexpansion @@ -312,11 +312,11 @@ def __init__(self): eventbus.on_async(RequestForegroundPushEvent, self._gain_focus, self) eventbus.on_async(RequestForegroundPopEvent, self._lose_focus, self) - ### Bluetooth ### + ### Bluetooth ### NOT WORKING YET # Register GATT server, the service and characteristics - self.ble_service = aioble.Service(_BLE_SERVICE_UUID) - self.sensor_characteristic = aioble.Characteristic(self.ble_service, _BLE_SENSOR_CHAR_UUID, read=True, notify=True) - self.led_characteristic = aioble.Characteristic(self.ble_service, _BLE_LED_UUID, read=True, write=True, notify=True, capture=True) + #self.ble_service = aioble.Service(_BLE_SERVICE_UUID) + #self.sensor_characteristic = aioble.Characteristic(self.ble_service, _BLE_SENSOR_CHAR_UUID, read=True, notify=True) + #self.led_characteristic = aioble.Characteristic(self.ble_service, _BLE_LED_UUID, read=True, write=True, notify=True, capture=True) # Register service(s) #aioble.register_services(self.ble_service) From 608114575e30e981c6b1a56c8047f9f223676a68 Mon Sep 17 00:00:00 2001 From: robotmad Date: Sun, 29 Dec 2024 17:43:54 +0000 Subject: [PATCH 14/16] line follower application --- linefollower.py | 2472 +++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 2472 insertions(+) create mode 100644 linefollower.py diff --git a/linefollower.py b/linefollower.py new file mode 100644 index 0000000..bc6061b --- /dev/null +++ b/linefollower.py @@ -0,0 +1,2472 @@ +import asyncio +#import aioble +#import bluetooth +import os +import time +from math import cos, pi +import ota +import settings +import vfs +from app_components.notification import Notification +from app_components.tokens import label_font_size, twentyfour_pt, clear_background, button_labels +from app_components import Menu +from events.input import BUTTON_TYPES, Button, Buttons, ButtonUpEvent +from frontboards.twentyfour import BUTTONS +from machine import I2C, Timer, Pin, disable_irq, enable_irq +from system.eventbus import eventbus +from system.hexpansion.events import (HexpansionInsertionEvent, + HexpansionRemovalEvent) +from system.hexpansion.header import HexpansionHeader, write_header, read_header +from system.hexpansion.util import get_hexpansion_block_devices +from system.hexpansion.config import HexpansionConfig +from system.patterndisplay.events import PatternDisable, PatternEnable +from system.scheduler import scheduler +from system.scheduler.events import (RequestForegroundPopEvent, + RequestForegroundPushEvent, + RequestStopAppEvent) + +from tildagonos import tildagonos + +import app + +from .utils import chain, draw_logo_animated, parse_version + +#import micropython +#micropython.alloc_emergency_exception_buf(100) + +# See the following for generating UUIDs: +# https://www.uuidgenerator.net/ +#_BLE_SERVICE_UUID = bluetooth.UUID('19b10000-e8f2-537e-4f6c-d104768a1214') +#_BLE_SENSOR_CHAR_UUID = bluetooth.UUID('19b10001-e8f2-537e-4f6c-d104768a1214') +#_BLE_LED_UUID = bluetooth.UUID('19b10002-e8f2-537e-4f6c-d104768a1214') +# How frequently to send advertising beacons. +#_ADV_INTERVAL_MS = 250_000 + + +# Hard coded to talk to EEPROMs on address 0x50 - because we know that is what is on the HexDrive Hexpansion +# makes it a lot more efficient than scanning the I2C bus for devices and working out what they are + +CURRENT_APP_VERSION = 6 # HEXDRIVE.PY Integer Version Number - checked against the EEPROM app.py version to determine if it needs updating + +_APP_VERSION = "0.1" # BadgeBot App Version Number + +# If you change the URL then you will need to regenerate the QR code +_QR_CODE = [0x1fcf67f, + 0x104cc41, + 0x174975d, + 0x1744e5d, + 0x175d45d, + 0x104ea41, + 0x1fd557f, + 0x001af00, + 0x04735f7, + 0x1070c97, + 0x1c23ae9, + 0x08ce9bd, + 0x1af3160, + 0x1270a80, + 0x1cc3549, + 0x097ef36, + 0x03ff5e9, + 0x1b18300, + 0x1b5a37f, + 0x0313b41, + 0x03f3d5d, + 0x078b65d, + 0x111e35d, + 0x0b57141, + 0x18bbd7f] + +# Screen positioning for movement sequence text +H_START = -63 +V_START = -58 +_BRIGHTNESS = 1.0 + + +# Line Follower +_NUM_LINE_SENSORS = 2 +_LINE_SENSOR_DEFAULT_THRESHOLD = 500 +_LINE_SENSOR_TIMEOUT = 2000 +FOLLOWER_HEXPANSION = 1 # Hexpansion slot for Line Follower Sensors - as it does not have an EEPROM to be detected automatically +FOLLOWER_SENSOR_SCAN_PERIOD = 25 # ms +DEFAULT_UPDATE_PERIOD = 50 # ms + +# Dedicated Pins +SENSOR_1_CTRL = 3 # hs pin (HSI) +SENSOR_1_SIGNAL = 2 # hs pin (HSH) +SENSOR_2_CTRL = 1 # hs pin (HSG) +SENSOR_2_SIGNAL = 0 # hs pin (HSF) + +# Motor Driver - Defaults +_MAX_POWER = 30000 +_POWER_STEP_PER_TICK = 7500 # effectively the acceleration + +# Servo Tester - Defaults +_SERVO_DEFAULT_STEP = 10 # us per step +_SERVO_DEFAULT_CENTRE = 1500 # us +_SERVO_DEFAULT_RANGE = 1000 # +/- 500us from centre +_SERVO_DEFAULT_RATE = 25 # *10us per s +_SERVO_DEFAULT_MODE = 0 # Off +_SERVO_DEFAULT_PERIOD = 20 # ms +_SERVO_MAX_RATE = 1000 # *10us per s +_SERVO_MIN_RATE = 1 # *10us per s +_SERVO_MAX_TRIM = 1000 # us +_MAX_SERVO_RANGE = 1400 # 1400us either side of centre (VERY WIDE) + +# Stepper Tester - Defaults +_STEPPER_MAX_SPEED = 200 # full steps per second +_STEPPER_MAX_POSITION = 3100 # full steps from one end to the other end +_STEPPER_DEFAULT_SPEED = 50 # full steps per second +_STEPPER_NUM_PHASES = 8 # half steps +_STEPPER_DEFAULT_SPR = 200 # full steps per revolution +_STEPPER_DEFAULT_STEP = 1 # half steps, (2 = full steps) + +# Timings +_TICK_MS = 10 # Smallest unit of change for power, in ms +_USER_DRIVE_MS = 50 # User specifed drive durations, in ms +_USER_TURN_MS = 20 # User specifed turn durations, in ms +_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 +_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 + + +# App states +STATE_INIT = -1 +STATE_WARNING = 0 +STATE_MENU = 1 +STATE_HELP = 2 +STATE_RECEIVE_INSTR = 3 +STATE_COUNTDOWN = 4 +STATE_RUN = 5 +STATE_DONE = 6 +STATE_CHECK = 7 # Checks for EEPROMs and HexDrives +STATE_DETECTED = 8 # Hexpansion ready for EEPROM initialisation +STATE_UPGRADE = 9 # Hexpansion ready for EEPROM upgrade +STATE_ERASE = 10 # Hexpansion ready for EEPROM erase +STATE_PROGRAMMING = 11 # Hexpansion EEPROM programming +STATE_REMOVED = 12 # Hexpansion removed +STATE_ERROR = 13 # Hexpansion error +STATE_MESSAGE = 14 # Message display +STATE_LOGO = 15 # Logo display +STATE_SERVO = 16 # Servo test +STATE_STEPPER = 17 # Stepper test +STATE_SETTINGS = 18 # Edit Settings +STATE_FOLLOWER = 19 # Line Follower + +# App states where user can minimise app +_MINIMISE_VALID_STATES = [0, 1, 7, 12, 13, 14, 15] +_LED_CONTROL_STATES = [0, 3, 4, 5, 6, 12, 13, 14, 15] + +# HexDrive Hexpansion constants +_EEPROM_ADDR = 0x50 +_EEPROM_NUM_ADDRESS_BYTES = 2 +_EEPROM_PAGE_SIZE = 32 +_EEPROM_TOTAL_SIZE = 64 * 1024 // 8 + +#Misceallaneous Settings +_LOGGING = False +_ERASE_SLOT = 0 # Slot for user to set if they want to erase EEPROMs on HexDrives + +# Main Menu Items +_main_menu_items = ["Line Follower","Motor Moves", "Stepper Test", "Servo Test", "Settings", "About","Exit"] +MENU_ITEM_LINE_FOLLOWER = 0 +MENU_ITEM_MOTOR_MOVES = 1 +MENU_ITEM_STEPPER_TEST = 2 +MENU_ITEM_SERVO_TEST = 3 +MENU_ITEM_SETTINGS = 4 +MENU_ITEM_ABOUT = 5 +MENU_ITEM_EXIT = 6 + +class StepperMode: + OFF = 0 + POSITION = 1 + SPEED = 2 + stepper_modes = ["OFF", "POSITION", "SPEED"] + + def __init__(self, mode = OFF): + self.mode = mode + + def set(self, mode): + self.mode = mode + + def inc(self): + self.mode = (self.mode + 1) % 3 + + def __eq__(self, other): + return self.mode == other + + def __str__(self): + return self.stepper_modes[self.mode] + + +class ServoMode: + OFF = 0 + TRIM = 1 + POSITION = 2 + SCANNING = 3 + servo_modes = ["OFF", "TRIM", "POSITION", "SCANNING"] + + def __init__(self, mode = OFF): + self.mode = mode + + def set(self, mode): + self.mode = mode + + def inc(self): + self.mode = (self.mode + 1) % 4 + + def __eq__(self, other): + return self.mode == other + + def __str__(self): + return self.servo_modes[self.mode] + + +class LineFollowerApp(app.App): + def __init__(self): + super().__init__() + # UI Button Controls + self.button_states = Buttons(self) + self.last_press: Button = BUTTON_TYPES["CANCEL"] + self.long_press_delta: int = 0 + 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 + + # UI Feature Controls + self._refresh: bool = True + self.rpm: float = 5 # logo rotation speed in RPM + self._animation_counter: float = 0 + self._pattern_status: bool = True # True = Pattern Enabled, False = Pattern Disabled + self.qr_code = _QR_CODE + self.b_msg: str = f"BadgeBot V{_APP_VERSION}" + self.t_msg: str = "RobotMad" + self.is_scroll: bool = False + self.scroll_offset: int = 0 + self.notification: Notification = None + self.error_message = [] + self.current_menu: str = None + self.menu: Menu = None + + # BadgeBot Control Sequence Variables + self.run_countdown_elapsed_ms: int = 0 + self.instructions = [] + self.current_instruction: Instruction = None + self.current_power_duration = ((0,0,0,0), 0) + self.power_plan_iter = iter([]) + + # Settings + self._settings = {} + self._settings['acceleration'] = MySetting(self._settings, _POWER_STEP_PER_TICK, 1, 65535) + self._settings['max_power'] = MySetting(self._settings, _MAX_POWER, 1000, 65535) + self._settings['drive_step_ms'] = MySetting(self._settings, _USER_DRIVE_MS, 5, 200) + self._settings['turn_step_ms'] = MySetting(self._settings, _USER_TURN_MS, 5, 200) + self._settings['servo_step'] = MySetting(self._settings, _SERVO_DEFAULT_STEP, 1, 100) + self._settings['servo_range'] = MySetting(self._settings, _SERVO_DEFAULT_RANGE, 100, _MAX_SERVO_RANGE) # one setting for all servos + self._settings['servo_period'] = MySetting(self._settings, _SERVO_DEFAULT_PERIOD, 5, 50) + self._settings['brightness'] = MySetting(self._settings, _BRIGHTNESS, 0.1, 1.0) + self._settings['logging'] = MySetting(self._settings, _LOGGING, False, True) + self._settings['erase_slot'] = MySetting(self._settings, _ERASE_SLOT, 0, 6) + self._settings['step_max_pos'] = MySetting(self._settings, _STEPPER_MAX_POSITION, 0, 65535) + self._settings['line_threshold'] = MySetting(self._settings, _LINE_SENSOR_DEFAULT_THRESHOLD, 0, 65535) + + self._edit_setting: int = None + self._edit_setting_value = None + self.update_settings() + + # Check what version of the Badge s/w we are running on + try: + ver = parse_version(ota.get_version()) + if ver is not None: + if self._settings['logging'].v: + print(f"BadgeSW V{ver}") + # Potential to do things differently based on badge s/w version + # e.g. if ver < [1, 9, 0]: + except: + pass + + # Hexpansion related + self._HEXDRIVE_TYPES = [HexDriveType(0xCBCB, motors=2, servos=4), + HexDriveType(0xCBCA, motors=2, name="2 Motor"), + HexDriveType(0xCBCC, servos=4, name="4 Servo"), + HexDriveType(0xCBCD, motors=1, servos=2, name="1 Mot 2 Srvo"), + HexDriveType(0xCBCE, steppers=1, name="Stepper")] + self.hexpansion_slot_type = [None]*6 + self.hexpansion_init_type: int = 0 + self.detected_port: int = None + self.waiting_app_port: int = None + self.erase_port: int = None + self.upgrade_port: int = None + self.hexdrive_port: int = None + self.ports_with_blank_eeprom = set() + self.ports_with_hexdrive = set() + self.ports_with_latest_hexdrive = set() + self.hexdrive_app = None + self.hexpansion_update_required: bool = False # flag from async to main loop + eventbus.on_async(HexpansionInsertionEvent, self._handle_hexpansion_insertion, self) + eventbus.on_async(HexpansionRemovalEvent, self._handle_hexpansion_removal, self) + self._time_since_last_update: int = 0 + + # Motor Driver + self.num_motors: int = 2 # Default assumed for a single HexDrive + self.num_steppers: int = 1 # Default assumed for a single HexDrive + self._stepper: Stepper = None + self.stepper_mode = StepperMode() + self.stepper_pos: int = 0 + + # Servo Tester + self._time_since_last_input: int = 0 + self._timeout_period: int = 120000 # ms (2 minutes - without any user input) + self._keep_alive_period: int = 500 # ms (half the value used in hexdrive.py) + self.num_servos: int = 4 # Default assumed for a single HexDrive + self.servo = [None]*4 # Servo Positions + self.servo_centre = [_SERVO_DEFAULT_CENTRE]*4 # Trim Servo Centre + self.servo_range = [_SERVO_DEFAULT_RANGE]*4 # Limit Servo Range + self.servo_rate = [_SERVO_DEFAULT_RATE]*4 # Servo Rate of Change + self.servo_mode = [ServoMode()]*4 # Servo Mode + self.servo_selected: int = 0 + + # Line Follower + self.num_line_sensors: int = _NUM_LINE_SENSORS + self._line_sensors = [None]*_NUM_LINE_SENSORS + self._hexpansion_config = HexpansionConfig(FOLLOWER_HEXPANSION) # There is no EEPROM on the Line Follower Sensor Hexpansion + + # Overall app state (controls what is displayed and what user inputs are accepted) + self.current_state = STATE_INIT + self.previous_state = self.current_state + self._update_period = DEFAULT_UPDATE_PERIOD # ms + + eventbus.on_async(RequestForegroundPushEvent, self._gain_focus, self) + eventbus.on_async(RequestForegroundPopEvent, self._lose_focus, self) + + ### Bluetooth ### NOT WORKING YET + # Register GATT server, the service and characteristics + #self.ble_service = aioble.Service(_BLE_SERVICE_UUID) + #self.sensor_characteristic = aioble.Characteristic(self.ble_service, _BLE_SENSOR_CHAR_UUID, read=True, notify=True) + #self.led_characteristic = aioble.Characteristic(self.ble_service, _BLE_LED_UUID, read=True, write=True, notify=True, capture=True) + # Register service(s) + #aioble.register_services(self.ble_service) + + # We start with focus on launch, without an event emmited + self._gain_focus(RequestForegroundPushEvent(self)) + + + ### ASYNC EVENT HANDLERS ### + + async def _handle_hexpansion_removal(self, event: HexpansionRemovalEvent): + self.hexpansion_slot_type[event.port-1] = None + if event.port in self.ports_with_blank_eeprom: + if self._settinfs['logging'].v: + print(f"H:EEPROM removed from port {event.port}") + self.ports_with_blank_eeprom.remove(event.port) + if event.port in self.ports_with_hexdrive: + if self._settings['logging'].v: + print(f"H:HexDrive removed from port {event.port}") + self.ports_with_hexdrive.remove(event.port) + if event.port in self.ports_with_latest_hexdrive: + if self._settings['logging'].v: + print(f"H:HexDrive V{_APP_VERSION} removed from port {event.port}") + self.ports_with_latest_hexdrive.remove(event.port) + if self.current_state == STATE_DETECTED and event.port == self.detected_port: + self.hexpansion_update_required = True + elif self.current_state == STATE_UPGRADE and event.port == self.upgrade_port: + self.hexpansion_update_required = True + elif self.hexdrive_port is not None and event.port == self.hexdrive_port: + self.hexpansion_update_required = True + elif self.waiting_app_port is not None and event.port == self.waiting_app_port: + self.hexpansion_update_required = True + elif self.erase_port is not None and event.port == self.erase_port: + self.hexpansion_update_required = True + + + async def _handle_hexpansion_insertion(self, event: HexpansionInsertionEvent): + if self.check_port_for_hexdrive(event.port): + self.hexpansion_update_required = True + + + async def _gain_focus(self, event: RequestForegroundPushEvent): + if event.app is self: + if self.current_state in _LED_CONTROL_STATES: + eventbus.emit(PatternDisable()) + elif self.current_state == STATE_RECEIVE_INSTR: + eventbus.on_async(ButtonUpEvent, self._handle_button_up, self) + + + async def _lose_focus(self, event: RequestForegroundPopEvent): + if event.app is self: + eventbus.emit(PatternEnable()) + self._pattern_status = True + if self.current_state == STATE_RECEIVE_INSTR: + eventbus.remove(ButtonUpEvent, self._handle_button_up, self) + + + async def _handle_button_up(self, event: ButtonUpEvent): + if self.current_state == STATE_RECEIVE_INSTR and event.button == BUTTONS["C"]: + self.is_scroll = not self.is_scroll + state = "yes" if self.is_scroll else "no" + self.notification = Notification(f"Scroll {state}") + + + async def background_task(self): + # Modiifed background task loop for shorter sleep time + 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(self._update_period) + last_time = cur_time + + + ### NON-ASYNC FUCNTIONS ### + + def background_update(self, delta: int): + if self.current_state == STATE_RUN: + # DC Motor Contorl + output = self.get_current_power_level(delta) + if output is None: + self.current_state = STATE_DONE + self._update_period = DEFAULT_UPDATE_PERIOD + elif self.hexdrive_app is not None: + self.hexdrive_app.set_motors(output) + elif self.current_state == STATE_FOLLOWER: + # Read the line sensors + for i in range(self.num_line_sensors): + if self._line_sensors[i] is not None: + self._line_sensors[i].read() + else: + print(f"Line Sensor {i} not initialised") + # Line Follower Sensor + if self._line_sensors[0].value() and not self._line_sensors[1].value(): + output = (-self._settings['max_power'].v, self._settings['max_power'].v) + elif not self._line_sensors[0].value() and self._line_sensors[1].value(): + output = (self._settings['max_power'].v, -self._settings['max_power'].v) + else: + output = (self._settings['max_power'].v, self._settings['max_power'].v) + if self.hexdrive_app is not None: + self.hexdrive_app.set_motors(output) + + + def generate_new_qr(self): + from .uQR import QRCode + qr = QRCode(error_correction=1, box_size=10, border=0) + qr.add_data("https://robotmad.odoo.com") + self.qr_code = qr.get_matrix() + # convert QR code made up of True/False into words of 1s and 0s + if 32 < len(self.qr_code): + print("QR code too big") + else: + qr_code_size = len(self.qr_code) + print("_QR_CODE = [") + for row in range(qr_code_size): + bitfield = 0x00000000 + for col in range(qr_code_size): + # LSBit is on the left + bitfield = bitfield | (1 << col) if self.qr_code[row][col] else bitfield + print(f"0x{bitfield:08x},") + print("]") + + + ### HEXPANSION FUNCTIONS ### + + # Scan the Hexpansion ports for EEPROMs and HexDrives in case they are already plugged in when we start + def scan_ports(self): + for port in range(1, 7): + self.check_port_for_hexdrive(port) + + + def check_port_for_hexdrive(self, port: int) -> bool: + # we know the EEPROM address so we can just read the header directly + if port not in range(1, 7): + return False + # We want to do this in two parts so that we detect if there is a valid EEPROM or not + try: + hexpansion_header = read_header(port, addr_len=_EEPROM_NUM_ADDRESS_BYTES) + except OSError: + # no EEPROM on this port + return False + except RuntimeError: + # not a valid header + if self._settings['logging'].v: + print(f"H:Found EEPROM on port {port}") + self.ports_with_blank_eeprom.add(port) + return True + # check is this is a HexDrive header by scanning the _HEXDRIVE_TYPES list + for index, hexpansion_type in enumerate(self._HEXDRIVE_TYPES): + if hexpansion_header.vid == hexpansion_type.vid and hexpansion_header.pid == hexpansion_type.pid: + if self._settings['logging'].v: + print(f"H:Found '{hexpansion_type.name}' HexDrive on port {port}") + if port not in self.ports_with_latest_hexdrive: + self.ports_with_hexdrive.add(port) + self.hexpansion_slot_type[port-1] = index + return True + # we are not interested in this type of hexpansion + return False + + + def update_app_in_eeprom(self, port: int, addr: int) -> bool: + # Copy hexdrive.mpy to EEPROM as app.mpy + if self._settings['logging'].v: + print(f"H:Updating HexDrive app.mpy on port {port}") + try: + i2c = I2C(port) + except Exception as e: + print(f"H:Error opening I2C port {port}: {e}") + return False + header = read_header(port, addr_len = _EEPROM_NUM_ADDRESS_BYTES) + if header is None: + if self._settings['logging'].v: + print(f"H:Error reading header on port {port}") + return False + try: + _, partition = get_hexpansion_block_devices(i2c, header, addr, addr_len = _EEPROM_NUM_ADDRESS_BYTES) + except RuntimeError as e: + print(f"H:Error getting block devices: {e}") + return False + mountpoint = '/hexpansion_' + str(port) + already_mounted = False + if not already_mounted: + if self._settings['logging'].v: + print(f"H:Mounting {partition} at {mountpoint}") + try: + vfs.mount(partition, mountpoint, readonly=False) + except OSError as e: + if e.args[0] == 1: + already_mounted = True + else: + print(f"H:Error mounting: {e}") + except Exception as e: + print(f"H:Error mounting: {e}") + source_path = "/" + __file__.rsplit("/", 1)[0] + "/hexdrive.mpy" + dest_path = f"{mountpoint}/app.mpy" + try: + # delete the existing app.mpy file + if self._settings['logging'].v: + print(f"H:Deleting {dest_path}") + os.remove(dest_path) + except Exception as e: + if e.args[0] != 2: + # ignore errors which will happen if the file does not exist + print(f"H:Error deleting {dest_path}: {e}") + + if self._settings['logging'].v: + print(f"H:Copying {source_path} to {dest_path}") + + try: + template = open(source_path, "rb") + except Exception as e: + print(f"H:Error opening {source_path}: {e}") + return False + + try: + appfile = open(dest_path, "wb") + except Exception as e: + print(f"H:Error opening {dest_path}: {e}") + return False + + try: + appfile.write(template.read()) + except Exception as e: + print(f"H:Error updating HexDrive: {e}") + return False + + try: + appfile.close() + template.close() + except Exception as e: + print(f"H:Error closing files: {e}") + return False + if not already_mounted: + try: + vfs.umount(mountpoint) + if self._settings['logging'].v: + print(f"H:Unmounted {mountpoint}") + except Exception as e: + print(f"H:Error unmounting {mountpoint}: {e}") + return False + if self._settings['logging'].v: + print(f"H:HexDrive app.mpy updated to version {CURRENT_APP_VERSION}") + return True + + + def prepare_eeprom(self, port: int, addr: int) -> bool: + if self._settings['logging'].v: + print(f"H:Initialising EEPROM on port {port}") + hexdrive_header = HexpansionHeader( + manifest_version="2024", + fs_offset=32, + eeprom_page_size=_EEPROM_PAGE_SIZE, + eeprom_total_size=_EEPROM_TOTAL_SIZE, + vid=self._HEXDRIVE_TYPES[self.hexpansion_init_type].vid, + pid=self._HEXDRIVE_TYPES[self.hexpansion_init_type].pid, + unique_id=0x0, + friendly_name="HexDrive", + ) + # Write and read back header efficiently + try: + i2c = I2C(port) + except Exception as e: + print(f"H:Error opening I2C port {port}: {e}") + return False + try: + write_header(port, hexdrive_header, addr_len = _EEPROM_NUM_ADDRESS_BYTES, page_size = _EEPROM_PAGE_SIZE) + except Exception as e: + print(f"H:Error writing header: {e}") + return False + try: + hexpansion_header = read_header(port, addr_len = _EEPROM_NUM_ADDRESS_BYTES) + except Exception as e: + print(f"H:Error reading header back: {e}") + return False + try: + # Get block devices + _, partition = get_hexpansion_block_devices(i2c, hexpansion_header, addr, addr_len = _EEPROM_NUM_ADDRESS_BYTES) + except RuntimeError as e: + print(f"H:Error getting block devices: {e}") + return False + try: + # Format + vfs.VfsLfs2.mkfs(partition) + if self._settings['logging'].v: + print("H:EEPROM formatted") + except Exception as e: + print(f"H:Error formatting: {e}") + return False + try: + # And mount! + mountpoint = '/hexpansion_' + str(port) + vfs.mount(partition, mountpoint, readonly=False) + if self._settings['logging'].v: + print("H:EEPROM initialised") + except OSError as e: + if e.args[0] == 1: + #already_mounted + if self._settings['logging'].v: + print("H:EEPROM initialised") + else: + print(f"H:Error mounting: {e}") + return False + except Exception as e: + print(f"H:Error mounting: {e}") + return False + return True + + + def erase_eeprom(self, port: int, addr: int) -> bool: + if self._settings['logging'].v: + print(f"H:Erasing EEPROM on port {port}") + try: + i2c = I2C(port) + # loop through all pages and erase them + for page in range(_EEPROM_TOTAL_SIZE // _EEPROM_PAGE_SIZE): + mem_addr = page * _EEPROM_PAGE_SIZE + #generate a bit mask for the address based on the number of address bytes + mem_addr_mask = 1<<(_EEPROM_NUM_ADDRESS_BYTES*8)-1 + i2c.writeto_mem((addr | (mem_addr >> (8*_EEPROM_NUM_ADDRESS_BYTES))), (mem_addr & mem_addr_mask), bytes([0xFF]*_EEPROM_PAGE_SIZE), addrsize = (8*_EEPROM_NUM_ADDRESS_BYTES)) + # check Ack + while True: + try: # Poll Ack + if i2c.writeto((addr | (mem_addr >> (8*_EEPROM_NUM_ADDRESS_BYTES))), bytes([mem_addr & 0xFF]) if _EEPROM_NUM_ADDRESS_BYTES == 1 else bytes([mem_addr >> 8, mem_addr & 0xFF])): + break + except OSError: + pass + finally: + time.sleep_ms(1) + except Exception as e: + print(f"H:Error erasing EEPROM: {e}") + return False + return True + + + def find_hexdrive_app(self, port: int) -> app: + for an_app in scheduler.apps: + if type(an_app).__name__ is 'HexDriveApp': + if hasattr(an_app, "config") and hasattr(an_app.config, "port") and an_app.config.port == port: + return an_app + return None + + + def update_settings(self): + for s in self._settings: + self._settings[s].v = settings.get(f"badgebot.{s}", self._settings[s].d) + + + def _pattern_management(self): + if self.current_state in _LED_CONTROL_STATES: + if self._pattern_status: + eventbus.emit(PatternDisable()) + self._pattern_status = False + # delay enough to allow the pattern to stop + time.sleep_ms(500) + elif self.current_state not in _LED_CONTROL_STATES and not self._pattern_status: + eventbus.emit(PatternEnable()) + self._pattern_status = True + + + ### MAIN APP CONTROL FUNCTIONS ### + + def update(self, delta: int): + if self.notification: + self.notification.update(delta) + + # manage PatternEnable/Disable for all states + self._pattern_management() + + self._update_hexpansion_management(delta) + self._update_main_application(delta) + + if self.current_state != self.previous_state: + if self._settings['logging'].v: + print(f"State: {self.previous_state} -> {self.current_state}") + self.previous_state = self.current_state + # manage PatternEnable/Disable for all states + self._pattern_management() + # something has changed - so worth redrawing + self._refresh = True + + if self.current_state in _LED_CONTROL_STATES: + if self._settings['brightness'].v < 1.0: + # Scale brightness + for i in range(1,13): + tildagonos.leds[i] = tuple(int(j * self._settings['brightness'].v) for j in tildagonos.leds[i]) + tildagonos.leds.write() + + + ### START UI FOR HEXPANSION INITIALISATION AND UPGRADE ### + + def _update_hexpansion_management(self, delta: int): + if self.current_state == STATE_INIT: + # One Time initialisation + self.scan_ports() + if (len(self.ports_with_hexdrive) == 0) and (len(self.ports_with_blank_eeprom) == 0): + # There are currently no possible HexDrives plugged in + self._animation_counter = 0 + self.current_state = STATE_WARNING + else: + self.current_state = STATE_CHECK + return + + if self.hexpansion_update_required: + # something has changed in the hexpansion ports + self.hexpansion_update_required = False + if self.current_state != STATE_CHECK: + print("H:Hexpansion Check") + self.set_menu(None) + self.current_state = STATE_CHECK + + if self.current_state == STATE_WARNING or self.current_state == STATE_LOGO: + self._update_state_warning(delta) + elif self.current_state == STATE_ERROR or self.current_state == STATE_MESSAGE or self.current_state == STATE_REMOVED: + self._update_state_error(delta) + elif self.current_state == STATE_PROGRAMMING: + # Programming the Hexpansion + self._update_state_programming(delta) + elif self.current_state == STATE_DETECTED: + # We have detected a Hexpansion with a blank EEPROM - asking the user if they want to initialise it + self._update_state_detected(delta) + elif self.current_state == STATE_ERASE: + self._update_state_erase(delta) + elif self.current_state == STATE_UPGRADE: + # We are currently asking the user if they want hexpansion App upgrading with latest App.mpy + self._update_state_upgrade(delta) + elif self.current_state in _MINIMISE_VALID_STATES: + if self._check_hexpansion_ports(delta): + pass + elif self._check_hexdrive_ports(delta): + pass + elif self.current_state == STATE_CHECK: + self._update_state_check(delta) + + + def _update_main_application(self, delta: int): + if self.current_state == STATE_MENU: + if self.current_menu is None: + self.set_menu("main") + self._refresh = True + else: + self.menu.update(delta) + if self.menu.is_animating != "none": + if self._settings['logging'].v: + print("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.is_scroll = False + self.minimise() + + ### Motor Moves Application ### + elif self.current_state == STATE_HELP: + self._update_state_help(delta) + elif self.current_state == STATE_RECEIVE_INSTR: + self._update_state_receive_instr(delta) + elif self.current_state == STATE_COUNTDOWN: + self._update_state_countdown(delta) + elif self.current_state == STATE_RUN: + self.clear_leds() + # Run is primarily managed in the background update + elif self.current_state == STATE_DONE: + self._update_state_done(delta) + + ### Line Follower Application ### + elif self.current_state == STATE_FOLLOWER: + self._update_state_follower(delta) + + ### Servo Tester Application ### + elif self.current_state == STATE_SERVO: + self._update_state_servo(delta) + + ### Stepper Tester Application ### + elif self.current_state == STATE_STEPPER: + self._update_state_stepper(delta) + + ### Settings Capability ### + elif self.current_state == STATE_SETTINGS: + self._update_state_settings(delta) + ### End of Update ### + + + def _update_state_warning(self, delta: int): + if self.button_states.get(BUTTON_TYPES["CONFIRM"]): + self.button_states.clear() + if self.current_state == STATE_WARNING or self.hexdrive_port is not None: + # Warning has been acknowledged by the user + self._animation_counter = 0 + self.current_state = STATE_MENU # allow access to settings and About + else: + # Return to Warning screen from Logo when no HexDrive is present + self.current_state = STATE_WARNING + else: + # "CANCEL" button is handled below in common for all MINIMISE_VALID_STATES + # Show the warning screen for 10 seconds + self._animation_counter += delta/1000 + self._refresh = True + if self.current_state == STATE_WARNING and self._animation_counter > 10: + # after 10 seconds show the logo + self._animation_counter = 0 + self.current_state = STATE_LOGO + elif self.current_state == STATE_LOGO: + # LED management - to match rotating logo: + for i in range(1,13): + colour = (255, 241, 0) # custom Robotmad shade of yellow + # raised cosine cubed wave + wave = self._settings['brightness'].v * pow((1.0 + cos(((i) * pi / 1.5) - (self.rpm * self._animation_counter * pi / 7.5)))/2.0, 3) + # 4 sides each projecting a pattern of 3 LEDs (12 LEDs in total) + tildagonos.leds[i] = tuple(int(wave * j) for j in colour) + else: # STATE_WARNING + for i in range(1,13): + tildagonos.leds[i] = (255,0,0) + + + def _update_state_error(self, delta: int): + if self.button_states.get(BUTTON_TYPES["CONFIRM"]): + # Error has been acknowledged by the user + self.button_states.clear() + self.current_state = STATE_CHECK + self.error_message = [] + else: + for i in range(1,13): + tildagonos.leds[i] = (0,255,0) if self.current_state == STATE_MESSAGE else (255,0,0) + + + def _update_state_programming(self, delta: int): + if self.upgrade_port is not None: + if self.update_app_in_eeprom(self.upgrade_port, _EEPROM_ADDR): + self.notification = Notification("Upgraded", port = self.upgrade_port) + #self.ports_with_latest_hexdrive.add(self.upgrade_port) + # Try to trigger hexpansion managment app to restart the HexDrive + # by emit hexpansion insertion event + eventbus.emit(HexpansionInsertionEvent(self.upgrade_port)) + self.error_message = ["Upgraded:","Please","reboop"] + self.current_state = STATE_MESSAGE + if self._settings['logging'].v: + print(f"H:HexDrive on port {self.upgrade_port} upgraded") + else: + self.notification = Notification("Failed", port = self.upgrade_port) + self.error_message = ["HexDrive","programming","failed"] + self.current_state = STATE_ERROR + self.upgrade_port = None + elif self.detected_port is not None: + if self.prepare_eeprom(self.detected_port, _EEPROM_ADDR): + self.notification = Notification("Initialised", port = self.detected_port) + self.upgrade_port = self.detected_port + self.hexpansion_slot_type[self.detected_port-1] = self.hexpansion_init_type + self.current_state = STATE_UPGRADE + else: + self.notification = Notification("Failed", port = self.detected_port) + self.error_message = ["EEPROM","initialisation","failed"] + self.hexpansion_slot_type[self.detected_port-1] = None + self.current_state = STATE_ERROR + self.detected_port = None + elif self._settings['logging'].v: + print("H:Error - no port to program") + + + def _update_state_detected(self, delta: int): + # We are currently asking the user if they want hexpansion EEPROM initialising + if self.button_states.get(BUTTON_TYPES["CONFIRM"]): + self.button_states.clear() + self.current_state = STATE_PROGRAMMING + elif self.button_states.get(BUTTON_TYPES["CANCEL"]): + self.button_states.clear() + if self._settings['logging'].v: + print("H:Initialise Cancelled") + self.detected_port = None + self.current_state = STATE_CHECK + elif self.button_states.get(BUTTON_TYPES["UP"]): + self.button_states.clear() + self.hexpansion_init_type = (self.hexpansion_init_type + 1) % len(self._HEXDRIVE_TYPES) + self._refresh = True + elif self.button_states.get(BUTTON_TYPES["DOWN"]): + self.button_states.clear() + self.hexpansion_init_type = (self.hexpansion_init_type - 1) % len(self._HEXDRIVE_TYPES) + self._refresh = True + elif self.button_states.get(BUTTON_TYPES["LEFT"]): + self.button_states.clear() + self.hexpansion_init_type = 1 + self._refresh = True + elif self.button_states.get(BUTTON_TYPES["RIGHT"]): + self.button_states.clear() + self.hexpansion_init_type = 2 + self._refresh = True + + + def _update_state_erase(self, delta: int): + # We are currently asking the user if they want hexpansion EEPROM Erased + if self.button_states.get(BUTTON_TYPES["CONFIRM"]): + # Yes + self.button_states.clear() + if self.erase_eeprom(self.erase_port, _EEPROM_ADDR): + self.error_message = ["Erased:","Please","reboop"] + self.notification = Notification("Erased", port = self.erase_port) + self.erase_port = None + self.current_state = STATE_MESSAGE + else: + self.notification = Notification("Failed", port = self.erase_port) + self.error_message = ["EEPROM","erasure","failed"] + self.current_state = STATE_ERROR + elif self.button_states.get(BUTTON_TYPES["CANCEL"]): + # No + if self._settings['logging'].v: + print("H:Erase Cancelled") + self.button_states.clear() + self.erase_port = None + self.current_state = STATE_CHECK + + + def _update_state_upgrade(self, delta: int): + if self.button_states.get(BUTTON_TYPES["CONFIRM"]): + # Yes + self.button_states.clear() + self.notification = Notification("Upgrading", port = self.upgrade_port) + self.current_state = STATE_PROGRAMMING + elif self.button_states.get(BUTTON_TYPES["CANCEL"]): + # No + if self._settings['logging'].v: + print("H:Upgrade Cancelled") + self.button_states.clear() + self.upgrade_port = None + self.current_state = STATE_CHECK + + + def _update_state_check(self, delta: int): + #print(f"Check: {self.ports_with_latest_hexdrive}") + if 0 < len(self.ports_with_latest_hexdrive): + # We have at least one HexDrive with the latest App.mpy + if self.hexdrive_port is not None and self.hexdrive_port not in self.ports_with_latest_hexdrive: + print(f"Check: {self.hexdrive_port} lost") + self.hexdrive_port = None + self.hexdrive_app = None + if self.hexdrive_port is None: + valid_port = next(iter(self.ports_with_latest_hexdrive)) + # Find our running hexdrive app + hexdrive_app = self.find_hexdrive_app(valid_port) + if hexdrive_app is not None: + self.hexdrive_port = valid_port + self.hexdrive_app = hexdrive_app + if self.hexpansion_slot_type[valid_port-1] is not None: + self.num_motors = self._HEXDRIVE_TYPES[self.hexpansion_slot_type[valid_port-1]].motors + self.num_servos = self._HEXDRIVE_TYPES[self.hexpansion_slot_type[valid_port-1]].servos + self.num_steppers = self._HEXDRIVE_TYPES[self.hexpansion_slot_type[valid_port-1]].steppers + # only intended for use with a single active HexDrive at once at present + if (0 < self._HEXDRIVE_TYPES[self.hexpansion_slot_type[valid_port-1]].steppers) or self.hexdrive_app.get_status(): + if self._settings['logging'].v: + print(f"H:HexDrive [{valid_port}] OK") + self.current_state = STATE_MENU + self._animation_counter = 0 + else: + if self._settings['logging'].v: + print(f"H:HexDrive {valid_port}: Failed to initialise PWM resources") + self.error_message = [f"HexDrive {valid_port}","PWM Init","Failed","Please","Reboop"] + self.current_state = STATE_ERROR + else: + if self._settings['logging'].v: + print(f"H:HexDrive {valid_port}: App not found, please reboop") + self.error_message = [f"HexDrive {valid_port}","App not found.","Please","reboop"] + self.current_state = STATE_ERROR + else: + # Still have hexdrive on original port + self.current_state = STATE_MENU + elif self.hexdrive_port is not None: + print(f"Check: {self.hexdrive_port} lost") + self.hexdrive_port = None + self.hexdrive_app = None + self.current_state = STATE_REMOVED + else: + self._animation_counter = 0 + self.current_state = STATE_WARNING + + + def _check_hexpansion_ports(self, delta: int) -> bool: + if 0 < len(self.ports_with_blank_eeprom): + # if there are any ports with blank eeproms + # Show the UI prompt and wait for button press + self.detected_port = self.ports_with_blank_eeprom.pop() + self.notification = Notification("Initialise?", port = self.detected_port) + self.current_state = STATE_DETECTED + return True + return False + + + def _check_hexdrive_ports(self, delta: int) -> bool: + #print(f"Check HexDrive Ports: {self.waiting_app_port} {self.ports_with_hexdrive}") + if self.waiting_app_port is not None or (0 < len(self.ports_with_hexdrive)): + # if there are any ports with HexDrives - check if they need upgrading/erasing + if self.waiting_app_port is None: + self.waiting_app_port = self.ports_with_hexdrive.pop() + self._animation_counter = 0 #timeout + if self._settings['erase_slot'].v == self.waiting_app_port: + # if the user has set a port to erase EEPROMs on + # Show the UI prompt and wait for button press + if self._settings['logging'].v: + print(f"H:HexDrive on port {self.waiting_app_port} Erase?") + self.erase_port = self.waiting_app_port + self.notification = Notification("Erase?", port = self.erase_port) + self.current_state = STATE_ERASE + else: + hexdrive_app = self.find_hexdrive_app(self.waiting_app_port) + # the scheduler is updated asynchronously from hexpansion insertion so we may not find the app immediately + if hexdrive_app is not None: + try: + hexdrive_app_version = hexdrive_app.get_version() + except Exception as e: + hexdrive_app_version = 0 + print(f"H:Error getting HexDrive app version - assume old: {e}") + elif 5.0 < self._animation_counter: + if self._settings['logging'].v: + print("H:Timeout waiting for HexDrive app to be started - assume it needs upgrading") + hexdrive_app_version = 0 + else: + if 0 == self._animation_counter: + if self._settings['logging'].v: + print(f"H:No app found on port {self.waiting_app_port} - WAITING for app to appear in Scheduler") + self.notification = Notification("Checking...", port = self.waiting_app_port) + self._animation_counter += delta/1000 + return True + if hexdrive_app_version == CURRENT_APP_VERSION: + if self._settings['logging'].v: + print(f"H:HexDrive on port {self.waiting_app_port} has latest App") + self.ports_with_latest_hexdrive.add(self.waiting_app_port) + self.current_state = STATE_CHECK + else: + # Show the UI prompt and wait for button press + if self._settings['logging'].v: + print(f"H:HexDrive on port {self.waiting_app_port} needs upgrading from version {hexdrive_app_version}") + self.upgrade_port = self.waiting_app_port + self.notification = Notification("Upgrade?", port = self.upgrade_port) + self.current_state = STATE_UPGRADE + self.waiting_app_port = None + self._animation_counter = 0 + return True + return False + + + def _update_state_help(self, delta: int): + if self.button_states.get(BUTTON_TYPES["CANCEL"]): + self.button_states.clear() + self.current_state = STATE_MENU + elif self.button_states.get(BUTTON_TYPES["CONFIRM"]): + self.button_states.clear() + self.is_scroll = True # so that release of this button will CLEAR Scroll mode + eventbus.on_async(ButtonUpEvent, self._handle_button_up, self) + self.current_state = STATE_RECEIVE_INSTR + else: + # Show the help for 10 seconds + self._animation_counter += delta/1000 + if self._animation_counter > 10: + # after 10 seconds show the logo + self._animation_counter = 0 + self.current_state = STATE_LOGO + + + def _update_state_receive_instr(self, delta: int): + # Enable/disable scrolling and check for long press + if self.button_states.get(BUTTON_TYPES["CONFIRM"]): + self.long_press_delta += delta + if self.long_press_delta >= _LONG_PRESS_MS: + # if there are no steps saved in the power plan then return to HELP, otherwise go to COUNTDOWN + if self.power_plan_iter is None: + self.current_state = STATE_HELP + else: + self.finalize_instruction() + self.current_state = STATE_COUNTDOWN + self.is_scroll = False + eventbus.remove(ButtonUpEvent, self._handle_button_up, self) + else: + # Confirm is not pressed. Reset long_press state + self.long_press_delta = 0 + if self.button_states.get(BUTTON_TYPES["CANCEL"]): + self.button_states.clear() + self._animation_counter = 0 + self.is_scroll = False + self.current_state = STATE_HELP + eventbus.remove(ButtonUpEvent, self._handle_button_up, self) + return + # Manage scrolling + if self.is_scroll: + if self.button_states.get(BUTTON_TYPES["DOWN"]): + self.button_states.clear() + self.scroll_offset -= 1 + self._refresh = True + elif self.button_states.get(BUTTON_TYPES["UP"]): + self.button_states.clear() + self.scroll_offset += 1 + self._refresh = True + # Instruction button presses + elif self.button_states.get(BUTTON_TYPES["RIGHT"]): + self._handle_instruction_press(BUTTON_TYPES["RIGHT"]) + self.button_states.clear() + self._set_direction_leds(BUTTON_TYPES["RIGHT"]) + self._refresh = True + elif self.button_states.get(BUTTON_TYPES["LEFT"]): + self._handle_instruction_press(BUTTON_TYPES["LEFT"]) + self.button_states.clear() + self._set_direction_leds(BUTTON_TYPES["LEFT"]) + self._refresh = True + elif self.button_states.get(BUTTON_TYPES["UP"]): + self._handle_instruction_press(BUTTON_TYPES["UP"]) + self.button_states.clear() + self._set_direction_leds(BUTTON_TYPES["UP"]) + self._refresh = True + elif self.button_states.get(BUTTON_TYPES["DOWN"]): + self._handle_instruction_press(BUTTON_TYPES["DOWN"]) + self.button_states.clear() + self._set_direction_leds(BUTTON_TYPES["DOWN"]) + self._refresh = True + else: + self._set_direction_leds(self.last_press) + + + def _set_direction_leds(self, direction: Button): + if direction == BUTTON_TYPES["RIGHT"]: + # Green = Starboard = Right + self.clear_leds() + tildagonos.leds[2] = (0, 255, 0) + tildagonos.leds[3] = (0, 255, 0) + elif direction ==BUTTON_TYPES["LEFT"]: + # Red = Port = Left + self.clear_leds() + tildagonos.leds[8] = (255, 0, 0) + tildagonos.leds[9] = (255, 0, 0) + elif direction == BUTTON_TYPES["UP"]: + # Cyan + self.clear_leds() + tildagonos.leds[12] = (0, 255, 255) + tildagonos.leds[1] = (0, 255, 255) + elif direction == BUTTON_TYPES["DOWN"]: + # Magenta + self.clear_leds() + tildagonos.leds[6] = (255, 0, 255) + tildagonos.leds[7] = (255, 0, 255) + + + def _update_state_countdown(self, delta: int): + self.clear_leds() + self.run_countdown_elapsed_ms += delta + if self.run_countdown_elapsed_ms >= _RUN_COUNTDOWN_MS: + self.power_plan_iter = chain(*(instr.power_plan for instr in self.instructions)) + if self.hexdrive_app is not None: + self.hexdrive_app.set_power(True) + self.current_state = STATE_RUN + self._update_period = 10 + + + def _update_state_done(self, delta: int): + if self.button_states.get(BUTTON_TYPES["CANCEL"]): + self.button_states.clear() + if self.hexdrive_app is not None: + self.hexdrive_app.set_power(False) + self.reset_robot() + elif self.button_states.get(BUTTON_TYPES["CONFIRM"]): + self.button_states.clear() + if self.hexdrive_app is not None: + self.hexdrive_app.set_power(False) + self.run_countdown_elapsed_ms = 1 # avoid "6" appearing on screen at all + self.current_power_duration = ((0,0,0,0), 0) + self.current_state = STATE_COUNTDOWN + + # Stepper Tester: + def _update_state_stepper(self, delta: int): + # Left/Right to adjust position + if self.button_states.get(BUTTON_TYPES["RIGHT"]): + if self._auto_repeat_check(delta, True): + if self.stepper_mode == StepperMode.SPEED: # Speed + speed = self._stepper.get_speed() + speed = self._inc(speed, self._auto_repeat_level+1) + if _STEPPER_MAX_SPEED < speed: + speed = _STEPPER_MAX_SPEED + self._stepper.speed(speed) + else: + if self.stepper_mode != StepperMode.POSITION: # Position Mode + self.stepper_mode.set(StepperMode.POSITION) + self._stepper.speed(_STEPPER_DEFAULT_SPEED) + self._stepper.track_target() + pos = self._stepper.get_pos() + pos = self._inc(pos, self._auto_repeat_level+1) + self._stepper.target(pos) + self._refresh = True + elif self.button_states.get(BUTTON_TYPES["LEFT"]): + if self._auto_repeat_check(delta, True): + if self.stepper_mode == StepperMode.SPEED: # Speed + speed = self._stepper.get_speed() + speed = self._dec(speed, self._auto_repeat_level+1) + if -_STEPPER_MAX_SPEED > speed: + speed = -_STEPPER_MAX_SPEED + self._stepper.speed(speed) + else: # Position Mode + if self.stepper_mode != StepperMode.POSITION: + self.stepper_mode.set(StepperMode.POSITION) + self._stepper.speed(_STEPPER_DEFAULT_SPEED) + self._stepper.track_target() + pos = self._stepper.get_pos() + pos = self._dec(pos, self._auto_repeat_level+1) + self._stepper.target(pos) + self._refresh = True + else: + self._auto_repeat_clear() + # non auto-repeating buttons + if self.button_states.get(BUTTON_TYPES["CANCEL"]): + self.button_states.clear() + if self.hexdrive_app is not None: + self._stepper.enable(False) + self.current_state = STATE_MENU + return + elif self.button_states.get(BUTTON_TYPES["CONFIRM"]): #Cycle Through Modes + self.button_states.clear() + self.stepper_mode.inc() + if self.stepper_mode == StepperMode.POSITION: # Position Mode + self._stepper.speed(_STEPPER_DEFAULT_SPEED) + self._stepper.target(self._stepper.get_pos()) + self._stepper.track_target() + elif self.stepper_mode == StepperMode.SPEED: # Speed Mode + self._stepper.speed(0) + self._stepper.free_run(1) + else: # Off + self._stepper.stop() + self._refresh = True + self.notification = Notification(f" Stepper:\n {self.stepper_mode}") + print(f"Stepper:{self.stepper_mode}") + if self._refresh: + self._time_since_last_input = 0 + else: + self._time_since_last_input += delta + if self._time_since_last_input > self._timeout_period: + self._stepper.stop() + self._stepper.speed(0) + self._stepper.enable(False) + self.current_state = STATE_MENU + self.notification = Notification(" Stepper:\n Timeout") + print("Stepper:Timeout") + elif self.stepper_mode == StepperMode.SPEED: # Speed Mode + self._refresh = True + self._time_since_last_update += delta + if self._time_since_last_update > self._keep_alive_period: + self._stepper.step() + self._time_since_last_update = 0 + + def _update_state_follower(self, delta: int): + # Line Follower: + # Cancel to exit + if self.button_states.get(BUTTON_TYPES["CANCEL"]): + self.button_states.clear() + if self.hexdrive_app is not None: + self.hexdrive_app.set_power(False) + for i in range(self.num_line_sensors): + self._line_sensors[i].disable() + self._update_period = DEFAULT_UPDATE_PERIOD + self.current_state = STATE_MENU + return + # Does the line sensor data need refreshing on the display? + for i in range(self.num_line_sensors): + if self._line_sensors[i].updated: + self._refresh = True + self._line_sensors[i].updated = False + + + def _update_state_servo(self, delta: int): + # Servo Tester: + # Up/Down to select Servo + # Left/Right to adjust position + if self.button_states.get(BUTTON_TYPES["RIGHT"]): + if self._auto_repeat_check(delta, (self.servo_mode[self.servo_selected] != ServoMode.SCANNING)): + if self.servo_mode[self.servo_selected] == ServoMode.TRIM: + # adjust the servo centre position + self.servo_centre[self.servo_selected] += self._settings['servo_step'].v + if self.servo_centre[self.servo_selected] > (_SERVO_DEFAULT_CENTRE + _SERVO_MAX_TRIM): + self.servo_centre[self.servo_selected] = _SERVO_DEFAULT_CENTRE + _SERVO_MAX_TRIM + if self.hexdrive_app is not None: + if not self.hexdrive_app.set_servocentre(self.servo_centre[self.servo_selected], self.servo_selected): + print("H:Failed to set servo centre") + elif self.servo_mode[self.servo_selected] == ServoMode.SCANNING: + # as the rate changes sign when it reaches the range, we must be careful to modify it in the correct direction + if self.servo_rate[self.servo_selected] < 0: + negative = True + rate = -self.servo_rate[self.servo_selected] + else: + negative = False + rate = self.servo_rate[self.servo_selected] + rate = self._inc(rate, self._auto_repeat_level) + if _SERVO_MAX_RATE < rate: + rate = _SERVO_MAX_RATE + if negative: + self.servo_rate[self.servo_selected] = -rate + else: + self.servo_rate[self.servo_selected] = rate + else: # Position Mode + if self.servo[self.servo_selected] is None: + self.servo[self.servo_selected] = 0 + self.servo_mode[self.servo_selected].set(ServoMode.POSITION) + self.servo[self.servo_selected] += self._settings['servo_step'].v + if self.servo[self.servo_selected] is not None: + if self.servo_range[self.servo_selected] < (self.servo[self.servo_selected] + (self.servo_centre[self.servo_selected] - _SERVO_DEFAULT_CENTRE)): + self.servo[self.servo_selected] = self.servo_range[self.servo_selected] - (self.servo_centre[self.servo_selected] - _SERVO_DEFAULT_CENTRE) + self._refresh = True + elif self.button_states.get(BUTTON_TYPES["LEFT"]): + if self._auto_repeat_check(delta, (self.servo_mode[self.servo_selected] != ServoMode.SCANNING)): + if self.servo_mode[self.servo_selected] == ServoMode.TRIM: + # adjust the servo centre position + self.servo_centre[self.servo_selected] -= self._settings['servo_step'].v + if self.servo_centre[self.servo_selected] < (_SERVO_DEFAULT_CENTRE - _SERVO_MAX_TRIM): + self.servo_centre[self.servo_selected] = _SERVO_DEFAULT_CENTRE - _SERVO_MAX_TRIM + if self.hexdrive_app is not None: + if not self.hexdrive_app.set_servocentre(self.servo_centre[self.servo_selected], self.servo_selected): + print("H:Failed to set servo centre") + elif self.servo_mode[self.servo_selected] == ServoMode.SCANNING: + # as the rate changes sign when it reaches the range, we must be careful to modify it in the correct direction + if self.servo_rate[self.servo_selected] < 0: + negative = True + rate = -self.servo_rate[self.servo_selected] + else: + negative = False + rate = self.servo_rate[self.servo_selected] + rate = self._dec(rate, self._auto_repeat_level) + if _SERVO_MIN_RATE > rate: + rate = _SERVO_MIN_RATE + if negative: + self.servo_rate[self.servo_selected] = -rate + else: + self.servo_rate[self.servo_selected] = rate + else: # Position Mode + if self.servo[self.servo_selected] is None: + self.servo[self.servo_selected] = 0 + self.servo_mode[self.servo_selected].set(ServoMode.POSITION) + self.servo[self.servo_selected] -= self._settings['servo_step'].v + if self.servo[self.servo_selected] is not None: + if -self.servo_range[self.servo_selected] > (self.servo[self.servo_selected] + (self.servo_centre[self.servo_selected] - _SERVO_DEFAULT_CENTRE)): + self.servo[self.servo_selected] = -self.servo_range[self.servo_selected] - (self.servo_centre[self.servo_selected] - _SERVO_DEFAULT_CENTRE) + self._refresh = True + else: + self._auto_repeat_clear() + # non auto-repeating buttons + if self.button_states.get(BUTTON_TYPES["UP"]): + self.button_states.clear() + self.servo_selected = (self.servo_selected - 1) % self.num_servos + self._refresh = True + elif self.button_states.get(BUTTON_TYPES["DOWN"]): + self.button_states.clear() + self.servo_selected = (self.servo_selected + 1) % self.num_servos + self._refresh = True + elif self.button_states.get(BUTTON_TYPES["CANCEL"]): + self.button_states.clear() + if self.hexdrive_app is not None: + self.hexdrive_app.set_power(False) + self.hexdrive_app.set_servoposition() # All Off + self.current_state = STATE_MENU + return + elif self.button_states.get(BUTTON_TYPES["CONFIRM"]): #Cycle Through Modes + self.button_states.clear() + self.servo_mode[self.servo_selected].inc() + if self.servo_mode[self.servo_selected] == ServoMode.OFF: + if self.hexdrive_app is not None: + self.hexdrive_app.set_servoposition(self.servo_selected, None) + else: + self._refresh = True + self.notification = Notification(f" Servo {self.servo_selected}:\n {self.servo_mode[self.servo_selected]}") + + if self._refresh: + self._time_since_last_input = 0 + else: + self._time_since_last_input += delta + if self._time_since_last_input > self._timeout_period: + if self.hexdrive_app is not None: + self.hexdrive_app.set_power(False) + self.hexdrive_app.set_servoposition() # All Off + self.current_state = STATE_MENU + self.notification = Notification(" Servo:\n Timeout") + + self._time_since_last_update += delta + if self._time_since_last_update > self._keep_alive_period: + self._time_since_last_update = 0 + self._refresh = True + + for i in range(self.num_servos): + _refresh = self._refresh + if self.servo_mode[i] == ServoMode.SCANNING: + # for any servo set to Scan mode, update the position + if self.servo[self.servo_selected] is None: + self.servo[self.servo_selected] = 0 + self.servo[i] = self.servo[i] + (10 * self.servo_rate[i] * delta / 1000) + if self.servo_range[i] < (self.servo[i] + (self.servo_centre[i] - _SERVO_DEFAULT_CENTRE)): + # swap direction + self.servo_rate[i] = -self.servo_rate[i] + self.servo[i] = self.servo_range[i] - (self.servo_centre[i] - _SERVO_DEFAULT_CENTRE) + elif -self.servo_range[i] > (self.servo[i] + (self.servo_centre[i] - _SERVO_DEFAULT_CENTRE)): + # swap direction + self.servo_rate[i] = -self.servo_rate[i] + self.servo[i] = -self.servo_range[i] - (self.servo_centre[i] - _SERVO_DEFAULT_CENTRE) + _refresh = True + if _refresh and self.hexdrive_app is not None and self.servo_mode[i] != ServoMode.OFF and self.servo[i] is not None: + # scanning servo or the selected servo + self.hexdrive_app.set_servoposition(i, int(self.servo[i])) + + + def _update_state_settings(self, delta: int): + 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._settings['logging'].v: + print(f"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._settings['logging'].v: + print(f"Setting: {self._edit_setting} (-) Value: {self._edit_setting_value}") + self._refresh = True + else: + # non auto-repeating buttons + self._auto_repeat_clear() + if self.button_states.get(BUTTON_TYPES["RIGHT"]) or self.button_states.get(BUTTON_TYPES["LEFT"]): + self.button_states.clear() + # Force default value + self._edit_setting_value = self._settings[self._edit_setting].d + if self._settings['logging'].v: + print(f"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() + # leave setting unchanged + if self._settings['logging'].v: + print(f"Setting: {self._edit_setting} Cancelled") + self.set_menu(_main_menu_items[MENU_ITEM_SETTINGS]) + self.current_state = STATE_MENU + elif self.button_states.get(BUTTON_TYPES["CONFIRM"]): + self.button_states.clear() + # set setting + if self._settings['logging'].v: + print(f"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.set_menu(_main_menu_items[MENU_ITEM_SETTINGS]) + self.current_state = STATE_MENU + + + def draw(self, ctx): + if self._refresh or self.notification is not None: + self._refresh = False + clear_background(ctx) + ctx.save() + 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 + if self.current_state == STATE_LOGO: + draw_logo_animated(ctx, self.rpm, self._animation_counter, [self.b_msg, self.t_msg], self.qr_code) + # Scroll mode indicator + elif self.is_scroll: + ctx.rgb(0,0.2,0).rectangle( -120,-120, 115+H_START,240).fill() + ctx.rgb(0,0 ,0).rectangle(H_START-5,-120,10-2*H_START,240).fill() + ctx.rgb(0,0.2,0).rectangle(5-H_START,-120, 115+H_START,240).fill() + else: + ctx.rgb(0,0,0).rectangle(-120,-120,240,240).fill() + # Main screen content + if self.current_state == STATE_WARNING: + self.draw_message(ctx, ["BadgeBot requires","HexDrive hexpansion","from RobotMad","github.com","/TeamRobotmad","/BadgeBot"], [(1,1,1),(1,1,0),(1,1,0),(1,1,1),(1,1,1),(1,1,1)], label_font_size) + elif self.current_state == STATE_REMOVED: + self.draw_message(ctx, ["HexDrive","removed.","Please reinsert"], [(1,1,0),(1,1,1),(1,1,1)], label_font_size) + elif self.current_state == STATE_DETECTED: + hexdrive_type = self._HEXDRIVE_TYPES[self.hexpansion_init_type].name + self.draw_message(ctx, ["Hexpansion",f"in slot {self.detected_port}:","Init EEPROM as",hexdrive_type,"HexDrive?"], [(1,1,1),(1,1,1),(1,1,1),(0,0,1),(1,1,0)], label_font_size) + button_labels(ctx, confirm_label="Yes", up_label="^", down_label="\u25BC", left_label=self._HEXDRIVE_TYPES[1].name, right_label=self._HEXDRIVE_TYPES[2].name, cancel_label="No") + elif self.current_state == STATE_ERASE: + self.draw_message(ctx, ["HexDrive",f"in slot {self.erase_port}:","Erase EEPROM?"], [(1,1,0),(1,1,1),(1,0,0)], label_font_size) + button_labels(ctx, confirm_label="Yes", cancel_label="No") + elif self.current_state == STATE_UPGRADE: + self.draw_message(ctx, ["HexDrive",f"in slot {self.upgrade_port}:","Upgrade","HexDrive app?"], [(1,1,0),(1,1,1),(1,1,1),(1,1,1)], label_font_size) + button_labels(ctx, confirm_label="Yes", cancel_label="No") + elif self.current_state == STATE_PROGRAMMING: + self.draw_message(ctx, ["HexDrive:","Programming","EEPROM","Please wait..."], [(1,1,0),(1,1,1),(1,1,1),(1,1,1)], label_font_size) + elif self.current_state == STATE_HELP: + self.draw_message(ctx, ["BadgeBot","To program:","Press C","When finished:","Long press C"], [(1,1,0),(1,1,1),(1,1,1),(1,1,1),(1,1,1)], label_font_size) + elif self.current_state == STATE_ERROR: + self.draw_message(ctx, self.error_message, [(1,0,0)]*len(self.error_message), label_font_size) + elif self.current_state == STATE_MESSAGE: + self.draw_message(ctx, self.error_message, [(0,1,0)]*len(self.error_message), label_font_size) + elif self.current_state == STATE_RECEIVE_INSTR: + self._draw_receive_instr(ctx) + # button labels clash with the instruction list - so not shown + #button_labels(ctx, confirm_label="Scroll", up_label="Fwd", down_label="Rev", left_label="Left", right_label="Right", cancel_label="Cancel") + elif self.current_state == STATE_COUNTDOWN: + countdown_val = 1 + ((_RUN_COUNTDOWN_MS - self.run_countdown_elapsed_ms) // 1000) + self.draw_message(ctx, [str(countdown_val)], [(1,1,0)], twentyfour_pt) + elif self.current_state == STATE_RUN: + # convert current_power_duration to string, dividing all four values down by 655 (to get a value from 0-100) + current_power, _ = self.current_power_duration + power_str = str(tuple([int(x/(self._settings['max_power'].v//100)) for x in current_power])) + self.draw_message(ctx, ["Running...",power_str], [(1,1,1),(1,1,0)], label_font_size) + elif self.current_state == STATE_DONE: + #self.draw_message(ctx, ["Program","complete!","Replay:Press C","Restart:Press F"], [(0,1,0),(0,1,0),(1,1,0),(0,1,1)], label_font_size) + self.draw_message(ctx, ["Program","complete!"], [(0,1,0),(0,1,0)], label_font_size) + button_labels(ctx, confirm_label="Replay", cancel_label="Restart") + elif self.current_state == STATE_FOLLOWER: + self._draw_state_follower(ctx) + elif self.current_state == STATE_SERVO: + self._draw_state_servo(ctx) + elif self.current_state == STATE_STEPPER: + self._draw_state_stepper(ctx) + elif self.current_state == STATE_SETTINGS: + self.draw_message(ctx, ["Edit Setting",f"{self._edit_setting}:",f"{self._edit_setting_value}"], [(1,1,1),(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") + ctx.restore() + + # These need to be drawn every frame as they contain animations + if self.current_state == STATE_MENU: + clear_background(ctx) + self.menu.draw(ctx) + + if self.notification: + self.notification.draw(ctx) + + + def _draw_receive_instr(self, ctx): + # Display list of movements + for i_num, instr in enumerate(["START"] + self.instructions + [self.current_instruction, "END"]): + # map the instruction to a colour & change language from up/down to fwd/rev + colour = (1,1,1) + if instr is not None: + direction = str(instr).split()[0] + #if self._settings['logging'].v: + # print(direction) + if direction == "UP": + instr = "FWD " + str(instr).split()[1] + colour = (0,1,1) + elif direction == "DOWN": + instr = "REV " + str(instr).split()[1] + colour = (1,0,1) + elif direction == "LEFT": + colour = (1,0,0) + elif direction == "RIGHT": + colour = (0,1,0) + elif direction == "START" or direction == "END": + colour = (0.5,0.5,0.5) + ctx.rgb(*colour).move_to(H_START, V_START + label_font_size * (self.scroll_offset + i_num)).text(str(instr)) + + + def _draw_state_stepper(self, ctx): + stepper_text = ["S"]*(1+self.num_steppers) # Servo Text + stepper_text_colours = [(0.4,0.0,0.0)]*(1+self.num_steppers) # Red + stepper_text[0] = "Stepper Test" + stepper_text_colours[0] = (1,1,1) # Title - White + if self._stepper is not None: + i = 0 + # Select Colour according to mode + if self.stepper_mode == StepperMode.OFF: + body_colour = (0.2,0.2,0.2) # Not activated - Grey + bar_colour = (0.4,0.4,0.4) # Not activated - Grey + else: + body_colour = (0.1,0.1,0.5) # Active - Blue + bar_colour = (0.1,0.1,1.0) # Active - Blue + stepper_text_colours[1] = (0.4,0.4,0.0) # Active - Yellow + + # draw the servo positions + ctx.save() + # y = i-1.5 for 4 servos, y = i-0.5 for 2 servos + ctx.translate(0, (i-(self.num_steppers/2)+0.5) * label_font_size) + # background for the servo position - grey + background_colour = (0.15,0.15,0.15) + ctx.rgb(*background_colour).rectangle(-100,1,200,label_font_size-2).fill() + c = 0 + # draw the stepper position (based on a centre halfway through the range) + x = 200 * (self._stepper.get_pos() / self._settings['step_max_pos'].v) - 100 + # vertical bar at stepper position + ctx.rgb(*bar_colour).rectangle(x-2,1,5,label_font_size-2).fill() + # horizontal bar from 0 to stepper position, not covering the centre marker or the stepper position bar + ctx.rgb(*body_colour) + if x > (c+4): + ctx.rectangle(c+1, 3, x-c-4, label_font_size-6).fill() + elif x < (c-4): + ctx.rectangle(x+4, 3, c-x-4, label_font_size-6).fill() + # marker for the centre - black (drawn last as it may have to go through the servo position bar) + ctx.rgb(0,0,0).move_to(c,0).line_to(c,label_font_size).stroke() + ctx.restore() + if self.stepper_mode == StepperMode.SPEED: # Speed + stepper_text[i+1] = f"{int(self._stepper.get_speed()):4}/s" # Speed in steps per second + else: # Position + stepper_text[i+1] = "Off" if (self.stepper_mode == StepperMode.OFF) else f"{int(self._stepper.get_pos()):+6} " + self.draw_message(ctx, stepper_text, stepper_text_colours, label_font_size) + button_labels(ctx, confirm_label="Mode", cancel_label="Exit", left_label="<--", right_label="-->") + + + def _draw_state_follower(self, ctx): + # Line Follower + print("d") + # draw the two line follower sensor values on/off as green/white circles + ctx.save() + for i in range(self.num_line_sensors): + #ctx.translate(0, (i-0.5) * label_font_size) + x = 40 - i * 80 + colour = (0,1,0) if self._line_sensors[i].value() else (0,0,0) + ctx.rgb(*colour).arc(x, 0, 30, 0, 2 * pi, True).fill() + ctx.rgb(1,1,1).arc(x, 0, 31, 0, 2 * pi, True).stroke() + ctx.restore() + + def _draw_state_servo(self, ctx): + servo_text = ["S"]*(1+self.num_servos) # Servo Text + servo_text_colours = [(0.4,0.0,0.0)]*(1+self.num_servos) # Red + servo_text[0] = "Servo Test" + servo_text_colours[0] = (1,1,1) # Title - White + for i in range(self.num_servos): + + # Select Colour according to mode + if self.servo[i] is None or self.servo_mode[i] == ServoMode.OFF: + body_colour = (0.2,0.2,0.2) # Not activated - Grey + bar_colour = (0.4,0.4,0.4) # Not activated - Grey + elif self.servo_mode[i] == ServoMode.SCANNING: + body_colour = (0.1,0.5,0.1) # Scanning - Green + bar_colour = (0.1,1.0,0.1) # Scanning - Green + servo_text_colours[1+i] = (0.4,0.0,0.4) # Scanning - Magenta + else: + body_colour = (0.1,0.1,0.5) # Active - Blue + bar_colour = (0.1,0.1,1.0) # Active - Blue + servo_text_colours[1+i] = (0.4,0.4,0.0) # Active - Yellow + + # draw the servo positions + ctx.save() + # y = i-1.5 for 4 servos, y = i-0.5 for 2 servos + ctx.translate(0, (i-(self.num_servos/2)+0.5) * label_font_size) + # background for the servo position - grey + background_colour = (0.1,0.1,0.1) if i != self.servo_selected else (0.15,0.15,0.15) + ctx.rgb(*background_colour).rectangle(-100,1,200,label_font_size-2).fill() + c = 100 * (self.servo_centre[i]-_SERVO_DEFAULT_CENTRE) / self.servo_range[i] + if self.servo[i] is not None: + #TODO refactor this into a reusable function for drawing sliders + # draw the servo position + x = 100 * (self.servo[i] + self.servo_centre[i] - _SERVO_DEFAULT_CENTRE) / self.servo_range[i] + + # vertical bar at servo position + ctx.rgb(*bar_colour).rectangle(x-2,1,5,label_font_size-2).fill() + # horizontal bar from 0 to servo position, not covering the centre marker or the servo position bar + ctx.rgb(*body_colour) + if x > (c+4): + ctx.rectangle(c+1, 3, x-c-4, label_font_size-6).fill() + elif x < (c-4): + ctx.rectangle(x+4, 3, c-x-4, label_font_size-6).fill() + # marker for the centre - black (drawn last as it may have to go through the servo position bar) + ctx.rgb(0,0,0).move_to(c,0).line_to(c,label_font_size).stroke() + ctx.restore() + if self.servo_mode[i] == ServoMode.SCANNING: + servo_text[i+1] = f"{int(abs(self.servo_rate[i])):4}/s" # Scanning Rate + else: # Position + servo_text[i+1] = "Off" if (self.servo[i] is None or self.servo_mode[i] == ServoMode.OFF) else f"{int(self.servo[i]):+5} " + # Selected Servo - Brighter Text + servo_text_colours[1+self.servo_selected] = tuple(int(j * 2.5) for j in servo_text_colours[1+self.servo_selected]) + self.draw_message(ctx, servo_text, servo_text_colours, label_font_size) + if self.servo_mode[self.servo_selected] == ServoMode.SCANNING: + # Scanning mode + button_labels(ctx, up_label="^", down_label="\u25BC", confirm_label="Mode", cancel_label="Exit", left_label="Slower", right_label="Faster") + elif self.servo_mode[self.servo_selected] == ServoMode.TRIM: + button_labels(ctx, up_label="^", down_label="\u25BC", confirm_label="Mode", cancel_label="Exit", left_label="Trim-", right_label="+Trim") + else: + #Position mode + button_labels(ctx, up_label="^", down_label="\u25BC", confirm_label="Mode", cancel_label="Exit", left_label="<--", right_label="-->") + # NB characters \u25B2, \u25C0, \u25BA, \u21A9, \u2611 do not exist, so ii seems \u25BC has been included as a special case + + + # Value increment/decrement functions for positive integers only + def _inc(self, v: int, l: int): + if l==0: + return v+1 + else: + d = 10**l + v = ((v // d) + 1) * d # round up to the next multiple of 10^l + return v + + def _dec(self, v: int, l: int): + if l==0: + return v-1 + else: + d = 10**l + v = (((v+(9*(10**(l-1)))) // d) - 1) * d # round down to the next multiple of 10^l + return v + + + def clear_leds(self): + for i in range(1,13): + tildagonos.leds[i] = (0, 0, 0) + + + def draw_message(self, ctx, message, colours, size=label_font_size): + 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 reset_servo(self): + # re-initialise the servo range for the servos + if self.hexdrive_app is not None: + self.hexdrive_app.set_power(True) + self.hexdrive_app.set_freq(1000 // self._settings['servo_period'].v) + # initialise the 4 servos + for i in range(4): + if self.hexdrive_app is not None: # Apply Trim + self.hexdrive_app.set_servocentre(self.servo_centre[self.servo_selected], self.servo_selected) + + # update the servo range in case settigns have changed + self.servo_range[i] = self._settings['servo_range'].v # only 1 setting actually for all servos at present + # check that the current position is within the new range + if self.servo[i] is not None: + if self.servo[i] > self.servo_range[i]: + self.servo[i] = self.servo_range[i] + elif self.servo[i] < -self.servo_range[i]: + self.servo[i] = -self.servo_range[i] + # leave the servo positions etc... as they are. but turn them back on + if self.hexdrive_app is not None: + self.hexdrive_app.set_servoposition(i, int(self.servo[i])) + # leave the servo modes as they are + self.servo_selected = 0 + self._time_since_last_update = 0 + self._time_since_last_input = 0 + + + +### MENU FUNCTIONALITY ### + + + def set_menu(self, menu_name = "main"): #: Literal["main"]): does it work without the type hint? + if self._settings['logging'].v: + print(f"H:Set Menu {menu_name}") + if self.menu is not None: + try: + self.menu._cleanup() + 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() + if self.num_servos == 0: + menu_items.remove(_main_menu_items[MENU_ITEM_SERVO_TEST]) + if self.num_steppers == 0: + menu_items.remove(_main_menu_items[MENU_ITEM_STEPPER_TEST]) + if self.num_motors == 0: + menu_items.remove(_main_menu_items[MENU_ITEM_MOTOR_MOVES]) + menu_items.remove(_main_menu_items[MENU_ITEM_LINE_FOLLOWER]) + if self.num_line_sensors == 0: + menu_items.remove(_main_menu_items[MENU_ITEM_LINE_FOLLOWER]) + self.menu = Menu( + self, + menu_items, + select_handler=self._main_menu_select_handler, + back_handler=self._menu_back_handler, + ) + elif menu_name == "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._settings['logging'].v: + print(f"H:Main Menu {item} at index {idx}") + if item == _main_menu_items[MENU_ITEM_LINE_FOLLOWER]: # Line Follower + if self.num_motors == 0: + self.notification = Notification("No Motors") + elif self.num_motors == 1: + self.notification = Notification(" 2 Motors Required") + else: + self.set_menu(None) + self.button_states.clear() + self._animation_counter = 0 + for i in range(self.num_line_sensors): + if self._line_sensors[i] is None: + # Pins + pins = {} + pins["ctrl"] = self._hexpansion_config.pin[SENSOR_1_CTRL if i == 0 else SENSOR_2_CTRL] + pins["sig"] = self._hexpansion_config.pin[SENSOR_1_SIGNAL if i == 0 else SENSOR_2_SIGNAL] + self._line_sensors[i] = LineSensor(self, pins, name = "Left" if i == 0 else "Right", threshold = self._settings['line_threshold'].v) + self._line_sensors[i].enable() + if self.hexdrive_app is not None: + if not self.hexdrive_app.set_power(True): + print("Failed to enable HexDrive power") + else: + print("No HexDrive App") + self.current_state = STATE_FOLLOWER + self._update_period = FOLLOWER_SENSOR_SCAN_PERIOD + self._refresh = True + elif item == _main_menu_items[MENU_ITEM_MOTOR_MOVES]: # Motor Test - Turtle/Logo mode + if self.num_motors == 0: + self.notification = Notification("No Motors") + elif self.num_motors == 1: + self.notification = Notification(" 2 Motors Required") + else: + self.set_menu(None) + self.button_states.clear() + self._animation_counter = 0 + self.current_state = STATE_HELP + self._refresh = True + elif item == _main_menu_items[MENU_ITEM_STEPPER_TEST]: # Stepper Test + if self.num_steppers == 0: + self.notification = Notification("No Steppers") + else: + if self._stepper is None: + # try timer IDs 0-3 until one is free + for i in range(4): + try: + self._stepper = Stepper(self, self.hexdrive_app, step_size=1, timer_id=i, max_pos=self._settings['step_max_pos'].v) + break + except: + pass + if self._stepper is None: + self.notification = Notification("No Free Timers") + else: + self.set_menu(None) + self.button_states.clear() + self.current_state = STATE_STEPPER + self._refresh = True + self._auto_repeat_clear() + self._stepper.enable(True) + self._time_since_last_input = 0 + elif item == _main_menu_items[MENU_ITEM_SERVO_TEST]: # Servo Test + if self.num_servos == 0: + self.notification = Notification("No Servos") + else: + self.set_menu(None) + self.button_states.clear() + self.reset_servo() + self.current_state = STATE_SERVO + self._refresh = True + self._auto_repeat_clear() + 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._animation_counter = 0 + self.current_state = STATE_LOGO + self._refresh = True + elif item == _main_menu_items[MENU_ITEM_EXIT]: # Exit + eventbus.remove(HexpansionInsertionEvent, self._handle_hexpansion_insertion, self) + eventbus.remove(HexpansionRemovalEvent, self._handle_hexpansion_removal, self) + 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._settings['logging'].v: + print(f"H:Setting {item} @ {idx}") + if idx == 0: #Save + if self._settings['logging'].v: + print("H:Settings Save All") + settings.save() + self.notification = Notification(" Settings Saved") + self.set_menu("main") + elif idx == 1: #Default + if self._settings['logging'].v: + print("H: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("main") + else: + self.set_menu(None) + self.button_states.clear() + self.current_state = STATE_SETTINGS + self._refresh = True + self._auto_repeat_clear() + self._edit_setting = item + self._edit_setting_value = self._settings[item].v + + + def _menu_back_handler(self): + if self.current_menu == "main": + self.minimise() + # There are only two menus so this is the only other option + self.set_menu("main") + + +### BADGEBOT DEMO FUNCTIONALITY ### + + def _handle_instruction_press(self, press_type: Button): + if self.last_press == press_type: + self.current_instruction.inc() + else: + self.finalize_instruction() + self.current_instruction = Instruction(press_type) + self.last_press = press_type + + + # multi level auto repeat + # if speed_up is True, the auto repeat gets faster the longer the button is held + # otherwise it is a fixed rate, but the level is used to determine the scale of the increase in the setttings inc() and dec() functions + def _auto_repeat_check(self, delta: int, speed_up: bool = True) -> bool: + 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._settings['logging'].v: + print(f"Auto Repeat Level: {self._auto_repeat_level}") + + return True + return False + + + def _auto_repeat_clear(self): + 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 + + + def reset_robot(self): + self.current_state = STATE_HELP + self.last_press = BUTTON_TYPES["CONFIRM"] + self._animation_counter = 0 + self.long_press_delta = 0 + self.is_scroll = False + self.scroll_offset = 0 + self.run_countdown_elapsed_ms = 0 + self.instructions = [] + self.current_instruction = None + self.current_power_duration = ((0,0), 0) + self.power_plan_iter = iter([]) + + + def get_current_power_level(self, delta: int) -> int: + # takes in delta as ms since last call + # if delta was > 10... what to do + if delta >= _TICK_MS: + delta = _TICK_MS-1 + + current_power, current_duration = self.current_power_duration + + updated_duration = current_duration - delta + if updated_duration <= 0: + try: + next_power, next_duration = next(self.power_plan_iter) + except StopIteration: + # returns None when complete + return None + next_duration += updated_duration + self.current_power_duration = next_power, next_duration + return next_power + else: + self.current_power_duration = current_power, updated_duration + return current_power + + + def finalize_instruction(self): + if self.current_instruction is not None: + self.current_instruction.make_power_plan(self._settings) + self.instructions.append(self.current_instruction) + if len(self.instructions) >= 5: + self.scroll_offset -= 1 + self.current_instruction = None + + + + +######## LINE SENSOR CLASS ######## + +class LineSensor: + def __init__(self, container, pins, name: str = "LineSensor", threshold: int = _LINE_SENSOR_DEFAULT_THRESHOLD): + try: + self._container = container + self._threshold = threshold + self._state = False + self._name = name + self._start_time = 0 + self.updated = False + + # Pins for hardware + self._pins = pins + self._pins["ctrl"].init(mode=Pin.OUT) + self._pins["ctrl"].off() + self._pins["sig"].init(mode=Pin.IN, pull=Pin.PULL_UP) + #self._pins["sig"].irq(trigger=Pin.IRQ_FALLING, handler=self._handler) + + except Exception as e: + print(f"{self._name} Init failed:{e}") + + def disable(self): + # tidy up + self._pins["ctrl"].off() + self._pins["sig"].irq(handler=None) + + def enable(self): + self._start_time = 0 + #self._pins["sig"].irq(trigger=Pin.IRQ_FALLING, handler=self._handler) + + # Read the sensor - if there is a reflection the value is 1, otherwise with black or no surface it is 0 + def read(self) -> bool: + # Set Signal to Output and High on SENSOR_1_CTRL + # Delay 10us + # Set Signal to Input + # Read Signal by timing how long it takes to go low + # Compare the duration against the threshold + if self._start_time != 0: + # already in progress + # if the last start time was more than 10ms ago then the sensor is not active so allow new reading to be made + if time.ticks_diff(time.ticks_us(), self._start_time) > 10000: + pass + else: + # print a message to the console INCLUDING the name of the sensor + print(f"Sensor {self._name} already in progress") + return False + else: + self.updated = False + self._pins["ctrl"].on() + self._pins["sig"].init(mode=Pin.OUT) + self._pins["sig"].on() + time.sleep_us(10) + # configure the pin as an input and wait for it to go low + self._pins["sig"].irq(trigger=Pin.IRQ_FALLING, handler=self._handler) + state = disable_irq() + self._start_time = time.ticks_us() + self._pins["sig"].init(mode=Pin.IN, pull=None) + enable_irq(state) + return True + + def value(self) -> bool: + return self._state + + def _handler(self, pin): + now = time.ticks_us() + self._pins["ctrl"].off() + self._pins["sig"].irq(handler=None) + # This is the interrupt handler for the line sensor + # read the time and compare to the start time + # if the time is less than the threshold then the sensor is active + if self._start_time == 0: + print(f"Sensor {self._name} spurious interrupt") + # not expecting an interrupt + return + prev_state = self._state + self._state = True if time.ticks_diff(now, self._start_time) < self._threshold else False + if (prev_state != self._state): + print("u") + self.updated = True + self._start_time = 0 + + +######## STEPPER MOTOR CLASS ######## + +class Stepper: + def __init__(self, container, hexdrive_app, step_size: int = 1, steps_per_rev: int = _STEPPER_DEFAULT_SPR, speed_sps: int = _STEPPER_DEFAULT_SPEED, max_sps: int = _STEPPER_MAX_SPEED, max_pos: int = _STEPPER_MAX_POSITION, timer_id: int = 0): + self._container = container + self._hexdrive_app = hexdrive_app + self._phase = 0 + self._calibrated = False + self._timer = Timer(timer_id) + self._timer_is_running = False + self._timer_mode = 0 + self._free_run_mode = 0 # direction of free run mode + self._enabled = False + self._target_pos = 0 + self._pos = 0 # current position in half steps + self._max_sps = int(max_sps) # max speed in full steps per second + self._steps_per_sec = int(speed_sps) # current speed in full steps per second + self._steps_per_rev = int(steps_per_rev) # full steps per revolution + self._max_pos = 2*int(max_pos) # max position stored in half steps + self._freq = 0 + self._min_period = 0 + self._step_size = int(step_size) # 1 = half steps, 2 = full steps + self._last_step_time = 0 + self.track_target() + + def step_size(self,sz=1): + if sz < 1: + sz = 1 + elif sz > 2: + sz = 2 + self._step_size = int(sz) + + def speed(self,sps): # speed in FULL steps per second + if self._free_run_mode == 1 and sps < 0: + self._free_run_mode = -1 + elif self._free_run_mode == -1 and sps > 0: + self._free_run_mode = 1 + if sps > self._max_sps: + sps = self._max_sps + elif sps < -self._max_sps: + sps = -self._max_sps + self._steps_per_sec = int(sps) + self._update_timer((2//self._step_size)*abs(self._steps_per_sec)) # steps per second + + def speed_rps(self,rps): + self.speed(rps*self._steps_per_rev) + + def get_speed(self) -> int: + return self._steps_per_sec + + def target(self,t): + if self._calibrated and t < 0: + # when already calibrated limit to 0 + self._target_pos = 0 + elif self._calibrated and (2*int(t)) > self._max_pos: + # when already calibrated limit to max + self._target_pos = self._max_pos + else: + self._target_pos = 2*int(t) + + def target_deg(self,deg): + self.target(self._steps_per_rev*deg/360.0) # target pos is in steps + + def target_rad(self,rad): + self.target(self._steps_per_rev*rad/(2*pi)) # target pos is in steps + + def get_pos(self) -> int: + return (self._pos//2) # convert half steps to full steps + + def get_pos_deg(self) -> float: + return self._pos*180.0/self._steps_per_rev # half steps to degrees + + def get_pos_rad(self) -> float: + return self._pos*pi/self._steps_per_rev # half steps to radians + + def overwrite_pos(self,p=0): + self._pos = 2*int(p) # convert full steps to half steps + + def overwrite_pos_deg(self,deg): + self._pos = deg*self._steps_per_rev/180.0 # degrees to half steps + + def overwrite_pos_rad(self,rad): + self._pos = rad*self._steps_per_rev/pi # radians to half steps + + def step(self,d=0): + cur_time = time.ticks_ms() + if time.ticks_diff(cur_time, self._last_step_time) < self._min_period: + # avoid stepping too quickly as this causes skipped steps + return + self._last_step_time = cur_time + if d>0: + self._pos+=self._step_size + self._phase = (self._phase-self._step_size)%_STEPPER_NUM_PHASES + elif d<0: + self._pos-=self._step_size + self._phase = (self._phase+self._step_size)%_STEPPER_NUM_PHASES + # Check position limits + if self._calibrated and self._pos < 0: + print("s/w min endstop") + self._pos = 0 + self.speed(0) + return + elif self._calibrated and self._pos > self._max_pos: + print("s/w max endstop") + self._pos = self._max_pos + self.speed(0) + return + try: + self._hexdrive_app.motor_step(self._phase) + except Exception as e: + print(f"step phase {self._phase} failed:{e}") + + # There is no code to handle the endstop being hit at present - it needs to be specific to the hardware + # i.e. which pin is connected to the endstop. + def _hit_endstop(self): + print("Endstop - hit") + if not self._calibrated: + self._calibrated = True + # set this as the new zero position + self.overwrite_pos(0) + # if we were moving towards the endstop, stop + if self._free_run_mode < 0: + self.speed(0) + elif self._free_run_mode == 0 and self._target_pos < self._pos: + self.speed(0) + + def _timer_callback_fwd(self,t): + self.step(1) + + def _timer_callback_rev(self,t): + self.step(-1) + + def _timer_callback(self,t): + if self._target_pos>self._pos: + self.step(1) + elif self._target_pos0: + self._timer.init(freq=freq,callback=self._timer_callback_fwd) + elif self._free_run_mode<0: + self._timer.init(freq=freq,callback=self._timer_callback_rev) + else: + self._timer.init(freq=freq,callback=self._timer_callback) + self._freq = freq + self._min_period = (1000//freq) - 1 + self._timer_is_running=True + self._timer_mode = self._free_run_mode + except Exception as e: + print(f"update_timer failed:{e}") + elif freq == 0: + print("Timer: 0Hz") + + + def stop(self): + self._update_timer(0) + try: + self._hexdrive_app.motor_release() + except Exception as e: + print(f"stop failed:{e}") + + def enable(self,e = True): + self._enabled=e + try: + if e: + if self._free_run_mode!=0: + self._update_timer((2//self._step_size)*abs(self._steps_per_sec)) # half steps per second + self._hexdrive_app.motor_step(self._phase) + else: + self._update_timer(0) + self._hexdrive_app.motor_release() + self._hexdrive_app.set_power(e) + except Exception as e: + print(f"enable failed:{e}") + + def is_enabled(self) -> bool: + return self._enabled + +########## END OF STEPPER CLASS ########## + + + +class HexDriveType: + def __init__(self, pid, vid = 0xCAFE, motors = 0, steppers = 0, servos = 0, name ="Unknown"): + self.vid = vid + self.pid = pid + self.name = name + self.motors = motors + self.servos = servos + self.steppers = steppers + + +class MySetting: + def __init__(self, container, default, minimum, maximum): + self._container = container + self.d = default + self.v = default + self._min = minimum + self._max = maximum + + + def __str__(self): + return str(self.v) + + + def _index(self): + for k,v in self._container.items(): + if v == self: + return k + return None + + + # This returns an increase in the value passed in - subject to max and with scale of increase depending on level + # based on the type of the setting + # it does not affect the current value of the setting + def inc(self, v, l=0): + 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 # round up to the next multiple of 10^l, being very careful not to cause big jumps when value was nearly at the next multiple + + if v > self._max: + v = self._max + elif isinstance(self.v, float): + # only float at present is brightness from 0.0 to 1.0 + v += 0.1 + if v > self._max: + v = self._max + elif self._container['logging'].v: + print(f"H:inc type: {type(self.v)}") + return v + + # This returns a decrease in the value passed in - subject to min and with scale of increase depending on level + # based on the type of the setting + # it does not affect the current value of the setting + def dec(self, v, l=0): + 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 # round down to the next multiple of 10^l + + if v < self._min: + v = self._min + elif isinstance(self.v, float): + # only float at present is brightness from 0.0 to 1.0 + v -= 0.1 + if v < self._min: + v = self._min + elif self._container['logging'].v: + print(f"H: dec type: {type(self.v)}") + return v + + + def persist(self): + # only save non-default settings to the settings store + try: + if self.v != self.d: + settings.set(f"badgebot.{self._index()}", self.v) + else: + settings.set(f"badgebot.{self._index()}", None) + except Exception as e: + print(f"H:Failed to persist setting {self._index()}: {e}") + + +class Instruction: + def __init__(self, press_type: Button) -> None: + self._press_type = press_type + self._duration = 1 + self.power_plan = [] + + + @property + def press_type(self) -> Button: + return self._press_type + + + def inc(self): + self._duration += 1 + + + def __str__(self): + return f"{self.press_type.name} {self._duration}" + + + def directional_power_tuple(self, power): + if self._press_type == BUTTON_TYPES["UP"]: + return ( power, power) + elif self._press_type == BUTTON_TYPES["DOWN"]: + return (-power, -power) + elif self._press_type == BUTTON_TYPES["LEFT"]: + return (-power, power) + elif self._press_type == BUTTON_TYPES["RIGHT"]: + return ( power, -power) + + + def directional_duration(self, mysettings): + if self._press_type == BUTTON_TYPES["UP"] or self._press_type == BUTTON_TYPES["DOWN"]: + return (mysettings['drive_step_ms'].v) + elif self._press_type == BUTTON_TYPES["LEFT"] or self._press_type == BUTTON_TYPES["RIGHT"]: + return (mysettings['turn_step_ms'].v) + + + def make_power_plan(self, mysettings): + # return collection of tuples of power and their duration + curr_power = 0 + ramp_up = [] + for i in range(1*(self._duration+3)): + ramp_up.append((self.directional_power_tuple(curr_power), _TICK_MS)) + curr_power += mysettings['acceleration'].v + if curr_power >= mysettings['max_power'].v: + ramp_up.append((self.directional_power_tuple(mysettings['max_power'].v), _TICK_MS)) + break + user_power_duration = (self.directional_duration(mysettings) * self._duration)-(2*(i+1)*_TICK_MS) + power_durations = ramp_up.copy() + if user_power_duration > 0: + power_durations.append((self.directional_power_tuple(mysettings['max_power'].v), user_power_duration)) + ramp_down = ramp_up.copy() + ramp_down.reverse() + power_durations.extend(ramp_down) + if mysettings['logging'].v: + print("Power durations:") + print(power_durations) + self.power_plan = power_durations + + +__app_export__ = LineFollowerApp From 600f44a270beeadb82a114af76625ccdbf8700ae Mon Sep 17 00:00:00 2001 From: robotmad Date: Wed, 1 Jan 2025 18:17:03 +0000 Subject: [PATCH 15/16] integration of gr10_30 gesture sensor --- gr10_30.py | 398 ++++++++++++++++++++++++++++++++++++++++++++++++ linefollower.py | 119 +++++++++++++-- 2 files changed, 507 insertions(+), 10 deletions(-) create mode 100644 gr10_30.py diff --git a/gr10_30.py b/gr10_30.py new file mode 100644 index 0000000..2dbf1b8 --- /dev/null +++ b/gr10_30.py @@ -0,0 +1,398 @@ +# -*- coding: utf-8 -* +'''! + @file DFRobot_GR10_30.py + @brief This is the basic library of GR30_10 sensor + @copyright Copyright (c) 2021 DFRobot Co.Ltd (http://www.dfrobot.com) + @license The MIT License (MIT) + @author zhixinliu(zhixinliu@dfrobot.com) + @version V1.0 + @date 2022-07-27 + @url https://github.com/DFRobor/DFRobot_GR10_30 +''' + +import time +from machine import I2C + +DEV_ADDRESS = 0x73 + +#Input Register +GR30_10_INPUTREG_PID =0x00 #Device PID +GR30_10_INPUTREG_VID =0x01 #VID of the device, fixed to be 0x3343 +GR30_10_INPUTREG_ADDR =0x02 #Device address of the module +GR30_10_INPUTREG_BAUDRATE =0x03 #UART baud rate +GR30_10_INPUTREG_STOPBIT =0x04 #UART check bit and stop bit +GR30_10_INPUTREG_VERSION =0x05 #Firmware version information +GR30_10_INPUTREG_DATA_READY =0x06 #Data ready register +GR30_10_INPUTREG_INTERRUPT_STATE =0x07 #Gesture interrupt status +GR30_10_INPUTREG_EXIST_STATE =0x08 #Presence status +#Holding Register +GR30_10_HOLDINGREG_INTERRUPT_MODE =0x09 #The gesture that can trigger an interrupt +GR30_10_HOLDINGREG_LRUP_WIN =0x0a #Detection window +GR30_10_HOLDINGREG_L_RANGE =0x0b #The distance your hand should move to the left +GR30_10_HOLDINGREG_R_RANGE =0x0c #The distance your hand should move to the right +GR30_10_HOLDINGREG_U_RANGE =0x0d #The distance your hand should move up +GR30_10_HOLDINGREG_D_RANGE =0x0e #The distance your hand should move down +GR30_10_HOLDINGREG_FORWARD_RANGE =0x0f #The distance your hand should move forward +GR30_10_HOLDINGREG_BACKWARD_RANGE =0x10 #The distance your hand should move backward +GR30_10_HOLDINGREG_WAVE_COUNT =0x11 #The times you need to wave hands +GR30_10_HOLDINGREG_HOVR_WIN =0x12 #Hover detection window +GR30_10_HOLDINGREG_HOVR_TIMER =0x13 #The duration your hand should hover +GR30_10_HOLDINGREG_CWS_ANGLE =0x14 #Clockwise rotation angle, each value equals 22.5° +GR30_10_HOLDINGREG_CCW_ANGLE =0x15 #Counterclockwise rotation angle, each value equals 22.5° +GR30_10_HOLDINGREG_CWS_ANGLE_COUNT =0x16 #Continuous clockwise rotation angle, each value equals 22.5° +GR30_10_HOLDINGREG_CCW_ANGLE_COUNT =0x17 #Continuous counterclockwise rotation angle, each value equals 22.5° +GR30_10_HOLDINGREG_RESET =0x18 #Reset sensor + +GESTURE_UP = (1<<0) +GESTURE_DOWN = (1<<1) +GESTURE_LEFT = (1<<2) +GESTURE_RIGHT = (1<<3) +GESTURE_FORWARD = (1<<4) +GESTURE_BACKWARD = (1<<5) +GESTURE_CLOCKWISE = (1<<6) +GESTURE_COUNTERCLOCKWISE = (1<<7) +GESTURE_WAVE = (1<<8) +GESTURE_HOVER = (1<<9) +GESTURE_UNKNOWN = (1<<10) +GESTURE_CLOCKWISE_C = (1<<14) +GESTURE_COUNTERCLOCKWISE_C = (1<<15) + +class DFRobot_GR10_30(): + __temp_buffer = [0]*2 + def __init__(self ,bus = 2): + self.gestures = 0 + self.__temp_buffer = [0]*2 + self.i2cbus = I2C(bus) + + def _detect_device_address(self): + '''! + @brief Get sensor address + @return Return sensor address + ''' + rbuf = self._read_reg(GR30_10_INPUTREG_ADDR, 2) + data = rbuf[0]<<8 | rbuf[1] + time.sleep(0.1) + return data + + def begin(self) -> bool: + '''! + @brief Init the sensor + ''' + if self._detect_device_address() != DEV_ADDRESS: + return False + self.reset_sensor() + time.sleep(0.5) + return True + + def en_gestures(self, gestures) -> bool: + '''! + @brief Set what gestures the module can recognize to trigger interrupt + @param gestures The gesture to be enabled + @n GESTURE_UP + @n GESTURE_DOWN + @n GESTURE_DOWN + @n GESTURE_LEFT + @n GESTURE_RIGHT + @n GESTURE_FORWARD + @n GESTURE_BACKWARD + @n GESTURE_CLOCKWISE + @n GESTURE_COUNTERCLOCKWISE + @n GESTURE_WAVE It is not suggested to enable rotation gesture (CW/CCW) and wave gesture at the same time. + @n GESTURE_HOVER Disable other gestures when hover gesture enables. + @n GESTURE_UNKNOWN + @n GESTURE_CLOCKWISE_C Rotate clockwise continuously + @n GESTURE_COUNTERCLOCKWISE_C Rotate counterclockwise continuously + @return Return True if the configuration is successful, otherwise return False + ''' + self.gestures = gestures&0xc7ff + self.__temp_buffer[0] = (gestures>>8)&0xC7 # For changing to 8bit + self.__temp_buffer[1] = gestures&0x00ff + return (self._write_reg(GR30_10_HOLDINGREG_INTERRUPT_MODE, self.__temp_buffer)) + + def set_udlr_win(self, ud_size, lr_size): + '''! + @brief Set the detection window + @param udSize Distance from top to bottom distance range 1-30 + @param lrSize Distance from left to right distance range 1-30 + @return NONE + ''' + lr_size = lr_size&0x001f + ud_size = ud_size&0x001f + self.__temp_buffer[0] = lr_size + self.__temp_buffer[1] = ud_size + self._write_reg(GR30_10_HOLDINGREG_LRUP_WIN, self.__temp_buffer) + time.sleep(0.1) + + def set_left_range(self, range): + '''! + @brief Set how far your hand should move to the left so the sensor can recognize it + @param range + @n Distance range 5-25, must be less than distance from left to right of the detection window + @return NONE + ''' + range = range&0x1f + self.__temp_buffer[0] = 0 + self.__temp_buffer[1] = range + self._write_reg(GR30_10_HOLDINGREG_L_RANGE, self.__temp_buffer) + time.sleep(0.1) + + def set_right_range(self, range): + '''! + @brief Set how far your hand should move to the right so the sensor can recognize it + @param range + @n Distance range 5-25, must be less than distance from left to right of the detection window + ''' + range = range&0x1f + self.__temp_buffer[0] = 0 + self.__temp_buffer[1] = range + self._write_reg(GR30_10_HOLDINGREG_R_RANGE, self.__temp_buffer) + time.sleep(0.1) + + def set_up_range(self, range): + '''! + @brief Set how far your hand should move up so the sensor can recognize it + @param range + @n Distance range 5-25, must be less than distance from top to bottom of the detection window + ''' + range = range&0x1f + self.__temp_buffer[0] = 0 + self.__temp_buffer[1] = range + self._write_reg(GR30_10_HOLDINGREG_U_RANGE, self.__temp_buffer) + time.sleep(0.1) + + def set_down_range(self, range): + '''! + @brief Set how far your hand should move down so the sensor can recognize it + @param range + @n Distance range 5-25, must be less than distance from top to bottom of the detection window + ''' + range = range&0x1f + self.__temp_buffer[0] = 0 + self.__temp_buffer[1] = range + self._write_reg(GR30_10_HOLDINGREG_D_RANGE, self.__temp_buffer) + time.sleep(0.1) + + def set_forward_range(self, range): + '''! + @brief Set how far your hand should move forward so the sensor can recognize it + @param range + @n Distance range 1-15 + ''' + range = range&0x1f + self.__temp_buffer[0] = 0 + self.__temp_buffer[1] = range + self._write_reg(GR30_10_HOLDINGREG_FORWARD_RANGE, self.__temp_buffer) + time.sleep(0.1) + + def set_backward_range(self, range): + '''! + @brief Set how far your hand should move backward so the sensor can recognize it + @param range + @n Distance range 1-15 + ''' + range = range&0x1f + self.__temp_buffer[0] = 0 + self.__temp_buffer[1] = range + self._write_reg(GR30_10_HOLDINGREG_BACKWARD_RANGE, self.__temp_buffer) + time.sleep(0.1) + + def set_wave_number(self, number): + '''! + @brief Set how many times you need to wave hands so the sensor can recognize it + @param number + @n Number range 1-15 + @return NONE + ''' + number = number&0x0f + self.__temp_buffer[0] = 0 + self.__temp_buffer[1] = number + self._write_reg(GR30_10_HOLDINGREG_WAVE_COUNT, self.__temp_buffer) + time.sleep(0.1) + + def set_hover_win(self, ud_size, lr_size): + '''! + @brief Set hover detection window + @param udSize Distance from top to bottom distance range 1-30 + @param lrSize Distance from left to right distance range 1-30 + @return NONE + ''' + lr_size = lr_size&0x001f + ud_size = ud_size&0x001f + self.__temp_buffer[0] = lr_size + self.__temp_buffer[1] = ud_size + self._write_reg(GR30_10_HOLDINGREG_HOVR_WIN, self.__temp_buffer) + time.sleep(0.1) + + def set_hover_timer(self, timer): + '''! + @brief Set how long your hand should hover to trigger the gesture + @param timer Each value is 10ms + @n timer Maximum is 200 default is 60 600ms + ''' + timer = timer&0x03FF + self.__temp_buffer[0] = (timer>>8)&0x03 # For changing to 8bit + self.__temp_buffer[1] = timer&0x00ff # For changing to 8bit + self._write_reg(GR30_10_HOLDINGREG_HOVR_TIMER, self.__temp_buffer) + time.sleep(0.1) + + def set_cws_angle(self, count): + '''! + @brief Set how many degrees your hand should rotate clockwise to trigger the gesture + @param count Default 16, maximum 31 + @n count Rotation angle = 22.5 * count + @n For example: count = 16, 22.5*count = 360, rotate 360° to trigger the gesture + @return NONE + ''' + count = count&0x1f + self.__temp_buffer[0] = 0 + self.__temp_buffer[1] = count + self._write_reg(GR30_10_HOLDINGREG_CWS_ANGLE, self.__temp_buffer) + time.sleep(0.1) + + def set_ccw_angle(self, count): + '''! + @brief Set how many degrees your hand should rotate counterclockwise to trigger the gesture + @param count Default 16, maximum 31 + @n count Rotation angle = 22.5 * count + @n For example: count = 16, 22.5*count = 360, rotate 360° to trigger the gesture + @return NONE + ''' + count = count&0x1f + self.__temp_buffer[0] = 0 + self.__temp_buffer[1] = count + self._write_reg(GR30_10_HOLDINGREG_CCW_ANGLE, self.__temp_buffer) + time.sleep(0.1) + + def set_cws_angle_count(self, count): + '''! + @brief Set how many degrees your hand should rotate clockwise continuously to trigger the gesture + @param count Default 4, maximum 31 + @n count Continuous rotation angle = 22.5 * count + @n For example: count = 4 22.5*count = 90 + @n Trigger the clockwise/counterclockwise rotation gesture first, + @n if keep rotating, then the continuous rotation gesture will be triggered once every 90 degrees + @return NONE + ''' + count = count&0x1f + self.__temp_buffer[0] = 0 + self.__temp_buffer[1] = count + self._write_reg(GR30_10_HOLDINGREG_CWS_ANGLE_COUNT, self.__temp_buffer) + time.sleep(0.1) + + def set_ccw_angle_count(self, count): + '''! + @brief Set how many degrees your hand should rotate counterclockwise continuously to trigger the gesture + @param count Default 4, maximum 31 + @n count Continuous rotation angle = 22.5 * count + @n For example: count = 4 22.5*count = 90 + @n Trigger the clockwise/counterclockwise rotation gesture first, + @n if keep rotating, then the continuous rotation gesture will be triggered once every 90 degrees + @return NONE + ''' + count = count&0x1f + self.__temp_buffer[0] = 0 + self.__temp_buffer[1] = count + self._write_reg(GR30_10_HOLDINGREG_CCW_ANGLE_COUNT, self.__temp_buffer) + time.sleep(0.1) + + + def reset_sensor(self): + '''! + @brief Reset sensor + @return NONE + ''' + self.__temp_buffer[0] = 0x55 + self.__temp_buffer[1] = 0x00 + self._write_reg(GR30_10_HOLDINGREG_RESET, self.__temp_buffer) + time.sleep(0.1) + + + def get_data_ready(self) -> bool: + '''! + @brief Get if a gesture is detected + @return If a gesture is detected + @retval True Detected + @retval False Not detected + ''' + rbuf = self._read_reg(GR30_10_INPUTREG_DATA_READY, 2) + data = rbuf[0]*256 + rbuf[1] + if data == 0x01: + return True + else: + return False + + def get_gestures(self) -> int: + '''! + @brief Get gesture type + @return Gesture type + @retval GESTURE_UP + @retval GESTURE_DOWN + @retval GESTURE_DOWN + @retval GESTURE_LEFT + @retval GESTURE_RIGHT + @retval GESTURE_FORWARD + @retval GESTURE_BACKWARD + @retval GESTURE_CLOCKWISE + @retval GESTURE_COUNTERCLOCKWISE + @retval GESTURE_WAVE + @retval GESTURE_HOVER + @retval GESTURE_UNKNOWN + @retval GESTURE_CLOCKWISE_C + @retval GESTURE_COUNTERCLOCKWISE_C + ''' + rbuf = self._read_reg(GR30_10_INPUTREG_INTERRUPT_STATE, 2) + data = rbuf[0]*256 + rbuf[1] + return data + + + def get_exist(self) -> bool: + '''! + @brief Get whether the object is in the sensor detection range + @return If the object is in the sensor detection range + @retval 0 No + @retval 1 Yes + ''' + rbuf = self._read_reg(GR30_10_INPUTREG_EXIST_STATE, 2) + data = rbuf[0]*256 + rbuf[1] + return data + +class DFRobot_GR30_10_I2C(DFRobot_GR10_30): + '''! + @brief An example of an i2c interface module + ''' + def __init__(self ,bus ,addr): + self._addr = addr + DFRobot_GR10_30.__init__(self,bus) + + + def _read_reg(self, reg_addr ,length) -> list: + '''! + @brief read the data from the register + @param reg_addr register address + @param length read data + ''' + reg = reg_addr + try: + rslt = self.i2cbus.readfrom_mem(self._addr, reg, length) + except: + rslt = [0,0] + return rslt + + def _write_reg(self, reg_addr ,data) -> bool: + '''! + @brief write the data from the register + @param reg_addr register address + @param data Data to be written to register + ''' + self._reg = reg_addr + # convert data from list to bytes which supports buffer protocol + new_data = bytearray(data) + #print(f"write to addr:{self._addr} reg:{self._reg} data:{new_data}") + try: + self.i2cbus.writeto_mem(self._addr, self._reg, new_data) + rslt = True + except Exception as e: + print(f"write error:{e}") + rslt = False + return rslt + \ No newline at end of file diff --git a/linefollower.py b/linefollower.py index bc6061b..00f8db4 100644 --- a/linefollower.py +++ b/linefollower.py @@ -30,6 +30,7 @@ import app from .utils import chain, draw_logo_animated, parse_version +from .gr10_30 import * #import micropython #micropython.alloc_emergency_exception_buf(100) @@ -88,6 +89,9 @@ _LINE_SENSOR_DEFAULT_THRESHOLD = 500 _LINE_SENSOR_TIMEOUT = 2000 FOLLOWER_HEXPANSION = 1 # Hexpansion slot for Line Follower Sensors - as it does not have an EEPROM to be detected automatically +GESTURE_HEXPANSION = 2 # Hexpansion slot for Gesture Sensor - as it does not have an EEPROM to be detected automatically +GESTURE_ADDRESS = 0x73 + FOLLOWER_SENSOR_SCAN_PERIOD = 25 # ms DEFAULT_UPDATE_PERIOD = 50 # ms @@ -331,10 +335,38 @@ def __init__(self): self.servo_selected: int = 0 # Line Follower + self._override = False self.num_line_sensors: int = _NUM_LINE_SENSORS self._line_sensors = [None]*_NUM_LINE_SENSORS self._hexpansion_config = HexpansionConfig(FOLLOWER_HEXPANSION) # There is no EEPROM on the Line Follower Sensor Hexpansion + # Gesture Sensor + self._gesture_sensor = DFRobot_GR30_10_I2C(GESTURE_HEXPANSION, GESTURE_ADDRESS) + if self._gesture_sensor.begin(): + print("GR10_30 Sensor initialize success") + if (not self._gesture_sensor.en_gestures(GESTURE_UP|GESTURE_DOWN|GESTURE_LEFT|GESTURE_RIGHT)): + print("Sensor enable gestures failed!!") + time.sleep(0.1) + self._gesture_sensor.set_udlr_win(20, 20) + self._gesture_sensor.set_left_range(5) + self._gesture_sensor.set_right_range(5) + self._gesture_sensor.set_up_range(5) + self._gesture_sensor.set_down_range(5) + + self._gesture_sensor.set_forward_range(15) + self._gesture_sensor.set_backward_range(15) + + #self._gesture_sensor.set_wave_number(5) + #self._gesture_sensor.set_hover_win(30, 30) + #self._gesture_sensor.set_hover_timer(20) # Each value is about 10ms + #self._gesture_sensor.set_cws_angle(2) + #self._gesture_sensor.set_ccw_angle(2) + #self._gesture_sensor.set_cws_angle_count(4) + #self._gesture_sensor.set_ccw_angle_count(4) + else: + print("GR10_30 Sensor initialize failed!!") + self._gesture_sensor = None + # Overall app state (controls what is displayed and what user inputs are accepted) self.current_state = STATE_INIT self.previous_state = self.current_state @@ -440,13 +472,26 @@ def background_update(self, delta: int): self._line_sensors[i].read() else: print(f"Line Sensor {i} not initialised") - # Line Follower Sensor - if self._line_sensors[0].value() and not self._line_sensors[1].value(): - output = (-self._settings['max_power'].v, self._settings['max_power'].v) - elif not self._line_sensors[0].value() and self._line_sensors[1].value(): - output = (self._settings['max_power'].v, -self._settings['max_power'].v) + output = (0,0) + # if _override is a bool + if type(self._override) is bool: + if not self._override: + # Line Follower Sensor + if self._line_sensors[0].value() and not self._line_sensors[1].value(): + output = (-self._settings['max_power'].v, self._settings['max_power'].v) + elif not self._line_sensors[0].value() and self._line_sensors[1].value(): + output = (self._settings['max_power'].v, -self._settings['max_power'].v) + else: + output = (self._settings['max_power'].v, self._settings['max_power'].v) else: - output = (self._settings['max_power'].v, self._settings['max_power'].v) + if self._override == BUTTON_TYPES["UP"]: + output = (self._settings['max_power'].v, self._settings['max_power'].v) + elif self._override == BUTTON_TYPES["DOWN"]: + output = (-self._settings['max_power'].v, -self._settings['max_power'].v) + elif self._override == BUTTON_TYPES["LEFT"]: + output = (-self._settings['max_power'].v, self._settings['max_power'].v) + elif self._override == BUTTON_TYPES["RIGHT"]: + output = (self._settings['max_power'].v, -self._settings['max_power'].v) if self.hexdrive_app is not None: self.hexdrive_app.set_motors(output) @@ -717,8 +762,10 @@ def update(self, delta: int): self._pattern_management() self._update_hexpansion_management(delta) + self._update_sensor(delta) self._update_main_application(delta) + if self.current_state != self.previous_state: if self._settings['logging'].v: print(f"State: {self.previous_state} -> {self.current_state}") @@ -782,6 +829,55 @@ def _update_hexpansion_management(self, delta: int): self._update_state_check(delta) + def _update_sensor(self, delta: int): + # GR10_30 Gesture Sensor + if self._gesture_sensor: + if self._gesture_sensor.get_exist() == True: + self._override = True + self._time_since_last_update = 0 + else: + # wait for 2 seconds before reverting to False + self._time_since_last_update += delta + if self._time_since_last_update > 2000: + self._time_since_last_update = 0 + if type(self._override) is not bool or self._override == True: + print("Timeout") + self._override = False + if self._gesture_sensor.get_data_ready() == True: + gesture = self._gesture_sensor.get_gestures() + self._time_since_last_update = 0 + # Sensor is at 90 degrees to the badge - so the gestures are rotated + if gesture&GESTURE_UP != False: + print("up\r\n") + self._override = BUTTON_TYPES["LEFT"] + elif gesture&GESTURE_DOWN != False: + print("down\r\n") + self._override = BUTTON_TYPES["RIGHT"] + elif gesture&GESTURE_LEFT != False: + print("left\r\n") + self._override = BUTTON_TYPES["DOWN"] + elif gesture&GESTURE_RIGHT != False: + print("right\r\n") + self._override = BUTTON_TYPES["UP"] + elif gesture&GESTURE_FORWARD != False: + print("forward\r\n") + elif gesture&GESTURE_BACKWARD != False: + print("backward\r\n") + elif gesture&GESTURE_CLOCKWISE != False: + print("clockwise\r\n") + elif gesture&GESTURE_COUNTERCLOCKWISE != False: + print("counter clockwise\r\n") + elif gesture&GESTURE_WAVE != False: + print("wave\r\n") + elif gesture&GESTURE_HOVER != False: + print("hover\r\n") + elif gesture&GESTURE_UNKNOWN != False: + print("unknown\r\n") + elif gesture&GESTURE_CLOCKWISE_C != False: + print("continue clockwise\r\n") + elif gesture&GESTURE_COUNTERCLOCKWISE_C != False: + print("counter continue clockwise\r\n") + def _update_main_application(self, delta: int): if self.current_state == STATE_MENU: if self.current_menu is None: @@ -1299,6 +1395,10 @@ def _update_state_follower(self, delta: int): self._update_period = DEFAULT_UPDATE_PERIOD self.current_state = STATE_MENU return + # Button/Gesture Commands: + #if self.button_states.get(BUTTON_TYPES["CONFIRM"]): + # self.button_states.clear() + # self.override = True # Does the line sensor data need refreshing on the display? for i in range(self.num_line_sensors): if self._line_sensors[i].updated: @@ -1635,7 +1735,7 @@ def _draw_state_stepper(self, ctx): def _draw_state_follower(self, ctx): # Line Follower - print("d") + #print("d") # draw the two line follower sensor values on/off as green/white circles ctx.save() for i in range(self.num_line_sensors): @@ -1846,6 +1946,7 @@ def _main_menu_select_handler(self, item: str, idx: int): self._line_sensors[i] = LineSensor(self, pins, name = "Left" if i == 0 else "Right", threshold = self._settings['line_threshold'].v) self._line_sensors[i].enable() if self.hexdrive_app is not None: + self.hexdrive_app.set_logging(False) if not self.hexdrive_app.set_power(True): print("Failed to enable HexDrive power") else: @@ -2031,8 +2132,6 @@ def finalize_instruction(self): self.current_instruction = None - - ######## LINE SENSOR CLASS ######## class LineSensor: @@ -2111,7 +2210,7 @@ def _handler(self, pin): prev_state = self._state self._state = True if time.ticks_diff(now, self._start_time) < self._threshold else False if (prev_state != self._state): - print("u") + #print("u") self.updated = True self._start_time = 0 From 8b1a4a7814835a3d374abc9ac92c41ba4fd58433 Mon Sep 17 00:00:00 2001 From: robotmad Date: Mon, 23 Feb 2026 18:47:14 +0000 Subject: [PATCH 16/16] commit old changes and cross fingers --- linefollower.py | 99 ++++++++++++++++++++++++++++++++++--------------- 1 file changed, 69 insertions(+), 30 deletions(-) diff --git a/linefollower.py b/linefollower.py index 00f8db4..c0c9a9f 100644 --- a/linefollower.py +++ b/linefollower.py @@ -92,7 +92,7 @@ GESTURE_HEXPANSION = 2 # Hexpansion slot for Gesture Sensor - as it does not have an EEPROM to be detected automatically GESTURE_ADDRESS = 0x73 -FOLLOWER_SENSOR_SCAN_PERIOD = 25 # ms +FOLLOWER_SENSOR_SCAN_PERIOD = 10 # ms DEFAULT_UPDATE_PERIOD = 50 # ms # Dedicated Pins @@ -162,7 +162,7 @@ # App states where user can minimise app _MINIMISE_VALID_STATES = [0, 1, 7, 12, 13, 14, 15] -_LED_CONTROL_STATES = [0, 3, 4, 5, 6, 12, 13, 14, 15] +_LED_CONTROL_STATES = [0, 3, 4, 5, 6, 12, 13, 14, 15, 19] # HexDrive Hexpansion constants _EEPROM_ADDR = 0x50 @@ -321,6 +321,7 @@ def __init__(self): self._stepper: Stepper = None self.stepper_mode = StepperMode() self.stepper_pos: int = 0 + self._output = (0,0) # Servo Tester self._time_since_last_input: int = 0 @@ -337,8 +338,12 @@ def __init__(self): # Line Follower self._override = False self.num_line_sensors: int = _NUM_LINE_SENSORS + self._s = [False, False] self._line_sensors = [None]*_NUM_LINE_SENSORS self._hexpansion_config = HexpansionConfig(FOLLOWER_HEXPANSION) # There is no EEPROM on the Line Follower Sensor Hexpansion + self._sample_count: int = 0 + self._sample_time: int = 0 + self._rate: int = 0 # sample rate # Gesture Sensor self._gesture_sensor = DFRobot_GR30_10_I2C(GESTURE_HEXPANSION, GESTURE_ADDRESS) @@ -467,22 +472,33 @@ def background_update(self, delta: int): self.hexdrive_app.set_motors(output) elif self.current_state == STATE_FOLLOWER: # Read the line sensors - for i in range(self.num_line_sensors): - if self._line_sensors[i] is not None: - self._line_sensors[i].read() - else: - print(f"Line Sensor {i} not initialised") + #for i in range(self.num_line_sensors): + # if self._line_sensors[i] is not None: + # self._line_sensors[i].read() + # else: + # print(f"Line Sensor {i} not initialised") output = (0,0) # if _override is a bool if type(self._override) is bool: if not self._override: # Line Follower Sensor - if self._line_sensors[0].value() and not self._line_sensors[1].value(): - output = (-self._settings['max_power'].v, self._settings['max_power'].v) - elif not self._line_sensors[0].value() and self._line_sensors[1].value(): - output = (self._settings['max_power'].v, -self._settings['max_power'].v) + # have either sensors changed their line detection status? + s0 = self._line_sensors[0].value() + s1 = self._line_sensors[1].value() + self._line_sensors[0].read() + self._line_sensors[1].read() + if (s0 != self._s[0] or s1 != self._s[1]): + self._s[0] = s0 + self._s[1] = s1 + if s0 and not s1: + output = (-self._settings['max_power'].v, self._settings['max_power'].v) + elif not s0 and s1: + output = (self._settings['max_power'].v, -self._settings['max_power'].v) + else: + output = (self._settings['max_power'].v, self._settings['max_power'].v) + self._output = output else: - output = (self._settings['max_power'].v, self._settings['max_power'].v) + output = self._output else: if self._override == BUTTON_TYPES["UP"]: output = (self._settings['max_power'].v, self._settings['max_power'].v) @@ -492,8 +508,7 @@ def background_update(self, delta: int): output = (-self._settings['max_power'].v, self._settings['max_power'].v) elif self._override == BUTTON_TYPES["RIGHT"]: output = (self._settings['max_power'].v, -self._settings['max_power'].v) - if self.hexdrive_app is not None: - self.hexdrive_app.set_motors(output) + self.hexdrive_app.set_motors(output) def generate_new_qr(self): @@ -1385,6 +1400,7 @@ def _update_state_stepper(self, delta: int): def _update_state_follower(self, delta: int): # Line Follower: + self._sample_time += delta # Cancel to exit if self.button_states.get(BUTTON_TYPES["CANCEL"]): self.button_states.clear() @@ -1735,17 +1751,25 @@ def _draw_state_stepper(self, ctx): def _draw_state_follower(self, ctx): # Line Follower - #print("d") # draw the two line follower sensor values on/off as green/white circles ctx.save() + # display the average sample rate (2 sensors) to 1 decimal place + if (self._sample_time > 2000): + state = disable_irq() + self._rate = int(((1000/2) * self._sample_count) // self._sample_time) + self._sample_count = 0 + enable_irq(state) + self._sample_time = 0 + print(f"Rate: {self._rate} Hz") + ctx.rgb(1,1,1).move_to(-50, -2*label_font_size).text(f"{self._rate} Hz") for i in range(self.num_line_sensors): - #ctx.translate(0, (i-0.5) * label_font_size) x = 40 - i * 80 colour = (0,1,0) if self._line_sensors[i].value() else (0,0,0) ctx.rgb(*colour).arc(x, 0, 30, 0, 2 * pi, True).fill() ctx.rgb(1,1,1).arc(x, 0, 31, 0, 2 * pi, True).stroke() ctx.restore() + def _draw_state_servo(self, ctx): servo_text = ["S"]*(1+self.num_servos) # Servo Text servo_text_colours = [(0.4,0.0,0.0)]*(1+self.num_servos) # Red @@ -2139,10 +2163,14 @@ def __init__(self, container, pins, name: str = "LineSensor", threshold: int = _ try: self._container = container self._threshold = threshold - self._state = False - self._name = name - self._start_time = 0 - self.updated = False + self._state: bool = False + self._prev_state: bool = False + self._name: str = name + self._start_time: int = 0 + self._diff: int = 0 + self._irq_state = None + self.updated: bool = False + self._phase: int = 0 # Pins for hardware self._pins = pins @@ -2186,34 +2214,45 @@ def read(self) -> bool: self._pins["sig"].on() time.sleep_us(10) # configure the pin as an input and wait for it to go low + # "hard" NOT supported + #self._phase = 0 self._pins["sig"].irq(trigger=Pin.IRQ_FALLING, handler=self._handler) + #self._phase = 1 state = disable_irq() + #self._phase = 2 self._start_time = time.ticks_us() self._pins["sig"].init(mode=Pin.IN, pull=None) enable_irq(state) + #self._phase = 3 return True def value(self) -> bool: return self._state - def _handler(self, pin): - now = time.ticks_us() - self._pins["ctrl"].off() - self._pins["sig"].irq(handler=None) + def _handler(self, _): # This is the interrupt handler for the line sensor # read the time and compare to the start time # if the time is less than the threshold then the sensor is active + self._irq_state = disable_irq() + self._diff = time.ticks_diff(time.ticks_us(), self._start_time) + self._pins["sig"].irq(handler=None) + enable_irq(self._irq_state) + #self._pins["sig"].off() + #self._pins["sig"].init(mode=Pin.OUT) + self._pins["ctrl"].off() + if self._start_time == 0: - print(f"Sensor {self._name} spurious interrupt") + #print(f"Sensor {self._name} {self._phase} spurious interrupt") + #self._phase = 4 # not expecting an interrupt return - prev_state = self._state - self._state = True if time.ticks_diff(now, self._start_time) < self._threshold else False - if (prev_state != self._state): - #print("u") + #self._phase = 5 + self._prev_state = self._state + self._state = True if self._diff < self._threshold else False + if (self._prev_state != self._state): self.updated = True self._start_time = 0 - + self._container._sample_count += 1 ######## STEPPER MOTOR CLASS ########