diff --git a/examples/showcase/rover/lib/commander.py b/examples/showcase/rover/lib/commander.py index b564438..93def0b 100644 --- a/examples/showcase/rover/lib/commander.py +++ b/examples/showcase/rover/lib/commander.py @@ -4,6 +4,18 @@ from pimoroni_yukon.timing import ticks_diff, ticks_ms + +""" +JoyBTCommander is a class for communicating with an Android-based +bluetooth serial controller app of the same name. This app provides +a virtual joystick and up to 6 buttons on a smart phone's touch screen. + +This Micropython implementation is a port of an Arduino implmentation +originally created by Christopher "ZodiusInfuser" Parrott, found at: +https://github.com/ZodiusInfuser/JoyBTCommander +""" + + class JoyBTCommander(): STX = 0x02 ETX = 0x03 @@ -11,29 +23,27 @@ class JoyBTCommander(): BUTTON_COUNT = 6 DEFAULT_COMMS_TIMEOUT = 1.0 - def __init__(self, uart, timeout=DEFAULT_COMMS_TIMEOUT): + def __init__(self, uart, no_comms_timeout=DEFAULT_COMMS_TIMEOUT): self.__uart = uart - self.__no_comms_timeout = int(timeout * 1000) + self.__no_comms_timeout_ms = int(no_comms_timeout * 1000) self.__no_comms_timeout_callback = None - self.__last_received_millis = 0 - self.__timeout_reached = True + self.__last_received_ms = 0 + self.__timeout_reached = True # Set as true initially so the timeout callback does not get called immediately self.__receiving = False self.__data_index = 0 self.__in_buffer = bytearray(self.BUFFER_SIZE) - self.__button_state = [False, False, False, False, False, False] + self.__button_state = [False] * self.BUTTON_COUNT + self.__momentary_button = [False] * self.BUTTON_COUNT + self.__button_pressed_callback = [None] * self.BUTTON_COUNT + self.__button_released_callback = [None] * self.BUTTON_COUNT self.__joystick_x = 0.0 self.__joystick_y = 0.0 self.__joystick_callback = None - @property - def joystick(self): - return self.__joystick_x, self.__joystick_y - - def begin(self): - # Clear the buffer + # Clear the receive buffer while self.__uart.any() > 0: self.__uart.read() @@ -52,10 +62,10 @@ def check_receive(self): self.__receiving = False elif rx_byte == self.ETX: if self.__data_index == 1: - self.decode_button_state(self.__in_buffer[0]) # 3 Bytes ex: < STX "C" ETX > + self.__decode_button_state(self.__in_buffer[0]) # 3 Bytes ex: < STX "C" ETX > elif self.__data_index == 6: - self.decode_joystick_state(self.__in_buffer) # 6 Bytes ex: < STX "200" "180" ETX > - self.__last_received_millis = ticks_ms() + self.__decode_joystick_state(self.__in_buffer) # 6 Bytes ex: < STX "200" "180" ETX > + self.__last_received_ms = ticks_ms() self.__timeout_reached = False self.__receiving = False @@ -64,21 +74,73 @@ def check_receive(self): self.__data_index += 1 current_millis = ticks_ms() - if (ticks_diff(current_millis, self.__last_received_millis) > self.__no_comms_timeout) and not self.__timeout_reached: - print("here") - if self.__no_comms_timeout_callback != None: + if (ticks_diff(current_millis, self.__last_received_ms) > self.__no_comms_timeout_ms) and not self.__timeout_reached: + if self.__no_comms_timeout_callback is not None: self.__no_comms_timeout_callback() self.__timeout_reached = True - def button_state(self, button): + def send_fields(self, data_field1="XXX", data_field2="XXX", data_field3="XXX"): + # Data frame transmitted back from Micropython to Android device: + # < 0X02 Buttons state 0X01 DataField#1 0x04 DataField#2 0x05 DataField#3 0x03 > + # < 0X02 "01011" 0X01 "120.00" 0x04 "-4500" 0x05 "Motor enabled" 0x03 > // example + + self.__uart.write(self.STX) # Start transmission + self.__uart.write(self.__button_states_to_string()) # Button state feedback + self.__uart.write(0x1) + self.__uart.write(str(data_field1)) # Data Field #1 + self.__uart.write(0x4) + self.__uart.write(str(data_field2)) # Data Field #2 + self.__uart.write(0x5) + self.__uart.write(str(data_field3)) # Data Field #3 + self.__uart.write(self.ETX) # End transmission + + def is_button_pressed(self, button): + if button < 0 or button >= self.BUTTON_COUNT: + raise ValueError(f"button out of range. Expected 0 to {self.BUTTON_COUNT - 1}") + return self.__button_state[button] - def set_button_state(self, button, pressed): + def set_button_pressed(self, button, pressed): + if button < 0 or button >= self.BUTTON_COUNT: + raise ValueError(f"button out of range. Expected 0 to {self.BUTTON_COUNT - 1}") + if pressed: - self.handle_button_press(button) + self.__handle_button_press(button) else: - self.handle_button_release(button) + self.__handle_button_release(button) + + def is_momentary_button(self, button): + if button < 0 or button >= self.BUTTON_COUNT: + raise ValueError(f"button out of range. Expected 0 to {self.BUTTON_COUNT - 1}") + + return self.__momentary_button[button] + + def set_button_as_momentary(self, button, momentary): + if button < 0 or button >= self.BUTTON_COUNT: + raise ValueError(f"button out of range. Expected 0 to {self.BUTTON_COUNT - 1}") + + self.__momentary_button[button] = momentary + + @property + def joystick_x(self): + return self.__joystick_x + + @property + def joystick_y(self): + return self.__joystick_y + + def set_button_pressed_callback(self, button, button_callback): + if button < 0 or button >= self.BUTTON_COUNT: + raise ValueError(f"button out of range. Expected 0 to {self.BUTTON_COUNT - 1}") + + self.__button_pressed_callback[button] = button_callback + + def set_button_released_callback(self, button, button_callback): + if button < 0 or button >= self.BUTTON_COUNT: + raise ValueError(f"button out of range. Expected 0 to {self.BUTTON_COUNT - 1}") + + self.__button_released_callback[button] = button_callback def set_timeout_callback(self, timeout_callback): self.__no_comms_timeout_callback = timeout_callback @@ -86,68 +148,78 @@ def set_timeout_callback(self, timeout_callback): def set_joystick_callback(self, joystick_callback): self.__joystick_callback = joystick_callback - def decode_button_state(self, data): + def __decode_button_state(self, data): # ----------------- BUTTON #1 ----------------------- if data == ord('A'): - self.handle_button_press(0) + self.__handle_button_press(0) elif data == ord('B'): - self.handle_button_release(0) + self.__handle_button_release(0) # ----------------- BUTTON #2 ----------------------- elif data == ord('C'): - self.handle_button_press(1) + self.__handle_button_press(1) elif data == ord('D'): - self.handle_button_release(1) + self.__handle_button_release(1) # ----------------- BUTTON #3 ----------------------- elif data == ord('E'): - self.handle_button_press(2) + self.__handle_button_press(2) elif data == ord('F'): - self.handle_button_release(2) + self.__handle_button_release(2) # ----------------- BUTTON #4 ----------------------- elif data == ord('G'): - self.handle_button_press(3) + self.__handle_button_press(3) elif data == ord('H'): - self.handle_button_release(3) + self.__handle_button_release(3) # ----------------- BUTTON #5 ----------------------- elif data == ord('I'): - self.handle_button_press(4) + self.__handle_button_press(4) elif data == ord('J'): - self.handle_button_release(4) + self.__handle_button_release(4) # ----------------- BUTTON #6 ----------------------- elif data == ord('K'): - self.handle_button_press(5) + self.__handle_button_press(5) elif data == ord('L'): - self.handle_button_release(5) - - def decode_joystick_state(self, rx_byte): - joy_x = (rx_byte[0] - 48) * 100 + (rx_byte[1] - 48) * 10 + (rx_byte[2] - 48) # obtain the Int from the ASCII representation + self.__handle_button_release(5) + + def __decode_joystick_state(self, rx_byte): + # Obtain the int from the ASCII representation + joy_x = (rx_byte[0] - 48) * 100 + (rx_byte[1] - 48) * 10 + (rx_byte[2] - 48) joy_y = (rx_byte[3] - 48) * 100 + (rx_byte[4] - 48) * 10 + (rx_byte[5] - 48) - joy_x = joy_x - 200; # Offset to avoid - joy_y = joy_y - 200; # transmitting negative numbers + + # Offset to avoid transmitting negative numbers + joy_x = joy_x - 200 + joy_y = joy_y - 200 if joy_x < -100 or joy_x > 100 or joy_y < -100 or joy_y > 100: - return # commmunication error + return # Invalid data, so just ignore it self.__joystick_x = float(joy_x) / 100.0 self.__joystick_y = float(joy_y) / 100.0 - if self.__joystick_callback != None: + if self.__joystick_callback is not None: self.__joystick_callback(self.__joystick_x, self.__joystick_y) - def handle_button_press(self, button): - self.__button_state[button] = True + def __handle_button_press(self, button): + if not self.__momentary_button[button]: # Only set the button state to pressed if the button is not momentary + self.__button_state = True + + if self.__button_pressed_callback[button] is not None: + self.__button_pressed_callback[button]() + + def __handle_button_release(self, button): + self.__button_state = False - def handle_button_release(self, button): - self.__button_state[button] = False + if self.__button_released_callback[button] is not None: + self.__button_released_callback[button]() - def button_states_to_string(self): + def __button_states_to_string(self): state = "" for i in range(0, len(self.__button_state)): - if self.__button_state[i]: + if self.__button_state[i] is True: state += "1" else: state += "0" diff --git a/examples/showcase/rover/main.py b/examples/showcase/rover/main.py index 930b8c7..cd6cf49 100644 --- a/examples/showcase/rover/main.py +++ b/examples/showcase/rover/main.py @@ -12,16 +12,16 @@ from commander import JoyBTCommander # Constants -UPDATES = 50 # How many times to update LEDs and Servos per second +UPDATES = 50 # How many times to update motors and LEDs per second TIMESTEP = 1 / UPDATES TIMESTEP_MS = int(TIMESTEP * 1000) -MOTOR_EXTENT = 0.4 # How far from zero to drive the motors +MOTOR_SPEED = 0.4 # The top speed to drive each motor at STRIP_TYPE = LEDStripModule.NEOPIXEL # The type of LED strip being driven STRIP_PIO = 0 # The PIO system to use (0 or 1) to drive the strip STRIP_SM = 0 # The State Machines (SM) to use to drive the strip LEDS_PER_STRIP = 120 # How many LEDs are on the strip -PULSE_TIME = 2.0 # The time to perform a complete pulse of the LEDs when motors_active +SPEED_HUE_RANGE = 1.5 # The speed range that will result in the full green -> blue -> red hue range BT_UART_ID = 1 # The ID of the hardware UART to use for bluetooth comms via a serial tranceiver BT_BAUDRATE = 9600 # The baudrate of the bluetooth serial tranceiver's serial @@ -49,40 +49,46 @@ exited_due_to_low_voltage = True # Record if the program exited due to low voltage (assume true to start) +# Function for mapping a value from one range to another +def map_float(input, in_min, in_max, out_min, out_max): + return (((input - in_min) * (out_max - out_min)) / (in_max - in_min)) + out_min + + +# Function that gets called when no communication have been received for a given time def no_comms_callback(): # Disable both motors, causing them to coast to a stop left_driver.motor.disable() right_driver.motor.disable() +# Function that gets called when new joystick data is received def joystick_callback(x, y): x *= y # Prevent turning on the spot (which the chassis cannot achieve) by scaling the side input by the forward input # Update the left and right motor speeds based on the forward and side inputs - left_speed = -y-x - right_speed = y-x - left_driver.motor.speed(left_speed * MOTOR_EXTENT) - right_driver.motor.speed(right_speed * MOTOR_EXTENT) + left_speed = -y - x + right_speed = y - x + left_driver.motor.speed(left_speed * MOTOR_SPEED) + right_driver.motor.speed(right_speed * MOTOR_SPEED) MID_LED = led_module.strip.num_leds() // 2 # Update the left side LEDs to a colour based on the left speed - lefthue = map_float(left_speed, 1.5, -1.5, 0.999, 0.333) + left_hue = map_float(left_speed, SPEED_HUE_RANGE, -SPEED_HUE_RANGE, 0.999, 0.333) for led in range(0, MID_LED): - led_module.strip.set_hsv(led, lefthue, 1.0, 1.0) + led_module.strip.set_hsv(led, left_hue, 1.0, 1.0) # Update the right side LEDs to a colour based on the right speed - righthue = map_float(right_speed, -1.5, 1.5, 0.999, 0.333) + right_hue = map_float(right_speed, -SPEED_HUE_RANGE, SPEED_HUE_RANGE, 0.999, 0.333) for led in range(MID_LED, led_module.strip.num_leds()): - led_module.strip.set_hsv(led, righthue, 1.0, 1.0) + led_module.strip.set_hsv(led, right_hue, 1.0, 1.0) led_module.strip.update() # Send the new colours to the LEDs -# Function for mapping a value from one range to another -def map_float(input, in_min, in_max, out_min, out_max): - return (((input - in_min) * (out_max - out_min)) / (in_max - in_min)) + out_min - +# Assign timeout and joystick callbacks to the controller +controller.set_timeout_callback(no_comms_callback) +controller.set_joystick_callback(joystick_callback) # Ensure the input voltage is above the low level if yukon.read_input_voltage() > LOW_VOLTAGE_LEVEL: @@ -95,11 +101,6 @@ def map_float(input, in_min, in_max, out_min, out_max): yukon.register_with_slot(right_driver, RIGHT_SLOT) yukon.register_with_slot(led_module, LED_SLOT) - # Assign timeout and joystick callbacks to the controller, and start it - controller.set_timeout_callback(no_comms_callback) - controller.set_joystick_callback(joystick_callback) - controller.begin() - yukon.verify_and_initialise() # Verify that modules are attached to Yukon, and initialise them yukon.enable_main_output() # Turn on power to the module slots @@ -113,10 +114,10 @@ def map_float(input, in_min, in_max, out_min, out_max): # Loop until the BOOT/USER button is pressed while not yukon.is_boot_pressed(): - controller.check_receive() - print(controller.button_states_to_string(), controller.joystick[0], controller.joystick[1], sep=", ") + controller.check_receive() # Check the controller for any new inputs + print(f"LSpeed = {left_driver.motor.speed()}, RSpeed = {right_driver.motor.speed()}", end=", ") - # Perform a pulsing animation on the LEDs if there is no controller connection + # Set the LEDs to a static colour if there is no controller connected if not controller.is_connected(): # Update all the LEDs to show the same colour for led in range(led_module.strip.num_leds()): @@ -133,15 +134,17 @@ def map_float(input, in_min, in_max, out_min, out_max): except RuntimeError as e: left_driver.disable() right_driver.disable() - import time + led_module.disable() print(str(e)) time.sleep(1.0) yukon.enable_main_output() left_driver.enable() right_driver.enable() + led_module.enable() # Get the average voltage recorded from monitoring, and print it out - avg_voltage = yukon.get_readings()["Vi_avg"] + readings = yukon.get_readings() + avg_voltage = readings["Vi_avg"] print(f"V = {avg_voltage}") # Check if the average input voltage was below the low voltage level @@ -149,6 +152,14 @@ def map_float(input, in_min, in_max, out_min, out_max): exited_due_to_low_voltage = True break # Break out of the loop + # Convert the average voltage, current, and temperature to text to display on the controller + voltage_text = "{:.2f}V".format(round(readings["Vi_avg"], 2)) + current_text = "{:.2f}A".format(round(readings["C_avg"], 2)) + temperature_text = "{:.2f}°C".format(round(readings["T_avg"], 2)) + + # Send the converted data back to the controller + controller.sendFields(voltage_text, current_text, temperature_text) + finally: # Put the board back into a safe state, regardless of how the program may have ended yukon.reset() @@ -166,4 +177,3 @@ def map_float(input, in_min, in_max, out_min, out_max): time.sleep(BUZZER_PERIOD * BUZZER_DUTY) buzzer.off() time.sleep(BUZZER_PERIOD * (1.0 - BUZZER_DUTY)) -