haptic_skin.nav

  1"""Navigation controller — haptic compass that always points the way to go.
  2
  3Streaming model (decouples fix rate from motor pulse timing):
  4  * ``update(fix)``  consumes one ``{lat, lon, heading}`` fix and updates the
  5    target: the bearing to the next waypoint, **relative to where you face**.
  6  * ``vector(now)``  returns the 8 PWM values to apply right now (pulse cadence
  7    from a monotonic clock).
  8
  9It is a *homing* compass: the active motor always indicates the direction to
 10the next waypoint relative to your heading. Turn your head and the active node
 11moves; reach a waypoint and it swings to the following one (that's the "turn").
 12Distance to the next waypoint only changes the intensity/urgency:
 13
 14    > 20 m    guide     gentle steady pulse toward the next waypoint
 15    <= 20 m   approach  firmer, faster pulse (the maneuver is close)
 16    reached   advance to the next waypoint
 17    all done  arrived   circular sweep
 18"""
 19
 20from __future__ import annotations
 21
 22import math
 23from dataclasses import dataclass
 24from typing import List
 25
 26from . import protocol
 27from .geo import (
 28    EARTH_RADIUS_M,
 29    LatLon,
 30    bearing_deg,
 31    bucket_from_relative,
 32    haversine_m,
 33    relative_bearing,
 34)
 35from .routing import Route
 36
 37# -- tunables ---------------------------------------------------------------
 38APPROACH_M = 20.0          # within this, the next waypoint is "close"
 39ARRIVE_STEP_M = 6.0        # within this, the waypoint is reached -> advance
 40OFF_ROUTE_M = 30.0         # farther than this from the current leg -> "off route"
 41SWEEP_PERIOD_S = 1.5       # full 8-motor circle on 'arrived'
 42
 43# cadence per band: (intensity 0..1, on_ms, off_ms)
 44_CADENCE = {
 45    "guide": (0.40, 600, 200),     # mostly-on gentle pulse -> clearly directional
 46    "approach": (0.85, 150, 150),  # urgent blink near the maneuver
 47    "off_route": (1.00, 150, 150),  # strong alarm on the back motor
 48}
 49_ZERO = [0] * protocol.NUM_MOTORS
 50
 51
 52def _point_segment_distance_m(p: LatLon, a: LatLon, b: LatLon) -> float:
 53    """Distance from p to segment a-b, metres (equirectangular approx)."""
 54    lat0 = math.radians((a[0] + b[0]) / 2)
 55
 56    def xy(q: LatLon):
 57        return (math.radians(q[1]) * math.cos(lat0) * EARTH_RADIUS_M,
 58                math.radians(q[0]) * EARTH_RADIUS_M)
 59
 60    px, py = xy(p)
 61    ax, ay = xy(a)
 62    bx, by = xy(b)
 63    dx, dy = bx - ax, by - ay
 64    seg2 = dx * dx + dy * dy
 65    if seg2 == 0:
 66        return haversine_m(p, a)
 67    t = max(0.0, min(1.0, ((px - ax) * dx + (py - ay) * dy) / seg2))
 68    cx, cy = ax + t * dx, ay + t * dy
 69    return math.hypot(px - cx, py - cy)
 70
 71
 72def _segment_progress_t(p: LatLon, a: LatLon, b: LatLon) -> float:
 73    """Signed projection of p onto segment a-b, UNCLAMPED: t<0 before a, t>1 past b.
 74
 75    Lets us detect we've walked past a corner even without passing close to it.
 76    """
 77    lat0 = math.radians((a[0] + b[0]) / 2)
 78
 79    def xy(q: LatLon):
 80        return (math.radians(q[1]) * math.cos(lat0) * EARTH_RADIUS_M,
 81                math.radians(q[0]) * EARTH_RADIUS_M)
 82
 83    px, py = xy(p)
 84    ax, ay = xy(a)
 85    bx, by = xy(b)
 86    dx, dy = bx - ax, by - ay
 87    seg2 = dx * dx + dy * dy
 88    if seg2 == 0:
 89        return 1.0
 90    return ((px - ax) * dx + (py - ay) * dy) / seg2
 91
 92
 93@dataclass
 94class NavState:
 95    band: str                 # guide | approach | arrived
 96    bucket: int               # active motor 0..7 (front=0, clockwise)
 97    distance_m: float
 98    instruction: str
 99    step_index: int
100    total_steps: int
101    finished: bool
102
103
104class Navigator:
105    def __init__(self, route: Route):
106        self.route = route
107        self.idx = 1                       # waypoint we are heading toward (0 = start)
108        self.state = NavState("guide", 0, math.inf, "", 1, len(route), False)
109
110    # -- input ---------------------------------------------------------------
111    def update(self, fix: dict) -> NavState:
112        pos: LatLon = (fix["lat"], fix["lon"])
113        heading = float(fix.get("heading", 0.0))
114        wps = self.route.waypoints
115
116        # skip every waypoint we've reached OR already walked past (corner overshoot)
117        while self.idx < len(wps):
118            tgt = wps[self.idx].point
119            prev_pt = wps[self.idx - 1].point
120            reached = haversine_m(pos, tgt) <= ARRIVE_STEP_M
121            passed = _segment_progress_t(pos, prev_pt, tgt) >= 1.0
122            if reached or passed:
123                self.idx += 1
124            else:
125                break
126
127        if self.idx >= len(wps):
128            self.state = NavState("arrived", 0, 0.0, "Arrivée",
129                                  len(wps), len(wps), True)
130            return self.state
131
132        target = wps[self.idx]
133        dist = haversine_m(pos, target.point)
134
135        # off-route alarm: strayed too far from the current leg -> back motor
136        prev = wps[self.idx - 1]
137        if _point_segment_distance_m(pos, prev.point, target.point) > OFF_ROUTE_M:
138            self.state = NavState("off_route", 4, dist, "Hors itinéraire",
139                                  self.idx, len(wps), False)
140            return self.state
141
142        # homing: steer toward the NEXT waypoint POSITION (where to physically walk
143        # next). With the overshoot-advance above, the active motor swings onto the
144        # new street exactly when you reach the corner — never toward an abstract
145        # future heading (which would aim through a wall before the turn).
146        rel = relative_bearing(bearing_deg(pos, target.point), heading)
147        bucket = bucket_from_relative(rel)
148        band = "approach" if dist <= APPROACH_M else "guide"
149
150        self.state = NavState(band, bucket, dist, target.instruction,
151                              self.idx, len(wps), False)
152        return self.state
153
154    # -- output --------------------------------------------------------------
155    def vector(self, now: float) -> List[int]:
156        """8 PWM values to apply at time ``now`` (seconds, monotonic)."""
157        s = self.state
158        if s.band == "arrived":
159            step = SWEEP_PERIOD_S / protocol.NUM_MOTORS
160            motor = int((now / step) % protocol.NUM_MOTORS)
161            return self._one(motor, 0.8)
162
163        if s.band not in _CADENCE:
164            return list(_ZERO)
165
166        intensity, on_ms, off_ms = _CADENCE[s.band]
167        period = (on_ms + off_ms) / 1000.0
168        phase = (now % period) * 1000.0
169        return self._one(s.bucket, intensity) if phase < on_ms else list(_ZERO)
170
171    @staticmethod
172    def _one(bucket: int, intensity: float) -> List[int]:
173        vec = [0] * protocol.NUM_MOTORS
174        vec[int(bucket)] = protocol.intensity_to_pwm(intensity)
175        return vec
176
177
178def simulate(route: Route, fixes: List[dict]) -> List[NavState]:
179    """Feed a list of fixes through a Navigator and return the NavState sequence.
180
181    Pure (no hardware) — used by tests and for sanity-checking a trace.
182    """
183    nav = Navigator(route)
184    return [nav.update(f) for f in fixes]