Tire

#![allow(unused)]
fn main() {
pub struct TireParams { pub mu: f64, pub c_stiff: f64 }

/// F = -mu*N * tanh(c_stiff * |slip| / (mu*N)) * slip.normalized()
pub fn tire_force(p: &TireParams, n_force: f64, slip_vel: Vec2) -> Vec2;
}

The tire model is a 2D slip-velocity law with a tanh saturation. Two useful properties fall out for free:

• Friction circle — combined longitudinal and lateral limit is automatic. |F| ≤ mu*N in every direction, always.
• Smooth linear regime — for small slip, |F| ≈ c_stiff * |slip|, matching what a linear cornering stiffness gives you.

Force opposes slip direction, so a wheel skidding forward gets pushed backward, a wheel scrubbing sideways gets pushed sideways-back.

PD tracking under limited grip

The pink line at the bottom is the cross-track error over time.

Source (Scene B)

#![allow(unused)]
fn main() {
//! Scene B — PD path following.
//!
//! A reference dot advances along a cubic Bezier at constant ground speed.
//! The controller commands `twist = kp*err + kd*(ref_vel - vel) + ref_vel`,
//! then chews it up with the same naive open-loop module servo as Scene A.
//! We overlay the P and D vector contributions separately so their roles are
//! legible.

use std::sync::OnceLock;

use anim_core::{arrow, auto_range, Cmd, Color, CubicBez, DrawList, LineChart, Series, Vec2};
use anim_timeline::{simulate, Track};
use kinematics::controllers::pd_path::{PdMem, PdPathFollower};
use kinematics::sim::SimState;
use kinematics::{RobotParams, SwerveState, SwerveSystem};

use super::swerve_common as sc;

pub const DURATION: f64 = 6.0;

type PdState = SimState<PdMem>;

fn path() -> CubicBez {
    CubicBez::new(
        Vec2::new(-2.5, -0.5),
        Vec2::new(-0.5, 1.8),
        Vec2::new(1.0, -1.8),
        Vec2::new(2.5, 0.6),
    )
}

fn bake() -> &'static Track<PdState> {
    static T: OnceLock<Track<PdState>> = OnceLock::new();
    T.get_or_init(|| {
        let params = RobotParams::default_frc();
        let p = path();
        let ctrl = PdPathFollower::new(p, 0.4, 8.0, 4.0, 6.0, 1.6, params);
        // Robot starts 0.5 m off path start (below) to make the capture visible.
        let mut initial = SwerveState::default();
        initial.chassis.pos = p.p0 + Vec2::new(0.0, -0.5);
        let sys = SwerveSystem::new(params, ctrl, initial);
        simulate(&sys, DURATION, 1000.0, 120.0)
    })
}

fn camera(track: &Track<PdState>) -> sc::Camera {
    let p = path();
    let mut pts: Vec<Vec2> = vec![p.p0, p.p1, p.p2, p.p3];
    for s in track.samples() {
        pts.push(s.swerve.chassis.pos);
    }
    let trajectory_rect = sc::viewport_rect(0.0, 0.28, 1.0, 0.72);
    sc::Camera::fit_in_rect(&pts, trajectory_rect, 32.0, 3.0)
}

pub fn scene(t: f64) -> DrawList {
    let track = bake();
    let params = RobotParams::default_frc();
    let cam = camera(track);
    let state = track.lerp(t);

    let mut out = DrawList::new();
    out.extend(sc::field_grid(&cam, 5.0));

    // Path (map control points through camera).
    let p = path();
    let mapped_path = CubicBez::new(cam.m(p.p0), cam.m(p.p1), cam.m(p.p2), cam.m(p.p3));
    out.push(Cmd::Stroke {
        path: mapped_path,
        width: 2.0,
        color: Color::rgba(160, 160, 170, 0.7),
    });

    // Trail so far.
    let dt = track.dt();
    let idx = ((t / dt) as usize).min(track.samples().len() - 1);
    let stride = ((track.samples().len() / 200).max(1)).max(1);
    let trail = sc::build_trail(
        track.samples(),
        |s: &PdState| s.swerve.chassis.pos,
        idx,
        stride,
    );
    sc::draw_trail(&cam, &trail, sc::TRAIL_COL, false, &mut out);

    // Reference dot.
    out.push(Cmd::FillCircle {
        center: cam.m(state.mem.ref_pos),
        r: 6.0,
        color: sc::GHOST_COL,
    });

    // Robot.
    sc::draw_robot(&cam, &params, &state.swerve, Vec2::new(0.6, 0.6), &mut out);

    // Error vector (gray) reference → robot.
    let err_screen = cam.m(state.swerve.chassis.pos) - cam.m(state.mem.ref_pos);
    out.extend(arrow(
        cam.m(state.mem.ref_pos),
        err_screen,
        1.5,
        Color::rgba(180, 180, 190, 0.6),
    ));

    // P and D contributions as arrows from the robot's centre.
    let p_col = Color::rgb(190, 130, 255);
    let d_col = Color::rgb(120, 220, 200);
    let p_scale = 0.35;
    let d_scale = 0.35;
    sc::draw_overlay_arrow(
        &cam,
        state.swerve.chassis.pos,
        state.mem.p_term,
        p_scale,
        2.0,
        p_col,
        &mut out,
    );
    sc::draw_overlay_arrow(
        &cam,
        state.swerve.chassis.pos,
        state.mem.d_term,
        d_scale,
        2.0,
        d_col,
        &mut out,
    );

    // Cross-track error chart along the bottom of the viewport. Axis is
    // computed from the full baked track so playback doesn't rescale.
    let xs: Vec<f64> = (0..track.samples().len()).map(|k| k as f64 * dt).collect();
    let ys: Vec<f64> = track.samples().iter().map(|s| s.mem.cross_track).collect();
    let (y_lo, y_hi) = auto_range(ys.iter().copied(), true);
    let chart = LineChart {
        rect: sc::viewport_rect(0.03, 0.03, 0.94, 0.20),
        x_range: (0.0, DURATION),
        y_range: (y_lo, y_hi),
        now: Some(t),
        ..LineChart::default()
    };
    out.extend(chart.render(&[Series {
        xs: &xs,
        ys: &ys,
        color: Color::rgba(240, 120, 200, 0.95),
        width: 2.0,
    }]));

    out
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn capture_and_track() {
        let track = bake();
        // After 1.5 s the capture transient should be over; max cross-track
        // error over the remaining duration stays < 0.12 m.
        let dt = track.dt();
        let after = (1.5 / dt).ceil() as usize;
        let mut max_ct = 0.0f64;
        for s in track.samples().iter().skip(after) {
            max_ct = max_ct.max(s.mem.cross_track);
            assert!(s.swerve.chassis.pos.x.is_finite());
        }
        assert!(max_ct < 0.12, "max cross-track after capture = {max_ct} m");
    }
}
}