2026-09-21 04:19:29 +00:00
|
|
|
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(600, 500, 300, 200)
|
|
|
|
|
drive_base.use_gyro(True)
|
|
|
|
|
|
|
|
|
|
def set_default_speed():
|
|
|
|
|
drive_base.settings(600, 500, 300, 200)
|
|
|
|
|
|
|
|
|
|
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(110)
|
|
|
|
|
await drive_base.turn(-45)
|
2026-10-11 22:21:31 +00:00
|
|
|
await drive_base.straight(280)
|
2026-09-21 04:19:29 +00:00
|
|
|
await right_arm.run_angle(300,120)
|
2026-10-11 22:21:31 +00:00
|
|
|
await left_arm.run_angle(400,200)
|
|
|
|
|
await drive_base.straight(-70)
|
|
|
|
|
await left_arm.run_angle(600,-110)
|
|
|
|
|
await drive_base.straight(370)
|
2026-09-21 04:19:29 +00:00
|
|
|
await drive_base.turn(50)
|
2026-10-11 22:21:31 +00:00
|
|
|
await multitask(
|
|
|
|
|
drive_base.straight(250),
|
|
|
|
|
left_arm.run_angle(600,-60)
|
|
|
|
|
)
|
2026-09-21 04:19:29 +00:00
|
|
|
await drive_base.turn(40)
|
|
|
|
|
set_speed(100,200,300,200)
|
2026-10-11 22:21:31 +00:00
|
|
|
await drive_base.straight(170)
|
2026-09-21 04:19:29 +00:00
|
|
|
await drive_base.straight(-135)
|
2026-10-11 22:21:31 +00:00
|
|
|
await drive_base.turn(80)
|
|
|
|
|
await drive_base.straight(85)
|
2026-09-21 04:19:29 +00:00
|
|
|
await multitask(
|
2026-10-11 22:21:31 +00:00
|
|
|
drive_base.turn(-36),
|
|
|
|
|
right_arm.run_angle(200,-140)
|
2026-09-21 04:19:29 +00:00
|
|
|
)
|
|
|
|
|
set_speed(100,200,300,200)
|
2026-10-11 22:21:31 +00:00
|
|
|
await drive_base.straight(230)
|
|
|
|
|
await multitask(
|
|
|
|
|
right_arm.run_angle(300,160),
|
|
|
|
|
left_arm.run_angle(300,150)
|
|
|
|
|
)
|
|
|
|
|
await wait(200)
|
|
|
|
|
await drive_base.straight(30)
|
|
|
|
|
await drive_base.turn(-90)
|
|
|
|
|
await drive_base.straight(-170)
|
2026-09-21 04:19:29 +00:00
|
|
|
set_speed(600,500,300,200)
|
2026-10-11 22:21:31 +00:00
|
|
|
await drive_base.straight(100)
|
2026-09-21 04:19:29 +00:00
|
|
|
await drive_base.arc(800,None,-900)
|
|
|
|
|
|
|
|
|
|
run_task(main())
|