diff --git a/LEGO Technic 42176 XBOX RC.py b/LEGO Technic 42176 XBOX RC.py index 5ef77b3..2ebe2fb 100644 --- a/LEGO Technic 42176 XBOX RC.py +++ b/LEGO Technic 42176 XBOX RC.py @@ -1,275 +1,280 @@ -# LEGO Technic Move Hub 88019 (released in LEGO Technic 42176) -# remote-control with XBOX controller -# Daniele Benedettelli @profbricks - 6 August 2024 -# requires pygame and bleak -# INSTALLATION: -# pip intall pygame -# pip install bleak - -class color: - # ANSI escape codes for colors - red = "\033[31m" # Red text - green = "\033[32m" # Green text - yellow = "\033[33m" # Yellow text - blue = "\033[34m" # Blue text - magenta = "\033[35m" # Magenta text - cyan = "\033[36m" # Cyan text - white = "\033[37m" # White text - reset = "\033[0m" # Reset to default color - -colour = color() - -import os, sys -os.environ['PYGAME_HIDE_SUPPORT_PROMPT'] = '1' -# crucial! otherwise Bleak will raise exception -# see https://bleak.readthedocs.io/en/latest/troubleshooting.html#windows-bugs - -if sys.platform == "win32": - sys.coinit_flags = 0 - -import pygame -import asyncio -from bleak import BleakScanner, BleakClient -import time - -start_time = 0 - -class TechnicMoveHub: - def __init__(self, device_name): - self.device_name = device_name - self.service_uuid = "00001623-1212-EFDE-1623-785FEABCD123" - self.char_uuid = "00001624-1212-EFDE-1623-785FEABCD123" - self.client = None - - self.LIGHTS_OFF_OFF = 0b100 - self.LIGHTS_OFF_ON = 0b00000001 - self.LIGHTS_ON_ON = 0b000 - - def run_discover(self): - try: - devices = BleakScanner.discover(timeout=40) - return devices - except Exception as e: - print(f"Discovery failed with error: {e}") - return None - - async def scan_and_connect(self): - scanner = BleakScanner() - print(colour.green + f"Searching for Technic Move Hub...\nPlease press the on button to connect." + colour.reset) - devices = await scanner.discover(timeout =5) - - for device in devices: - if device.name is not None and self.device_name in device.name: - print(colour.green + f"Found device: {device.name} with address: {device.address} Connecting..." + colour.reset) - self.client = BleakClient(device) - - - await self.client.connect() - if self.client.is_connected: - print(colour.green + f"Connected to {self.device_name}" + colour.reset) - - paired = await self.client.pair(protection_level = 2) # this is crucial!!! - if not paired: - print(colour.red + f"Could Not Pair. Things could go wrong." + colour.reset) - return True - else: - print(colour.red + f"Failed to connect to {self.device_name}" + colour.reset) - print(colour.red + f"Device {self.device_name} not found." + colour.reset) - return False - - async def send_data(self, data): - global start_time - if self.client is None: - print("No BLE client connected.") - return - - try: - # Write the data to the characteristic - await self.client.write_gatt_char(self.char_uuid, data) - #print(f"Data written to characteristic {self.char_uuid}: {data}") - - elapsed_time_ms = (time.time() - start_time) * 1000 - #print(f"Timestamp: {elapsed_time_ms:.2f} ms", end=" ") - #print(' '.join(f'{byte:02x}' for byte in data)) - - except Exception as e: - print(f"Failed to write data: {e}") - - async def disconnect(self): - if self.client and self.client.is_connected: - await self.client.disconnect() - print("Disconnected from the device") - - LED_MODE_COLOR = 0x00 - LED_MODE_RGB = 0x01 - - async def change_led_color(self, colorID): - if self.client and self.client.is_connected: - await self.send_data(bytearray([0x08, 0x00, 0x81, self.ID_LED, self.IO_TYPE_RGB_LED, 0x51, self.LED_MODE_COLOR, colorID])) - - async def motor_start_power(self, motor, power): - if self.client and self.client.is_connected: - await self.send_data(bytearray([0x08, 0x00, 0x81, motor&0xFF, self.SC_BUFFER_NO_FEEDBACK, 0x51, self.MOTOR_MODE_POWER, 0xFF&power])) - - async def motor_stop(self, motor, brake=True): - # motor can be 0x32, 0x33, 0x34 - if self.client and self.client.is_connected: - await self.send_data(bytearray([0x08, 0x00, 0x81, motor&0xFF, self.SC_BUFFER_NO_FEEDBACK, 0x51, self.MOTOR_MODE_POWER, self.END_STATE_BRAKE if brake else 0x00])) - - async def calibrate_steering(self): - await self.send_data(bytes.fromhex("0d008136115100030000001000")) - #await asyncio.sleep(0.1) - await self.send_data(bytes.fromhex("0d008136115100030000000800")) - #await asyncio.sleep(0.1) - - async def drive(self, speed=0, angle=0, lights = 0x00): - await self.send_data(bytearray([0x0d,0x00,0x81,0x36,0x11,0x51,0x00,0x03,0x00, speed&0xFF, angle&0xFF, lights&0xFF,0x00])) - #await asyncio.sleep(0.1) - - -def get_left_joystick(joystick): - x = round(joystick.get_axis(0)*100) - y = -round(joystick.get_axis(1)*100) - return (x,y) - -def get_right_joystick(joystick): - x = round(joystick.get_axis(2)*100) - y = -round(joystick.get_axis(3)*100) - return (x,y) - -def get_triggers(joystick): - left = round((joystick.get_axis(4)+100)/2) - right = round((joystick.get_axis(5)+100)/2) - return (left, right) - - -def get_A_button(joystick): - return joystick.get_button(0) - -def get_B_button(joystick): - return joystick.get_button(1) - -def get_X_button(joystick): - return joystick.get_button(2) - -def get_Y_button(joystick): - return joystick.get_button(3) - -def get_left_bumper(joystick): - return joystick.get_button(4) - -def get_right_bumper(joystick): - return joystick.get_button(5) - - -async def main(): - device_name = "Technic Move" # Replace with your BLE device's name - hub = TechnicMoveHub(device_name) - if not await hub.scan_and_connect(): - print(colour.red + "Technic hub not found! Exiting... \nPlease make sure the Technic Hub is on then re-run the script." + colour.reset) - return - - # Initialize Pygame - pygame.init() - pygame.joystick.init() - - # Check for joystick - if pygame.joystick.get_count() == 0: - print("No Controller found! Exiting...\nPlease make sure that you have connected a controller then re-run the script.") - return - - print(colour.green + "Controller Connected!" + colour.reset) - - # Initialize the first joystick - joystick = pygame.joystick.Joystick(0) - joystick.init() - - print(f"Controller name: {joystick.get_name()}") - - await hub.calibrate_steering() - - lights = hub.LIGHTS_ON_ON - toggle_old = False - throttle_old = 0 - steering_old = 0 - lights_old = 0 - was_brake = False - start_time = time.time() - slow_mode = False - slow_mode_old = False - - try: - while True: - # Pump Pygame event loop - pygame.event.pump() # poll joystick - - # Print controller inputs - throttle = get_right_joystick(joystick)[1] - steering = get_left_joystick(joystick)[0] - #steering = get_right_joystick(joystick)[0] # use only one joystick? - - if abs(throttle)<3: - throttle = 0 - if abs(steering)< 3: - steering = 0 - - brake = get_right_bumper(joystick) - # toggle lights - toggle = get_Y_button(joystick) - if toggle and not toggle_old: - if lights == hub.LIGHTS_OFF_OFF : - print("lights on") - lights = hub.LIGHTS_ON_ON - else: - print("lights off") - lights = hub.LIGHTS_OFF_OFF - toggle_old = toggle - - slow_mode_cont = get_X_button(joystick) - if slow_mode_cont and not slow_mode_old: - if slow_mode == True: - print("slowmode off") - slow_mode = False - else: - print("slowmode on") - slow_mode = True - - - if brake and not was_brake: - joystick.rumble(0.0, 0.3, 300) - await hub.drive(0, steering, hub.LIGHTS_OFF_ON) - await asyncio.sleep(0.4) - throttle = 0 - throttle_old = 0 - - if not brake and was_brake: - await hub.drive(throttle, steering, lights) - - was_brake = brake - - if steering != steering_old or throttle != throttle_old or lights != lights_old and not brake: - print("throttle", throttle, "steering", steering) - if slow_mode == True: - throttle = round(throttle / 2) - await hub.drive(throttle, steering, lights) - else: - await hub.drive(throttle, steering, lights) - - throttle_old = throttle - steering_old = steering - lights_old = lights - slow_mode_old = slow_mode - - # Flush the output - sys.stdout.flush() - - asyncio.sleep(0.05) - - except KeyboardInterrupt: - pass - finally: - pygame.quit() - -if __name__ == "__main__": - asyncio.run(main()) - +# LEGO Technic Move Hub 88019 (released in LEGO Technic 42176) +# remote-control with XBOX controller +# Daniele Benedettelli @profbricks - 6 August 2024 +# requires pygame and bleak +# INSTALLATION: +# pip intall pygame +# pip install bleak + +class color: + # ANSI escape codes for colors + red = "\033[31m" # Red text + green = "\033[32m" # Green text + yellow = "\033[33m" # Yellow text + blue = "\033[34m" # Blue text + magenta = "\033[35m" # Magenta text + cyan = "\033[36m" # Cyan text + white = "\033[37m" # White text + reset = "\033[0m" # Reset to default color + +colour = color() + +import os, sys +os.environ['PYGAME_HIDE_SUPPORT_PROMPT'] = '1' +# crucial! otherwise Bleak will raise exception +# see https://bleak.readthedocs.io/en/latest/troubleshooting.html#windows-bugs + +if sys.platform == "win32": + sys.coinit_flags = 0 + +import pygame +import asyncio +from bleak import BleakScanner, BleakClient +import time + +start_time = 0 + +class TechnicMoveHub: + def __init__(self, device_name): + self.device_name = device_name + self.service_uuid = "00001623-1212-EFDE-1623-785FEABCD123" + self.char_uuid = "00001624-1212-EFDE-1623-785FEABCD123" + self.client = None + + self.LIGHTS_OFF_OFF = 0b100 + self.LIGHTS_OFF_ON = 0b00000001 + self.LIGHTS_ON_ON = 0b000 + self.ID_LED = 0x11 + + def run_discover(self): + try: + devices = BleakScanner.discover(timeout=40) + return devices + except Exception as e: + print(f"Discovery failed with error: {e}") + return None + + async def scan_and_connect(self): + scanner = BleakScanner() + print(colour.green + f"Searching for Technic Move Hub...\nPlease press the on button to connect." + colour.reset) + devices = await scanner.discover(timeout =5) + + for device in devices: + if device.name is not None and self.device_name in device.name: + print(colour.green + f"Found device: {device.name} with address: {device.address} Connecting..." + colour.reset) + self.client = BleakClient(device) + + + await self.client.connect() + if self.client.is_connected: + print(colour.green + f"Connected to {self.device_name}" + colour.reset) + + paired = await self.client.pair(protection_level = 2) # this is crucial!!! + if not paired: + print(colour.red + f"Could Not Pair. Things could go wrong." + colour.reset) + return True + else: + print(colour.red + f"Failed to connect to {self.device_name}" + colour.reset) + print(colour.red + f"Device {self.device_name} not found." + colour.reset) + return False + + async def send_data(self, data): + global start_time + if self.client is None: + print("No BLE client connected.") + return + + try: + # Write the data to the characteristic + await self.client.write_gatt_char(self.char_uuid, data) + #print(f"Data written to characteristic {self.char_uuid}: {data}") + + elapsed_time_ms = (time.time() - start_time) * 1000 + #print(f"Timestamp: {elapsed_time_ms:.2f} ms", end=" ") + #print(' '.join(f'{byte:02x}' for byte in data)) + + except Exception as e: + print(f"Failed to write data: {e}") + + async def disconnect(self): + if self.client and self.client.is_connected: + await self.client.disconnect() + print("Disconnected from the device") + + LED_MODE_COLOR = 0x00 + LED_MODE_RGB = 0x01 + + async def change_led_color(self, colorID): + if self.client and self.client.is_connected: + await self.send_data(bytearray([0x08, 0x00, 0x81, self.ID_LED, self.IO_TYPE_RGB_LED, 0x51, self.LED_MODE_COLOR, colorID])) + + async def motor_start_power(self, motor, power): + if self.client and self.client.is_connected: + await self.send_data(bytearray([0x08, 0x00, 0x81, motor&0xFF, self.SC_BUFFER_NO_FEEDBACK, 0x51, self.MOTOR_MODE_POWER, 0xFF&power])) + + async def motor_stop(self, motor, brake=True): + # motor can be 0x32, 0x33, 0x34 + if self.client and self.client.is_connected: + await self.send_data(bytearray([0x08, 0x00, 0x81, motor&0xFF, self.SC_BUFFER_NO_FEEDBACK, 0x51, self.MOTOR_MODE_POWER, self.END_STATE_BRAKE if brake else 0x00])) + + async def calibrate_steering(self): + await self.send_data(bytes.fromhex("0d008136115100030000001000")) + #await asyncio.sleep(0.1) + await self.send_data(bytes.fromhex("0d008136115100030000000800")) + #await asyncio.sleep(0.1) + + async def drive(self, speed=0, angle=0, lights = 0x00): + await self.send_data(bytearray([0x0d,0x00,0x81,0x36,0x11,0x51,0x00,0x03,0x00, speed&0xFF, angle&0xFF, lights&0xFF,0x00])) + #await asyncio.sleep(0.1) + + +def get_left_joystick(joystick): + x = round(joystick.get_axis(0)*100) + y = -round(joystick.get_axis(1)*100) + return (x,y) + +def get_right_joystick(joystick): + x = round(joystick.get_axis(2)*100) + y = -round(joystick.get_axis(3)*100) + return (x,y) + +def get_triggers(joystick): + left = round((joystick.get_axis(4)+100)/2) + right = round((joystick.get_axis(5)+100)/2) + return (left, right) + + +def get_A_button(joystick): + return joystick.get_button(0) + +def get_B_button(joystick): + return joystick.get_button(1) + +def get_X_button(joystick): + return joystick.get_button(2) + +def get_Y_button(joystick): + return joystick.get_button(3) + +def get_left_bumper(joystick): + return joystick.get_button(4) + +def get_right_bumper(joystick): + return joystick.get_button(5) + + +async def main(): + device_name = "Technic Move" # Replace with your BLE device's name + hub = TechnicMoveHub(device_name) + if not await hub.scan_and_connect(): + print(colour.red + "Technic hub not found! Exiting... \nPlease make sure the Technic Hub is on then re-run the script." + colour.reset) + return + + # Initialize Pygame + pygame.init() + pygame.joystick.init() + + # Check for joystick + if pygame.joystick.get_count() == 0: + print("No Controller found! Exiting...\nPlease make sure that you have connected a controller then re-run the script.") + return + + print(colour.green + "Controller Connected!" + colour.reset) + + # Initialize the first joystick + joystick = pygame.joystick.Joystick(0) + joystick.init() + + print(f"Controller name: {joystick.get_name()}") + + await hub.calibrate_steering() + + lights = hub.LIGHTS_ON_ON + toggle_old = False + throttle_old = 0 + steering_old = 0 + lights_old = 0 + was_brake = False + start_time = time.time() + slow_mode = False + slow_mode_old = False + change_lights_because_of_slowmode_on = False + change_lights_because_of_slowmode_off = False + + try: + while True: + # Pump Pygame event loop + pygame.event.pump() # poll joystick + + # Print controller inputs + throttle = get_right_joystick(joystick)[1] + steering = get_left_joystick(joystick)[0] + #steering = get_right_joystick(joystick)[0] # use only one joystick? + + if abs(throttle)<3: + throttle = 0 + if abs(steering)< 3: + steering = 0 + + brake = get_right_bumper(joystick) + # toggle lights + toggle = get_Y_button(joystick) + if toggle and not toggle_old: + if lights == hub.LIGHTS_OFF_OFF : + print("lights on") + lights = hub.LIGHTS_ON_ON + else: + print("lights off") + lights = hub.LIGHTS_OFF_OFF + toggle_old = toggle + + slow_mode_cont = get_X_button(joystick) + if slow_mode_cont and not slow_mode_old: + if slow_mode == True: + print("slowmode off") + slow_mode = False + #await hub.change_led_color(hub.LIGHTS_OFF_OFF) + else: + print("slowmode on") + slow_mode = True + #await hub.change_led_color(hub.LIGHTS_OFF_ON) + slow_mode_old = slow_mode_cont + + + if brake and not was_brake: + joystick.rumble(0.0, 0.3, 300) + await hub.drive(0, steering, hub.LIGHTS_OFF_ON) + await asyncio.sleep(0.4) + throttle = 0 + throttle_old = 0 + + if not brake and was_brake: + await hub.drive(throttle, steering, lights) + + was_brake = brake + + if steering != steering_old or throttle != throttle_old or lights != lights_old and not brake: + print("throttle", throttle, "steering", steering) + if slow_mode == True: + throttle = round(throttle / 2) + await hub.drive(throttle, steering, lights) + else: + await hub.drive(throttle, steering, lights) + + throttle_old = throttle + steering_old = steering + lights_old = lights + + # Flush the output + sys.stdout.flush() + + asyncio.sleep(0.05) + + except KeyboardInterrupt: + pass + finally: + pygame.quit() + +if __name__ == "__main__": + asyncio.run(main()) +