diff --git a/README.md b/README.md index 7291c00..65b9444 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 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 9a88b35..8867065 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 @@ -10,11 +12,11 @@ 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) -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 @@ -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 = 4 # 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.2" # 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, @@ -83,6 +96,13 @@ _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 @@ -115,7 +135,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] @@ -127,13 +148,56 @@ _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 = ["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 + 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): @@ -142,31 +206,31 @@ 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.notification = None + 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 = None - self.menu = None + self.current_menu: str = None + self.menu: 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_instruction: Instruction = None self.current_power_duration = ((0,0,0,0), 0) self.power_plan_iter = iter([]) @@ -182,8 +246,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 = None + self._edit_setting: int = None self._edit_setting_value = None self.update_settings() @@ -202,48 +267,61 @@ 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: Stepper = None + self.stepper_mode = StepperMode() + 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_input: int = 0 + 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 + 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 # 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) + ### 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)) - + self._gain_focus(RequestForegroundPushEvent(self)) ### ASYNC EVENT HANDLERS ### @@ -309,18 +387,19 @@ 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 ### NON-ASYNC FUCNTIONS ### - def background_update(self, delta): + 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) @@ -353,19 +432,17 @@ def scan_ports(self): self.check_port_for_hexdrive(port) - def check_port_for_hexdrive(self, port) -> bool: - # avoiding use of badge read_hexpansion_header as this triggers a full i2c scan each time + 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: - 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 +450,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: @@ -384,7 +461,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}") @@ -393,13 +470,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 @@ -465,7 +542,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( @@ -481,32 +558,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 @@ -538,19 +605,21 @@ 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: 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,25 +628,14 @@ 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: + 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 @@ -600,7 +658,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) @@ -629,7 +687,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() @@ -673,7 +731,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") @@ -706,13 +764,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: @@ -744,7 +806,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() @@ -755,7 +817,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) @@ -776,17 +838,19 @@ 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: 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() @@ -815,7 +879,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 @@ -838,7 +902,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() @@ -853,7 +917,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 @@ -869,10 +933,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 @@ -900,7 +965,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 @@ -911,7 +976,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 @@ -964,7 +1029,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 @@ -982,7 +1047,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 @@ -1040,7 +1105,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() @@ -1063,7 +1128,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: @@ -1071,9 +1136,10 @@ def _update_state_countdown(self, delta): 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): + 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: @@ -1087,14 +1153,93 @@ def _update_state_done(self, delta): 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_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): @@ -1102,7 +1247,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 @@ -1120,15 +1265,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].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, 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): @@ -1136,7 +1281,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 @@ -1154,7 +1299,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].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)): @@ -1180,14 +1325,25 @@ 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].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_modes[self.servo_mode[self.servo_selected]]}") + 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 @@ -1195,7 +1351,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 @@ -1209,12 +1365,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) @@ -1243,7 +1399,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() @@ -1253,7 +1409,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 @@ -1263,8 +1419,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: @@ -1317,6 +1473,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") @@ -1355,6 +1513,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 (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_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 @@ -1363,10 +1566,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 @@ -1399,17 +1602,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 @@ -1418,7 +1621,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: @@ -1426,7 +1629,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: @@ -1483,6 +1686,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 @@ -1505,9 +1709,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, @@ -1528,7 +1734,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 @@ -1542,7 +1748,29 @@ 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, 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[2]: # Servo Test if self.num_servos == 0: self.notification = Notification("No Servos") else: @@ -1552,24 +1780,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") @@ -1616,7 +1844,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]: @@ -1655,7 +1883,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: @@ -1687,13 +1915,216 @@ def finalize_instruction(self): self.current_instruction = None +######## 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, 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: @@ -1779,7 +2210,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/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/hexdrive.py b/hexdrive.py index f06194a..63abb30 100644 --- a/hexdrive.py +++ b/hexdrive.py @@ -4,18 +4,18 @@ 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 = 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 +_ENABLE_PIN = 0 # First LS pin used to enable the SMPSU +_DETECT_PIN = 1 # Second LS pin used to sense if the SMPSU has a source of power _DEFAULT_PWM_FREQ = 20000 _DEFAULT_SERVO_FREQ = 50 # 20mS period @@ -25,10 +25,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 +40,27 @@ 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._servo_centre = [_SERVO_CENTRE] * 4 eventbus.on_async(RequestStopAppEvent, self._handle_stop_app, self) try: @@ -62,104 +68,55 @@ 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) + 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: @@ -171,23 +128,28 @@ 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: + 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 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 pwm is not None: + try: + pwm.duty_u16(0) + except Exception as e: + print(f"H:{self.config.port}:PWM[{channel}]:Off failed {e}") + self.PWMOutput[channel] = None # Tidy Up + elif self._stepper: + self.motor_release() + # we keep retriggering in case anything else has corrupted the PWM outputs @@ -197,7 +159,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 @@ -213,27 +175,19 @@ 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: 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,30 +199,23 @@ 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): + 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: - if self._pwm_setup_failed: + 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: @@ -281,13 +228,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: - if self._pwm_setup_failed: + 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 @@ -299,31 +246,33 @@ 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: - if self._pwm_setup_failed: + def set_servoposition(self, channel: int | None = None, position: int | None = None) -> bool: + if not self._pwm_setup: return False if position is None: # position == None -> Turn off PWM (some servos will then turn off, others will stay in last position) if channel is None: # channel == None -> Turn off all PWM outputs - for 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[int(channel)].duty_ns(0) - if self._logging: - print(f"H:{self.config.port}:PWM[{int(channel)}]:Off") - except Exception as e: - print(f"H:{self.config.port}:PWM[{int(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: @@ -332,11 +281,12 @@ 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(): + 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: @@ -345,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 @@ -360,50 +310,61 @@ 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: - if self._pwm_setup_failed: + 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): return False if centre < (_SERVO_CENTRE - _SERVO_MAX_TRIM ) or centre > (_SERVO_CENTRE + _SERVO_MAX_TRIM): return False if channel is None: - self._servo_centre = [int(centre)] * 4 + self._servo_centre = [centre] * 4 else: - self._servo_centre[int(channel)] = int(centre) + self._servo_centre[channel] = centre return True # Set pairs of PWM duty cycles in one go using a signed value per motor channel (0-65535) def set_motors(self, outputs) -> bool: - if self._pwm_setup_failed: + if not self._pwm_setup: return False 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._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): @@ -414,8 +375,8 @@ 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: + def get_pwm(self, channel: int = 0) -> int: + if not self._pwm_setup: return 0 if channel >= len(self.PWMOutput): return 0 @@ -425,18 +386,91 @@ def get_pwm(self, channel=0) -> int: pwm = 0 return pwm +## Stepper Motor Support + # Stepper Motor Support - force output to a specific phase + def motor_step(self, phase: int): + if phase >= _STEPPER_NUM_PHASES: + return None + if not self._stepper: + # not currently configured for stepper motor - configure + self._pwm_deinit() + self._stepper = True + for channel, value in enumerate(self._step[phase]): + self.config.pin[channel].value(value) + self._outputs_energised = True + self._time_since_last_update = 0 + + 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, pwm in enumerate(self.PWMOutput): + if pwm is not None: + try: + pwm.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): 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'}") @@ -444,7 +478,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: @@ -458,7 +492,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)) @@ -500,57 +534,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: int , motors: int = 0, servos: int = 0, steppers: int = 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 diff --git a/linefollower.py b/linefollower.py new file mode 100644 index 0000000..c0c9a9f --- /dev/null +++ b/linefollower.py @@ -0,0 +1,2610 @@ +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 +from .gr10_30 import * + +#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 +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 = 10 # 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, 19] + +# 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 + self._output = (0,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._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) + 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 + 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") + output = (0,0) + # if _override is a bool + if type(self._override) is bool: + if not self._override: + # Line Follower Sensor + # 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._output + else: + 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) + 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_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}") + 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_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: + 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: + self._sample_time += delta + # 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 + # 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: + 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 + # 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): + 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: + self.hexdrive_app.set_logging(False) + 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: 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 + 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 + # "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, _): + # 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} {self._phase} spurious interrupt") + #self._phase = 4 + # not expecting an interrupt + return + #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 ######## + +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