main.cpp

#include "commander.h"
#include "commander_modules.h"   // generated by cmdr — run `cmdr module`
#include "secrets.h"
#include "hal/hal.h"
#include "i2c_ids.h"
#include "modules/locomotion/LocoProtocol.h"
#include "modules/locomotion/DriveMixer.h"
#include "modules/controller/StickFilter.h"
#include "core/IModule.h"
#include "transport/uart/UartTransport.h"
#include "pico/time.h"
#include <stdio.h>   // snprintf for the drivedbg readout

// Smooth differential-drive mixing (two-zone velocity/radius + ramping) lives in
// commander's DriveMixer; the robot just picks the stick layout and ships the
// result over I2C. Split-stick layout (same as the original): LEFT stick Y =
// throttle, RIGHT stick X = steering.
//
// Stick conditioning — the input low-pass filter, calibration, and the `calibrate`
// command — now lives in commander's controller module (it conditions every sample
// before publishing), so the sticks we get here are already clean. The robot only
// does the app-specific part: stop on calibrate, map sticks → drive, send over I2C.
//
// Control scheme: LEFT stick Y = throttle, RIGHT stick X = steering (arcs ONLY —
// stickSpin off, so a tight turn never trips an unwanted spin). Spin-in-place is on
// the triggers: hold either shoulder and the LEFT STICK picks the direction
// (forward = CW, back = CCW), while the trigger picks the speed — R2 normal, L2
// slow (half) for fine corrections. See the spin block in the drive ticker below.
static DriveMixer _mixer = [] {
    DriveMixer::Config c;
    c.stickSpin = false;
    return DriveMixer(c);
}();
static ControllerModule *_pad = nullptr;   // captured for the drivedbg readout

// Diagnostic latch: the drive ticker stashes raw vs conditioned vs mixer output;
// the `drivedbg` command pulls it and prints over its own console. Temporary.
static struct {
    volatile int16_t rawLy, rawRx, calLy, calRx, vel, rad;
} _dbg;

static volatile bool _driveSuppressed = false;   // true while `calibrate` runs

// Stick low-pass for the drive path, ticked HERE at 50 Hz. It must NOT come from the
// module's per-report conditioning: that EMA only advances when the pad sends a
// report, so when the sticks go quiet at rest the filter freezes at a lagging,
// off-center value and the base creeps. Run on rawState() at the loop rate so it
// always decays to center. (Temporal filtering belongs at the consumption rate.)
static StickFilter _driveFilter(25);

// Drive loop, POLL model. Runs at 50 Hz off the UART task (uart.addTicker), reading
// the controller's latest RAW sample and conditioning it here — NOT the push onUpdate
// callback (which only fires on a report, stalling the velocity ramp at rest) and NOT
// the module's pre-conditioned state() (whose filter freezes between reports → creep).
// Polling guarantees the ramp + filter complete, catches dropouts, and keeps the
// blocking I2C off the Bluetooth/cyw43 task.
struct DriveTicker : IModule {
    const char *name() const override { return "drive"; }
    void        init() override {}
    void        registerCommands(CommandRegistry &) override {}
    void        tick() override {
        static absolute_time_t next = {0};
        static bool moving   = false;
        static int  stopReps = 0;     // STOPs left to resend after a stop (drop-proofing)
        if (!time_reached(next)) return;
        next = make_timeout_time_ms(20);              // 50 Hz, independent of report rate
        if (_driveSuppressed || !_pad) return;        // calibration owns the base

        ControllerState raw = _pad->rawState();       // poll the latest RAW sample
        bool    drive  = false;
        int16_t vel    = 0;
        int16_t radius = LOCO_RADIUS_STRAIGHT;

        if (!raw.connected) {                         // dropout → no drive, reset the ramp
            _mixer.reset(); _driveFilter.reset();
        } else {
            // Condition at the loop rate: low-pass (here) then the module's calibration
            // profile (re-center / rescale / deadzone). Filtering before the deadzone so
            // a decayed-to-center stick lands inside it and zeroes out.
            ControllerState s = _pad->calibration().apply(_driveFilter.apply(raw));

            // Spin-in-place mode: hold a shoulder button and the LEFT STICK sets the spin
            // — forward = clockwise, back = counter-clockwise — at the same two-zone speed
            // curve as driving. R2/ZR = normal speed; L2/ZL = slow (half) for fine course
            // corrections. (Check digital L2/R2 and the analog triggers so it works
            // whichever way the pad reports ZL/ZR.) Right stick still arcs when not spinning.
            int16_t throttle = (int16_t)-s.ly;             // stick forward = +
            bool slow = s.pressed(BTN_L2) || s.lt > 64;
            bool norm = s.pressed(BTN_R2) || s.rt > 64;
            int     spin  = 0;
            int16_t scale = 100;
            if (slow || norm) {
                spin  = throttle > 0 ? -1 : (throttle < 0 ? 1 : 0);  // fwd = CW(-1), back = CCW(+1)
                scale = slow ? 50 : 100;                             // slow wins if both held
            }

            drive = _mixer.update(throttle, s.rx, &vel, &radius, spin, scale);
            _dbg.rawLy = raw.ly; _dbg.rawRx = raw.rx;  // latch raw vs conditioned for drivedbg
            _dbg.calLy = s.ly;   _dbg.calRx = s.rx;
            _dbg.vel   = vel;    _dbg.rad   = radius;
        }

        if (drive) {
            uint8_t p[LOCO_DRIVE_LEN];
            loco_pack_drive(vel, radius, p);
            hal_i2c_write(LOCO_BRIDGE_ADDR, CMD_LOCO_DRIVE, p, LOCO_DRIVE_LEN);
            moving = true; stopReps = 0;
        } else {
            // Just stopped (incl. dropout): resend STOP for ~0.5 s. DRIVE is implicitly
            // retried every tick; STOP was sent once, so a single dropped/missed STOP
            // left the bridge executing the last drive forever — the "stuck velocity"
            // creep. After the window go quiet so the bridge can idle-park.
            if (moving) { moving = false; stopReps = 25; }
            if (stopReps > 0) {
                hal_i2c_write(LOCO_BRIDGE_ADDR, CMD_LOCO_STOP, nullptr, 0);
                stopReps--;
            }
        }
    }
};
static DriveTicker _driveTicker;

// Capture the controller. The drive (incl. its own filter) runs in the ticker; this
// hook just wires up calibration suppression. The module's per-report filter isn't
// used by the drive path — the ticker filters rawState() at the loop rate instead.
void commander_on_controller_ready(ControllerModule &pad) {
    _pad = &pad;

    // Calibration suppresses input while it runs (the ticker bails on _driveSuppressed),
    // so the base would hold its last command — stop it at both ends, reset the ramp.
    pad.onCalibrate([](bool active, void *) {
        hal_i2c_write(LOCO_BRIDGE_ADDR, CMD_LOCO_STOP, nullptr, 0);
        _driveSuppressed = active;
        if (!active) _mixer.reset();
    }, nullptr);
}

// Pump the drive loop from the UART task at a fixed rate (poll model).
extern "C" void commander_on_uart_ready(UartTransport &uart) {
    uart.addTicker(_driveTicker);
}

extern "C" CommanderConfig commander_config() {
    CommanderConfig cfg;
    cfg.wifi_ssid     = WIFI_SSID;
    cfg.wifi_password = WIFI_PASSWORD;
    cfg.hostname      = "cmdr-robot";
    cfg.uart_baud     = 115200;
    cfg.uart_greeting = "cmdr-robot";
    return cfg;
}

extern "C" void commander_setup(CommandRegistry& reg) {
    commander_register_modules(reg);

    // Temporary diagnostic: print the latest raw -> conditioned -> mixer values at
    // 10 Hz for a few seconds, over whatever console you ran it on (telnet or USB).
    // Hold a stick while it runs to see jitter/curve. Usage: `drivedbg [seconds]`.
    reg.registerCommand(CMD("drivedbg",
        "stream raw->cal->vel/rad at 10Hz for N s (default 3) - telnet/USB",
        I2C_NONE,
        [](const char *args, Writer &out, void *) {
            int secs = 3;
            while (*args == ' ') ++args;
            if (*args >= '1' && *args <= '9') secs = *args - '0';   // 1..9 s
            out.writeln("drivedbg: hold a stick steady; sampling 10 Hz...");
            for (int i = 0; i < secs * 10; i++) {
                int16_t rad = _dbg.rad;
                const char *rtag = rad == LOCO_RADIUS_STRAIGHT ? "str"
                                 : rad == LOCO_RADIUS_CW       ? "cw"
                                 : rad == LOCO_RADIUS_CCW      ? "ccw" : "arc";
                char line[80];
                snprintf(line, sizeof(line),
                         "raw ly=%4d rx=%4d | cal ly=%4d rx=%4d | vel=%4d rad=%s",
                         _dbg.rawLy, _dbg.rawRx, _dbg.calLy, _dbg.calRx, _dbg.vel, rtag);
                out.writeln(line);
                hal_delay_ms(100);   // yields, so the drive task keeps updating _dbg
            }
            out.writeln("drivedbg: done");
        }, nullptr));
}