Michel/lego-pybricks-collection
1
0
Fork 0
Code Issues Pull Requests Activity

v0.0.0

Browse Source
This commit is contained in:
Michel
2022-06-20 12:24:39 +02:00
parent ec201226e8
commit b117e10a83
1 changed files with 172 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
42114-1 - 6x6 Volvo Articulated Hauler/main.py
+172
View File
@@ -0,0 +1,172 @@
################################################################################
# #
# pybricks script for 42114-1 - 6x6 Volvo Articulated Hauler #
# #
# 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: Drive motor #
# Port B: 4 seed gearbox with left and right limited #
# Port C: Not used #
# Port D: Steering with left and right limited #
# #
################################################################################
# Rebrickable: https://rebrickable.com/sets/42114-1/ #
# Script base: https://racingbrick.com/2021/08/remote-control-for-control-sets #
# -without-an-app-or-smartphone-pybricks/ #
################################################################################
# #
# Works with the following MOCs #
# #
################################################################################
# 42114 Volvo N10 6x4 Dump Truck - alternate build by timtimgo #
# https://rebrickable.com/mocs/MOC-81210/ #
################################################################################
# #
# Changelog #
# #
################################################################################
# v0.0.0 20-06-2022 #
# First version. #
# Based on v0.1.0 15-06-2022 of 42109 - RC Truck v.3 by ufotografol #
################################################################################
from pybricks.pupdevices import Motor, Remote
from pybricks.parameters import Port, Direction, Stop, Button, Color
from pybricks.tools import wait
# Initialize the motors.
gearbox = Motor(Port.B, Direction.CLOCKWISE)
steering = Motor(Port.D, Direction.COUNTERCLOCKWISE)
driving = Motor(Port.A, Direction.CLOCKWISE)
# Connect to the remote.
remote = Remote()
# Read the current settings
old_kp, old_ki, old_kd, _, _ = gearbox.control.pid()
old_kp, old_ki, old_kd, _, _ = steering.control.pid()
# Set new values
gearbox.control.pid(kp=old_kp*4, kd=old_kd*0.4)
steering.control.pid(kp=old_kp*4, kd=old_kd*0.4)
#Find the gearing endpoints but keep trying if value is below threshold of 200.
gear_angle = 0
while gear_angle < 200:
# Find the steering endpoint on the left and right.
# The middle is in between.
left_end_gearbox = gearbox.run_until_stalled(-200, then=Stop.HOLD)
right_end_gearbox = gearbox.run_until_stalled(200, then=Stop.HOLD)
# Set gearing angle
gear_angle = (((right_end_gearbox - left_end_gearbox)/2)-5)
print('Gear angle measured:',gear_angle)
# Find the steering endpoint on the left and right.
# The middle is in between.
left_end_steering = steering.run_until_stalled(-200, then=Stop.HOLD)
right_end_steering = steering.run_until_stalled(200, then=Stop.HOLD)
# Set gearing angle
# Real angle of gear mechanism is 270 degrees (135 both sides from center).
gear_angle = 270/2
gear_angle_1 = gear_angle
gear_angle_2 = gear_angle-(((gear_angle+gear_angle)/3)*1)
gear_angle_3 = gear_angle-(((gear_angle+gear_angle)/3)*2)
gear_angle_tipper = -gear_angle
print('Gear angle real:',gear_angle)
# Set steering angle
steer_angle = (((right_end_steering - left_end_steering)/2)-5)
print('steer angle:',steer_angle)
# We are now at the right. Reset this angle to be half the difference.
# That puts zero in the middle.
gearbox.reset_angle((right_end_gearbox - left_end_gearbox)/2)
gearbox.run_target(1400, gear_angle, Stop.HOLD, False)
steering.reset_angle((right_end_steering - left_end_steering)/2)
steering.run_target(speed=200, target_angle=0, wait=False)
# Set variable for gear
gear_old = 1
gear = 1
while True:
# Check which buttons are pressed.
pressed = remote.buttons.pressed()
# Check if the right middle button is pressed to change gear
if Button.RIGHT in pressed:
if gear_old is 1:
gear = 2
elif gear_old is 2:
gear = 3
elif gear_old is 3:
gear = 4
else:
gear = 1
while Button.RIGHT in pressed:
# Button debounce
wait(10)
pressed = remote.buttons.pressed()
# Check if the left middle button is pressed to change gear
if Button.LEFT in pressed:
if gear_old is 1:
gear = 4
elif gear_old is 2:
gear = 1
elif gear_old is 3:
gear = 2
else:
gear = 3
while Button.LEFT in pressed:
# Button debounce
wait(10)
pressed = remote.buttons.pressed()
if gear is not gear_old:
# Stop driving to preserve gears
driving.dc(0)
if gear is 1:
remote.light.on(Color.BLUE)
gearbox.run_target(1400, gear_angle_1, Stop.HOLD, False)
if gear_old is 4:
wait(500)
gear_old = 1
elif gear is 2:
remote.light.on(Color.WHITE)
gearbox.run_target(1400, gear_angle_2, Stop.HOLD, False)
gear_old = 2
elif gear is 3:
remote.light.on(Color.RED)
gearbox.run_target(1400, gear_angle_3, Stop.HOLD, False)
gear_old = 3
else:
remote.light.on(Color.GREEN)
gearbox.run_target(1400, gear_angle_tipper, Stop.HOLD, False)
if gear_old is 1:
wait(500)
gear_old = 4
# Wait before going on so changing gear won't happen while driving.
wait(250)
# Choose the steering angle based on the right controls.
if Button.RIGHT_MINUS in pressed:
steering.run_target(1400, -steer_angle, Stop.HOLD, False)
elif Button.RIGHT_PLUS in pressed:
steering.run_target(1400, steer_angle, Stop.HOLD, False)
else:
steering.track_target(0)
# Choose the drive speed based on the left controls.
if Button.LEFT_MINUS in pressed:
driving.dc(100)
elif Button.LEFT_PLUS in pressed:
driving.dc(-100)
else:
driving.dc(0)
# Wait.
wait(10)