114 lines
3.0 KiB
Python
114 lines
3.0 KiB
Python
|
|
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
|
|
|
|
|
|
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
|
|
right_motor = Motor(Port.B)
|
|
left_arm = Motor(Port.C, Direction.COUNTERCLOCKWISE)
|
|
right_arm = Motor(Port.D)
|
|
|
|
drive_base = DriveBase(left_motor, right_motor, wheel_diameter=68.8, axle_track=180)
|
|
|
|
drive_base.settings(550,700,100,100)
|
|
|
|
drive_base.use_gyro(True)
|
|
|
|
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)
|
|
|
|
|
|
def main():
|
|
if color_sensor.color() == Color.GREEN:
|
|
Run1()
|
|
|
|
if color_sensor.color() == Color.WHITE:
|
|
Run2()
|
|
|
|
if color_sensor.color() == Color.YELLOW:
|
|
Run3()
|
|
|
|
if color_sensor.color() == Color.BLUE:
|
|
Run5()
|
|
|
|
if color_sensor.color() == Color.RED:
|
|
Run6()
|
|
|
|
print(f'Detected color: {color_sensor.color()}')
|
|
|
|
# Main execution loop
|
|
while True:
|
|
main()
|
|
wait(100)
|