Large overhaul, added 2 speed gearbox

42099 B model 'Rocky' by Didumos (MOC-30630)/main.py

@@ -13,20 +13,14 @@
 #                                                                              #
 ################################################################################
 # Rebrickable: https://rebrickable.com/mocs/MOC-30630                          #
-# Script base: https://racingbrick.com/2021/08/remote-control-for-control-sets #
-#                -without-an-app-or-smartphone-pybricks/                       #
 ################################################################################
-#                                                                              #
-#                                   Changelog                                  #
-#                                                                              #
-################################################################################
-# v0.0.0 20-06-2022                                                            #
-#   First version.                                                             #
-#   Based on v0.0.0 20-06-2022 of 42129 B model 'Hot Trot' by Didumos          #
+# LEGO® is a trademark of the LEGO Group of companies which does not sponsor,  #
+# authorize or endorse this project.                                           #
 ################################################################################
 
 from pybricks.pupdevices import Motor, Remote
-from pybricks.parameters import Port, Direction, Stop, Button
+from pybricks.parameters import Port, Direction, Stop, Button, Color
+from pybricks.hubs import TechnicHub
 from pybricks.tools import wait
 
 # Initialize the motors.
@@ -37,11 +31,8 @@ driving_2 = Motor(Port.B, Direction.CLOCKWISE)
 # Connect to the remote.
 remote = Remote()
 
-# 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)
+# Initialize the hub.
+hub = TechnicHub()
 
 # Find the steering endpoint on the left and right.
 # The middle is in between.
@@ -54,31 +45,84 @@ steering.reset_angle((right_end - left_end)/2)
 steering.run_target(speed=200, target_angle=0, wait=False)
 
 # Set steering angle
-steer_angle = (((right_end - left_end)/2)-5)
-print('steer angle:',steer_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_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)
+    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 left middle button is pressed to change gear
+    if Button.LEFT in pressed:
+        if gear_old > 1:
+            gear -= 1
+        while Button.LEFT in pressed:
+            # Button debounce 
+            wait(10)
+            pressed = remote.buttons.pressed()
+
+    # Check if the right middle button is pressed to change gear
+    if Button.RIGHT in pressed:
+        if gear_old < gear_total:
+            gear += 1
+        while Button.RIGHT 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.
-    if Button.LEFT_MINUS in pressed:
-        driving_1.dc(100)
-        driving_2.dc(100)
-    elif Button.LEFT_PLUS in pressed:
-        driving_1.dc(-100)
-        driving_2.dc(-100)
+    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)
+    
+    # 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:
-        driving_1.dc(0)
-        driving_2.dc(0)
+        hub.light.on(Color.RED)
 
     # Wait.
-    wait(10)
+    wait(10)