Contribution about odometry and "follow_path"

[michelvancan]

I use the "Technic hub" with the Audi e-tron 42160 kit.
I'm surprised at the power found in Lego hubs.
Associated with the wonderful Pybricks software, this allows you to consider programming advanced robotics.

Here are 2 examples that can serve as a basis for your applications.
We can certainly improve these programs, all your comments are welcome.

Odometry

This makes it possible to know the position of a vehicle based on the rotation of the wheels and
the heading of the vehicle. The gyroscope works very well (as does the Pybricks processing) and
the positioning error is reasonable. Here is my program (Of course it must be adapted according
to the configuration of the motors and the size of the wheels):

async def odometry(x0, y0):
    global running, x, y
    running = True
    hub.imu.reset_heading(0)
    front.reset_angle(0)
    rear.reset_angle(0)
    x, y, old_enc = x0, y0, 0
    while running:
        cap = -umath.radians(hub.imu.heading()) # anti clockwise
        enc = 0.5*(front.angle() + rear.angle())
        denc = enc - old_enc
        old_enc = enc
        d = denc/360*16.30 # distance traveled for 360 degrees
        x += -d*umath.sin(cap)
        y += d*umath.cos(cap)
        await wait(20)
    print("End of odometry")

Follow path

Then the path tracking (defined by a succession of points).
Please note, the sequence of segments must be reasonable depending on the radius of curvature of the vehicle
(50cm for Audi-Etron). Here is the program:

# path : waypoint list
# tpath : list of segment : xstart, ystart, phi (segment angle in radians), lg (length of segment)
def transform_path(path):
    tpath = []
    x0, y0 = path[0]
    for i in range(1, len(path)) :
        x1, y1 = path[i]
        lg = umath.sqrt((x1 - x0)*(x1 - x0) + (y1 - y0)*(y1 - y0))
        phi = umath.atan2(y1 - y0, x1 - x0)
        tpath.append((x0, y0, phi, lg))
        x0, y0 = x1, y1
    return tpath

def find_goal(x, y, cap, L, segment):
    xd, yd, phi, lg = segment
    ux, uy = umath.cos(phi), umath.sin(phi)
    # compute future point
    xf = x + L*umath.sin(umath.radians(cap))
    yf = y + L*umath.cos(umath.radians(cap))
    # compute projection of future vector on segment (scalar product)
    proj = (xf - xd)*ux + (yf - yd)*uy
    if proj < 0:
        return True, xd, yd # ???
    if proj > lg: 
        # we need to explore the next segment
        return False, 0, 0
    gx, gy = xd + ux*proj, yd + uy*proj
    return True, gx, gy

def goto_goal(x, y, cap, gx, gy, k):
    theta = umath.degrees(umath.atan2(gx - x, gy - y))
    steering = theta - cap
    if steering > 180: steering -= 360
    if steering < -180: steering += 360
    steering = k*steering
    car.steer(steering)

async def follow_path(objective, path, speed):
    global x, y
    xobj, yobj = objective
    tpath = transform_path(path)
    car.drive_power(speed)
    car.steer(0)
    epsilon = 5 #cm
    L = 50 # cm lookahead distance
    k = 5
    if speed < 0: k = -k # to go back
    arrival = False
    s = 1
    for segment in tpath:
        while True:
            cap = hub.imu.heading()
            if speed < 0: cap = cap + 180
            result, gx, gy = find_goal(x, y, cap, L, segment)
            if not result : break  # goal not in segment, go to next segment
            goto_goal(x, y, cap, gx, gy, k)
            dist = umath.sqrt((x - xobj)*(x - xobj) + (y - yobj)*(y - yobj))
            if dist < epsilon and s == len(tpath): 
                arrival = True
                break
            await wait(50)
        #print(f"end s {s} {path[s]} : x = {x:.1f}, y = {y:.1f}, cap = {cap:.1f}")
        if arrival: break
        s += 1
    if not arrival:
        print("No more way, even though we haven't arrived")
    front.hold()
    rear.hold()
    car.steer(0)
    await wait(500)
    cap = hub.imu.heading()
    print(f"end ({objective}) : x = {x:.1f}, y = {y:.1f}, cap = {cap:.1f}")

Finally here is the complete program:

from pybricks.hubs import TechnicHub
from pybricks.pupdevices import Motor
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
from pybricks.robotics import Car
from pybricks.tools import wait, StopWatch, multitask, run_task
import umath

async def test():
    global running
    # half circle
    path = [(0, 0), (0, 30), (-60, 90), (-90, 90), (-150, 30), (-150, -50)]
    await follow_path((-150, 0), path, 40)
    running = False

async def main():
    await multitask(odometry(0, 0), test())

# Set up gyro
hub = TechnicHub()
hub.imu.settings(1.5, 250)  # Very important
wait(2000)
print(hub.battery.voltage(), hub.imu.settings(), hub.imu.ready(), hub.imu.stationary())

# Set up all devices.
steering = Motor(Port.D, Direction.CLOCKWISE)
front = Motor(Port.B, Direction.CLOCKWISE)
rear = Motor(Port.A, Direction.CLOCKWISE)
car = Car(steering, [front, rear])
wait(1000)

# Run the main program.
run_task(main())
print("it's over")

[laurensvalk]

Very nice! Do you have some kind of video demonstrating how this works?

[michelvancan]

Here an example

    await follow_path((-120, 150), [(0, 0), (0, 30), (-60, 90), (-90, 90), (-120, 120), (-120, 180)], 40)
    await follow_path((30, 90), [(-120, 150), (-60, 90), (60, 90)], -40)
    await follow_path((-45, -25), [(0, 90), (-45, 30), (-45, -100)], 40)
    # parking
    await follow_path((-80,55), [(-45, -25), (-85, 30), (-85, 100)], -30)
    await follow_path((-75,30), [(-80, 55), (-80, 150)], 30)

[michelvancan]

https://youtube.com/shorts/HZhNZJAP7X8