Add final/3main.py

2025-10-09 12:32:51 +00:00
parent 684447c01a
commit 227619df49
1 changed files with 102 additions and 0 deletions
--- a/final/3main.py
+++ b/final/3main.py
@@ -0,0 +1,102 @@
+from pybricks.hubs import PrimeHub
+from pybricks.pupdevices import Motor, ColorSensor
+from pybricks.parameters import Port, Stop, Color, Direction
+from pybricks.robotics import DriveBase
+from pybricks.tools import wait, StopWatch, multitask, run_task
+
+async def Run1():
+    await multitask(left_arm.run_angle(1000, 300), right_arm.run_angle(1000,500))
+    await drive_base.straight(320)
+
+    await right_arm.run_angle(5000,-500, Stop.HOLD)
+    await right_arm.run_angle(5000,500, Stop.HOLD)
+    await right_arm.run_angle(5000,-500, Stop.HOLD)
+    await right_arm.run_angle(5000,500, Stop.HOLD)
+    await right_arm.run_angle(5000,-500, Stop.HOLD)
+
+    await drive_base.turn(-20)
+    await drive_base.straight(275)
+    await drive_base.turn(20)
+    await drive_base.straight(63)
+
+    await multitask(drive_base.turn(-30), right_arm.run_angle(50,500))
+    await drive_base.turn(45)
+    await drive_base.straight(-135)
+    await drive_base.turn(-60)
+    await drive_base.straight(90)
+    await left_arm.run_angle(1000,-450)
+    await drive_base.straight(-145)
+    await left_arm.run_angle(1000,450)
+    await drive_base.straight(10)
+    await drive_base.turn(35)
+    await drive_base.straight(-500)
+
+async def Run2():
+    await drive_base.straight(200)
+    await drive_base.turn(-20)
+    await drive_base.straight(525)
+    await drive_base.turn(60)
+
+    await drive_base.straight(50)
+    await right_arm.run_angle(2000,1000)
+    await drive_base.straight(-50)
+    await drive_base.turn(45)
+    await drive_base.straight(50)
+    await right_arm.run_angle(350,-1000)
+
+    await drive_base.turn(-100)
+    await drive_base.straight(-600)
+
+async def Run3():
+    await drive_base.straight(915)
+    await drive_base.turn(-90)
+    await drive_base.straight(60)
+    await left_arm.run_angle(10000,-15000)
+    await drive_base.straight(-60)
+    await drive_base.turn(85)
+    await drive_base.straight(2000)
+
+async def Run5():
+    await drive_base.straight(420)
+    await right_arm.run_angle(300,-100)
+    await drive_base.straight(-100)
+    await right_arm.run_angle(300, 100)
+    await drive_base.straight(-350)
+
+
+async def Run6():
+    await drive_base.straight(420)
+    await right_arm.run_angle(300,-100)
+    await drive_base.straight(-100)
+    await right_arm.run_angle(300, 100)
+    await drive_base.straight(-350)
+
+async def main():
+    while True:
+        detected_color = color_sensor.color(True)
+    
+        if detected_color == Color.GREEN:
+            print('Detected color: Green')
+            await Run1()
+            
+        elif detected_color == Color.WHITE:
+            print('Detected color: White')
+            await Run2()
+            
+        elif detected_color == Color.YELLOW:
+            print('Detected color: Yellow')
+            await Run3()
+
+        elif detected_color == Color.BLUE:
+            print('Detected color: Blue')
+            await Run5()
+
+        elif detected_color == Color.RED:
+            print('Detected color: Red')
+            await Run6()
+            
+        else:
+            print("None detected")
+
+# Main execution
+run_task(main())