The basic trajectory drive. Needs some config, tweaking, and optimization, but this is the basic sta

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
+right_motor = Motor(Port.B, Direction.CLOCKWISE)
-left_arm = Motor(Port.C, Direction.CLOCKWISE, [[12,36]],[[12,20,24]] ) # Specify default direction
+left_arm = Motor(Port.C, Direction.CLOCKWISE, [[12,36]], [[12,20,24]])
-right_arm = Motor(Port.D, Direction.CLOCKWISE,[[12,36],[12,20,24]]) #Added gear train list for gear ration
+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)
+WHEEL_DIAMETER = 68.8
-AXLE_TRACK = 180  # mm (distance between wheels)
+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["x"] += r * umath.sin(umath.radians(a))
-    current.y += r*sin(a)
+    current["y"] += r * umath.cos(umath.radians(a))
-    current.a = a
+    current["a"] = a
 
-drive_base.drive()
+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")