Add discovery-phase/missions/mission_9.py

discovery-phase/missions/mission_9.py

@@ -0,0 +1,108 @@
+import umath
+from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
+from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
+from pybricks.tools import run_task, multitask
+from pybricks.tools import wait, StopWatch
+from pybricks.robotics import DriveBase
+from pybricks.hubs import PrimeHub
+
+
+# Initialize hub and devices
+hub = PrimeHub()
+left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
+right_motor = Motor(Port.B,Direction.CLOCKWISE) # Specify default direction
+left_arm = Motor(Port.C, Direction.CLOCKWISE, [[12,36]],[[12,20,24]] ) # Specify default direction
+right_arm = Motor(Port.D, Direction.CLOCKWISE,[[12,36]],[[12,20,24]]) #Added gear train list for gear ration
+lazer_ranger = UltrasonicSensor(Port.E)
+color_sensor = ColorSensor(Port.F)
+
+
+# DriveBase configuration
+WHEEL_DIAMETER = 62.4  # mm (adjust for your wheels)
+AXLE_TRACK = 150  # mm (distance between wheels)
+drive_base = DriveBase(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK)
+drive_base.settings() #settings(600, 500, 300, 200)
+drive_base.use_gyro(True)
+
+
+"""
+Debugging helps
+"""
+DEBUG = 1  # Enable when you want to show logs
+# Example conversion function (adjust min/max values as needed for your hub)
+async def get_battery_percentage(voltage_mV:float):
+   max_voltage = 8400.0  # max battery level https://assets.education.lego.com/v3/assets/blt293eea581807678a/bltb87f4ba8db36994a/5f8801b918967612e58a69a6/techspecs_techniclargehubrechargeablebattery.pdf?locale=en-us
+   min_voltage = 5000.0  # min battery level
+   percentage = ((float(voltage_mV) - min_voltage) / float(max_voltage - min_voltage) )* 100
+   return max(0, min(100, percentage)) # Ensure percentage is between 0 and 100
+
+
+async def wait_button_release():
+   """Wait for all buttons to be released"""
+   while hub.buttons.pressed():
+       await wait(500)
+   await wait(1000)  # Debounce delay
+
+
+WALL_DISTANCE = 300  # mm
+async def drive_forward():
+   """Drive forward continuously using DriveBase."""
+   drive_base.drive(1000,0)
+
+
+async def drive_backward():
+   """Drive forward continuously using DriveBase."""
+   drive_base.drive(400, 0)
+
+
+
+
+
+
+
+
+
+
+async def monitor_distance(stop_mm: int = WALL_DISTANCE):
+   """Monitor ultrasonic sensor and stop when wall is detected."""
+   while True:
+       distance = await lazer_ranger.distance()
+       print('Distancing...', distance)
+      
+       if distance is None:
+           await wait(50)
+           continue
+       if distance < stop_mm:
+           drive_base.stop()
+           print(f"Wall detected at {distance}mm!")
+           break
+
+
+       # Small delay to prevent overwhelming the sensor
+       await wait(50)
+
+
+# Use this to set default
+def set_default_speed():
+   drive_base.settings(600, 500, 300, 200)
+
+
+# Use this to change drive base movement
+def set_speed(straight_speed, st_acc, turn_speed, turn_acc):
+   drive_base.settings(straight_speed, st_acc, turn_speed, turn_acc)
+
+
+
+
+
+
+
+async def main():
+    await drive_base.straight(500)
+    #await drive_base.turn(-15)
+    await right_arm.run_angle(100,-90)
+    await drive_base.straight(-176)
+    await drive_base.turn(15)
+    await right_arm.run_angle(100,90)
+
+run_task(main())