From 72e777ac8f102ce9997b0eaf34f598e4588b4e8c Mon Sep 17 00:00:00 2001 From: Michel Date: Sun, 25 Sep 2022 22:29:01 +0200 Subject: [PATCH] v0.0.0 --- .../main.py | 141 ++++++++++++++++++ 1 file changed, 141 insertions(+) create mode 100644 Western Star 6900 TwinSteer by Nico71 (MOC-117722)/main.py diff --git a/Western Star 6900 TwinSteer by Nico71 (MOC-117722)/main.py b/Western Star 6900 TwinSteer by Nico71 (MOC-117722)/main.py new file mode 100644 index 0000000..85d9379 --- /dev/null +++ b/Western Star 6900 TwinSteer by Nico71 (MOC-117722)/main.py @@ -0,0 +1,141 @@ +################################################################################ +# # +# pybricks script for Western Star 6900 TwinSteer by Nico71 # +# # +# This script enables the LEGO® Powered Up Technic Hub to be used with the # +# LEGO® Powerd UP 88010 Remote. # +# # +# Configuration of the Technic Hub: # +# Port A: Steering with left and right limited # +# Port B: Winch # +# Port C: Drive motor # +# Port D: Drive motor # +# # +################################################################################ +# # +# Changelog # +# # +################################################################################ +# v0.0.0 21-08-2022 # +# First version. # +# Based on v0.1.0 05-07-2022 of 42124-1 - Off-Road Buggy # +################################################################################ + +from pybricks.pupdevices import Motor, Remote +from pybricks.parameters import Port, Direction, Stop, Button, Color +from pybricks.hubs import TechnicHub +from pybricks.tools import wait + +# Initialize the motors. +steering = Motor(Port.A, Direction.CLOCKWISE) +driving_1 = Motor(Port.C, Direction.CLOCKWISE) +driving_2 = Motor(Port.D, Direction.CLOCKWISE) +winch = Motor(Port.B, Direction.CLOCKWISE) + +# Connect to the remote. +remote = Remote() + +# Initialize the hub. +hub = TechnicHub() + +# Read the current settings +old_kp, old_ki, old_kd, _, _ = steering.control.pid() + +# Set new values +steering.control.pid(kp=old_kp*4, kd=old_kd*0.4) + +# Find the steering endpoint on the left and right. +# The middle is in between. +left_end = steering.run_until_stalled(-200, then=Stop.HOLD) +right_end = steering.run_until_stalled(200, then=Stop.HOLD) + +# We are now at the right. Reset this angle to be half the difference. +# That puts zero in the middle. +steering.reset_angle((right_end - left_end)/2) +steering.run_target(speed=200, target_angle=0, wait=False) + +# Set steering angle +steering_angle = (((right_end - left_end)/2)-5) + +# Set variable for gear +gear_total = 2 # Total number of gears. +gear_old = None # Empty variable to set later on (keep + # empty!). +gear = 1 # Gear at start (must be higher than 0 and + # lower or eaqual to gear_total). +gear_color = ["BLUE", "RED"] # Number of colors should at least be + # equal to gear_total. + +# Battery variables +voltage_current = hub.battery.voltage() +voltage = voltage_current + +while True: + # Check which buttons are pressed. + pressed = remote.buttons.pressed() + + # Choose the steering angle based on the right controls. + if Button.RIGHT_PLUS in pressed: + steering.run_target(1400, -steering_angle, Stop.HOLD, False) + elif Button.RIGHT_MINUS in pressed: + steering.run_target(1400, steering_angle, Stop.HOLD, False) + else: + steering.track_target(0) + + # Check if the right middle button is pressed to change gear + if Button.CENTER in pressed: + if gear_old < gear_total: + gear += 1 + else: + gear = 1 + while Button.CENTER in pressed: + # Button debounce + wait(10) + pressed = remote.buttons.pressed() + + # Set speed according to the choosen gear. + if gear is not gear_old: + remote.light.on(Color[gear_color[gear-1]]) + speed = (100/gear_total)*gear + gear_old = gear + + # Choose the drive speed based on the left controls. + drive_speed = 0 + if Button.LEFT_PLUS in pressed: + drive_speed += speed + elif Button.LEFT_MINUS in pressed: + drive_speed -= speed + + # Apply the selected speed. + driving_1.dc(drive_speed) + driving_2.dc(drive_speed) + + # Choose the winch speed based on the red buttons. + winch_speed = 0 + if Button.LEFT in pressed: + winch_speed = 100 + elif Button.RIGHT in pressed: + winch_speed -= 100 + + # Apply the selected speed. + winch.dc(winch_speed) + + # Show battery status by hub LED. + # Get current voltage and gradually up or down the value. + voltage_current = hub.battery.voltage() + if voltage_current < voltage: + voltage -= 1 + elif voltage_current > voltage: + voltage += 1 + # Show the correct color. + if voltage > 9000: + hub.light.on(Color.GREEN) + elif voltage > 8100: + hub.light.on(Color.YELLOW) + elif voltage > 7200: + hub.light.on(Color.ORANGE) + else: + hub.light.on(Color.RED) + + # Wait. + wait(10)