missions/main.py

@@ -0,0 +1,116 @@
+
+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
+import asyncio
+
+hub = PrimeHub()
+left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
+right_motor = Motor(Port.B)
+atarm1 = Motor(Port.E, Direction.COUNTERCLOCKWISE)
+atarm2 = Motor(Port.F)
+drive_base = DriveBase(left_motor, right_motor, wheel_diameter=56, axle_track=112)
+color_sensor = ColorSensor(Port.C)
+
+drive_base.settings(300, 500, 300, 200)
+Color.ORANGE = Color(10, 100, 100)
+Color.MAGENTA = Color(321, 100, 86)
+
+
+async def Run1():
+    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 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():
+    if color_sensor.color() == Color.ORANGE:
+        await Run1()
+        
+    if color_sensor.color() == Color.GREEN:
+        await Run2()
+        
+    if color_sensor.color() == Color.WHITE:
+        await Run3()
+        
+    if color_sensor.color() == Color.YELLOW:
+        await Run5()
+        
+    if color_sensor.color() == Color.BLUE:
+        await Run6()
+        
+    
+    print(f'Detected color: {color_sensor.color()}')
+
+# Main execution loop
+while True:
+    run_task(main())
+    wait(100)