Carro RC que regresa solo al inicio con GPS

ArduPilot Rover ya lo hace de fábrica, pero escribirlo tú mismo te enseña la base de la navegación de robots: distancia y rumbo entre puntos GPS, orientación y un controlador de dirección.

Pasos de la misión

1. Conecta el carro

   Servo de dirección y ESC a los canales 0 y 1 del PCA9685, GPS al UART del Pi y el Pi alimentado con un UBEC de 5 V.

2. Guarda el inicio

   Cuando el GPS tiene una posición firme, el script guarda el punto de partida.

3. Obtén la orientación

   Mientras el carro avanza, usa el rumbo sobre el suelo del GPS como orientación (una brújula es opcional y requiere calibración cuidadosa).

4. Dirige al inicio

   Calcula el rumbo hacia el inicio, réstale la orientación y gira las ruedas en proporción al error.

5. Detente con seguridad

   Corta el acelerador a menos de 3 m del inicio o si se pierde la señal GPS. Prueba en un campo vacío a velocidad de caminata.

Código

import math, serial, pynmea2
from adafruit_servokit import ServoKit

kit = ServoKit(channels=16)
steer, esc = kit.servo[0], kit.continuous_servo[1]
gps = serial.Serial('/dev/serial0', 9600, timeout=1)
home = None

def bearing(a, b):
    la1, lo1, la2, lo2 = map(math.radians, (*a, *b))
    y = math.sin(lo2 - lo1) * math.cos(la2)
    x = math.cos(la1) * math.sin(la2) - math.sin(la1) * math.cos(la2) * math.cos(lo2 - lo1)
    return (math.degrees(math.atan2(y, x)) + 360) % 360

def dist_m(a, b):
    k = 111320
    return math.hypot((b[0] - a[0]) * k, (b[1] - a[1]) * k * math.cos(math.radians(a[0])))

while True:
    line = gps.readline().decode(errors='ignore')
    if not line.startswith('$GNRMC') and not line.startswith('$GPRMC'):
        continue
    m = pynmea2.parse(line)
    if m.status != 'A':
        esc.throttle = 0; continue          # no fix: stop
    pos = (m.latitude, m.longitude)
    if home is None:
        home = pos; print('Home saved', home); continue
    if dist_m(pos, home) < 3:
        esc.throttle = 0; steer.angle = 90; continue
    err = (bearing(pos, home) - (m.true_course or 0) + 540) % 360 - 180
    steer.angle = 90 + max(-35, min(35, err * 0.8))
    esc.throttle = 0.15                     # slow and safe

Code comments are in English, like most libraries and docs — a good habit for makers.

Preguntas