From a1fe733115cbca9611093e9d6065699119cb42f8 Mon Sep 17 00:00:00 2001 From: Arcmyx Official Date: Thu, 7 May 2026 00:58:58 +0000 Subject: [PATCH] The basic trajectory drive. Needs some config, tweaking, and optimization, but this is the basic sta --- experiments/trajectory_drive.py | 171 +++++++++++++++++++++++++++----- 1 file changed, 145 insertions(+), 26 deletions(-) diff --git a/experiments/trajectory_drive.py b/experiments/trajectory_drive.py index 47948d7..c4bc94a 100644 --- a/experiments/trajectory_drive.py +++ b/experiments/trajectory_drive.py @@ -1,40 +1,159 @@ from pybricks.hubs import PrimeHub from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor -from pybricks.parameters import Button, Color, Direction, Port, Side, Stop +from pybricks.parameters import Button, Color, Direction, Port, Side, Stop, Axis from pybricks.robotics import DriveBase -from pybricks.tools import wait, StopWatch +from pybricks.tools import wait, StopWatch, run_task, multitask import umath + 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 +right_motor = Motor(Port.B, Direction.CLOCKWISE) +left_arm = Motor(Port.C, Direction.CLOCKWISE, [[12,36]], [[12,20,24]]) +right_arm = Motor(Port.D, Direction.CLOCKWISE, [[12,36],[12,20,24]]) lazer_ranger = UltrasonicSensor(Port.E) color_sensor = ColorSensor(Port.F) -# DriveBase configuration -WHEEL_DIAMETER = 68.8 # mm (adjust for your wheels) -AXLE_TRACK = 180 # mm (distance between wheels) + +WHEEL_DIAMETER = 68.8 +AXLE_TRACK = 180 drive_base = DriveBase(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK) drive_base.settings(600, 500, 300, 200) -drive_base.use_gyro(True) +drive_base.use_gyro(False) + +current = {"x": 0, "y": 0, "a": 0} +target = {"x": 0, "y": 200, "a": 0} +done = False -transformations = { - "d": [2, 3], - "a": [0, 90] -} -current = { - "x": 0, - "y": 0, - "a": 0 -} -# Extra useless code -#for transformation in transformations: -# current.x += transformation.d * umath.cos(umath.radians(transformation.a)) -# current.y += transformation.d * umath.sin(umath.radians(transformation.a)) def addtoposition(a, r): - current.x += r*cos(a) - current.y += r*sin(a) - current.a = a + current["x"] += r * umath.sin(umath.radians(a)) + current["y"] += r * umath.cos(umath.radians(a)) + current["a"] = a -drive_base.drive() \ No newline at end of file +async def track_heading(): + while True: + current["a"] = hub.imu.heading() + await wait(20) + +async def printStats(): + last_dist = 0 + while True: + if done: + break + current_dist = drive_base.distance() + instant_dist = current_dist - last_dist + last_dist = current_dist + + current["x"] += instant_dist * umath.sin(umath.radians(current["a"])) + current["y"] += instant_dist * umath.cos(umath.radians(current["a"])) + + dx = target["x"] - current["x"] + dy = target["y"] - current["y"] + remaining = umath.sqrt(dx*dx + dy*dy) + desired_a = 90 - umath.degrees(umath.atan2(dy, dx)) + print("x:", current["x"], "y:", current["y"], "desired_a:", desired_a, "current_a:", current["a"], "remaining:", remaining) + + await wait(50) + +async def pid_turn(target_angle): + KP = 3 + KI = 0.01 + KD = 1.5 + SETTLE_THRESHOLD = 0.5 + SETTLE_TIME = 500 + + integral = 0 + last_error = 0 + settled_timer = StopWatch() + settled = False + + while True: + error = target_angle - current["a"] + while error > 180: error -= 360 + while error < -180: error += 360 + + integral += error + integral = max(-100, min(100, integral)) + derivative = error - last_error + last_error = error + + turn_rate = KP * error + KI * integral + KD * derivative + turn_rate = max(-200, min(200, turn_rate)) + + drive_base.drive(0, turn_rate) + + if abs(error) < SETTLE_THRESHOLD: + if not settled: + settled = True + settled_timer.reset() + elif settled_timer.time() > SETTLE_TIME: + drive_base.stop() + break + else: + settled = False + + await wait(20) + +async def driveTrajectory(): + global done + ARRIVE_THRESHOLD = 30 # give stage 2 more room to work + FINAL_THRESHOLD = 7 + SPEED = 400 + SLOW_SPEED = 30 + TURN_GAIN = 3.0 + + # stage 1 + while True: + dx = target["x"] - current["x"] + dy = target["y"] - current["y"] + remaining = umath.sqrt(dx*dx + dy*dy) + if remaining < ARRIVE_THRESHOLD: + drive_base.stop() + break + desired_a = 90 - umath.degrees(umath.atan2(dy, dx)) + error = desired_a - current["a"] + while error > 180: error -= 360 + while error < -180: error += 360 + turn_rate = max(-200, min(200, error * TURN_GAIN)) + # in stage 1, scale speed down as it gets closer + speed = max(150, SPEED * min(1, remaining / 300)) + drive_base.drive(speed, turn_rate) + await wait(20) + + print("Stage 1 done") + + # stage 2 + while True: + dx = target["x"] - current["x"] + dy = target["y"] - current["y"] + remaining = umath.sqrt(dx*dx + dy*dy) + if remaining < FINAL_THRESHOLD: + drive_base.stop() + break + a_rad = umath.radians(current["a"]) + forward_x = umath.sin(a_rad) + forward_y = umath.cos(a_rad) + signed_dist = dx * forward_x + dy * forward_y + speed = SLOW_SPEED if signed_dist > 0 else -SLOW_SPEED + drive_base.drive(speed, 0) + await wait(20) + + drive_base.stop() + print("Stage 2 done") + + # stage 3 — PID angle correction + await pid_turn(target["a"]) + + done = True + print("Done! x:", current["x"], "y:", current["y"], "a:", current["a"]) + +async def main(): + drive_base.reset() + await multitask(driveTrajectory(), track_heading(), printStats()) + +hub.imu.reset_heading(0) +while True: + if hub.imu.ready(): + break + +run_task(main()) +print("done") \ No newline at end of file