main.cpp

/**
 * THE GHOST ROBOT — ESP32 drive and actuator firmware
 * ───────────────────────────────────────────────────
 * Controller : Bluetooth gamepad (Xbox) via Bluepad32
 * Drive      : TB6612FNG dual H-bridge, two 25GA-370 gearmotors
 * Actuator   : linear actuator on a separate dual H-bridge module
 *              (see wiring/wiring_notes.md — the exact module is not
 *              identified, so its VSS/logic-supply and ENB behaviour are
 *              treated as module-dependent and are not assumed here)
 * Power      : 4S LiPo (2x 2S in series, 14.8 V nominal / 16.8 V maximum)
 *              stepped down to a regulated +12V_MOTOR rail and a separate
 *              +5V_LOGIC rail — see wiring/wiring_notes.md
 *
 * Controls:
 *   Left stick Y   — forward / backward
 *   Right stick X  — turn
 *   Y button       — bypasses command smoothing and immediately applies the
 *                    current joystick command
 *   R1 (hold)      — drive actuator one way
 *   L1 (hold)      — drive actuator the other way
 *
 * Safety behaviour:
 *   - No controller connected (boot, disconnect, or loss of link):
 *     both PWM outputs are set to zero immediately, the actuator is stopped,
 *     and the TB6612FNG is put into standby (STBY LOW).
 *   - On reconnect the smoothed drive state is reset to zero before STBY is
 *     re-asserted, so a stale command can never move the robot.
 *   - Both actuator buttons pressed together stops the actuator.
 *
 * Output ownership:
 *   setup() performs a one-time boot initialisation that drives every control
 *   output low and both PWM channels to zero duty, before either task exists.
 *   After that the control task on core 1 is the ONLY runtime writer of the PWM
 *   outputs, the direction pins, STBY and the actuator pins. The Bluetooth task
 *   and the Bluepad32 callbacks only publish a copied command structure and
 *   never touch hardware.
 *
 * ─── CONFIGURATION ───────────────────────────────────────────────────────────
 * Edit the flags below before uploading. Nothing else needs changing.
 */

// Set to 1 to run a motor + actuator self-test on boot.
// Read the TEST_MODE safety notes in UPLOAD_GUIDE.md first — this moves the
// robot. Leave at 0 for normal use.
#define TEST_MODE                 0

// Set to 1 if the left motor spins the wrong way.
#define INVERT_LEFT_MOTOR         0

// Set to 1 if the right motor spins the wrong way.
#define INVERT_RIGHT_MOTOR        0

// Set to 1 if the actuator extends when it should retract. The physical
// direction depends on how the actuator leads are landed on the driver
// outputs; this flag corrects it without rewiring.
#define INVERT_ACTUATOR           0

// Competition-safe default: keep bonded controller keys so the gamepad
// reconnects automatically. Set to 1 for one boot only when you deliberately
// want to erase stored pairings and pair from scratch.
#define RESET_BT_KEYS_ON_BOOT     0

// Set to 1 to print controller input to Serial for bench verification.
#define INPUT_DEBUG_MODE          0

// ─────────────────────────────────────────────────────────────────────────────

#include <Arduino.h>
#include <Bluepad32.h>

// ─── TB6612FNG drive-motor pins ──────────────────────────────────────────────
#define PIN_PWMA        14    // Left motor PWM   — LEDC channel 0
#define PIN_AIN1        27    // Left motor direction A
#define PIN_AIN2        26    // Left motor direction B
#define PIN_BIN1        25    // Right motor direction A
#define PIN_BIN2        33    // Right motor direction B
#define PIN_PWMB        32    // Right motor PWM  — LEDC channel 1
#define PIN_STBY        13    // STBY — HIGH enables the driver, LOW = standby
                              // (GPIO13 chosen to avoid the GPIO12 strapping pin)

// ─── Actuator driver pins ────────────────────────────────────────────────────
// GPIO16 -> IN3 and GPIO17 -> IN4: the two direction inputs of the actuator
// H-bridge channel. Which input produces physical extension depends on how the
// actuator leads are landed on OUT3/OUT4; use INVERT_ACTUATOR to flip it.
#define PIN_ACT_IN3     16    // IN3
#define PIN_ACT_IN4     17    // IN4

// ─── LEDC hardware PWM ───────────────────────────────────────────────────────
// Arduino-ESP32 Core 2.x API: ledcSetup() + ledcAttachPin(), and ledcWrite()
// takes a *channel*. This project is pinned to Core 2.0.17 (shipped inside the
// Bluepad32 4.1.0 framework package — see platformio.ini), so only that API is
// used here. Core 3.x renamed these calls; porting would need a retest.
#define PWM_FREQ        20000 // 20 kHz — above audible range
#define PWM_RES_BITS    8     // 8-bit duty (0–255)
#define LEDC_CH_LEFT    0
#define LEDC_CH_RIGHT   1
#define MAX_PWM         255

// ─── Control loop ────────────────────────────────────────────────────────────
// Deterministic slew-rate limiter: the commanded PWM magnitude may change by at
// most SLEW_STEP_PWM counts per control period, so a full 0 -> 255 transition
// takes about (255 / 5) * 2 ms ≈ 102 ms.
//
// These are conservative initial parameters, selected to limit launch
// transients. They are NOT derived from friction, traction or current
// measurements, no slip testing was performed, and they have not been validated
// on the physical robot. Treat them as a starting point requiring hardware
// validation and retuning.
#define CONTROL_PERIOD_MS   2
#define SLEW_STEP_PWM       5
#define BT_POLL_MS          10
#define STICK_MAX           512
#define STICK_DEADZONE      30

// ─── Shared command state ────────────────────────────────────────────────────
// The Bluepad32 controller object is only ever touched inside the Bluetooth
// task, which is also the task that calls BP32.update(). That task copies the
// inputs it needs into this plain-old-data structure, which is published under
// a spinlock and consumed by the motor task on the other core. Nothing else
// crosses cores.
struct DriveCommand {
    bool    connected;
    int16_t stickY;      // forward/back, already sign-corrected (+ = forward)
    int16_t stickRX;     // turn
    bool    turbo;
    bool    actExtend;
    bool    actRetract;
};

static const DriveCommand NEUTRAL_COMMAND = {false, 0, 0, false, false, false};

static portMUX_TYPE  g_cmdMux      = portMUX_INITIALIZER_UNLOCKED;
static DriveCommand  g_sharedCmd   = NEUTRAL_COMMAND;

static void publishCommand(const DriveCommand& cmd) {
    portENTER_CRITICAL(&g_cmdMux);
    g_sharedCmd = cmd;
    portEXIT_CRITICAL(&g_cmdMux);
}

static DriveCommand consumeCommand() {
    DriveCommand cmd;
    portENTER_CRITICAL(&g_cmdMux);
    cmd = g_sharedCmd;
    portEXIT_CRITICAL(&g_cmdMux);
    return cmd;
}

// ─── Controller handle (Bluetooth task only) ─────────────────────────────────
static ControllerPtr g_controller = nullptr;

// ─── Motor state (motor task only) ───────────────────────────────────────────
static int  g_leftCurrent    = 0;
static int  g_rightCurrent   = 0;
static bool g_driversEnabled = false;

// ─── Low-level outputs ───────────────────────────────────────────────────────
static void driveMotor(int channel, int pinA, int pinB, int pwm, bool invert) {
    if (invert) pwm = -pwm;

    if (pwm > 0) {
        digitalWrite(pinA, HIGH);
        digitalWrite(pinB, LOW);
    } else if (pwm < 0) {
        digitalWrite(pinA, LOW);
        digitalWrite(pinB, HIGH);
        pwm = -pwm;
    } else {
        digitalWrite(pinA, LOW);
        digitalWrite(pinB, LOW);
    }
    ledcWrite(channel, constrain(pwm, 0, MAX_PWM));
}

static void driveActuator(bool extend, bool retract) {
    // Both directions asserted, or neither, means stop. Never pick a winner.
    if (extend == retract) {
        digitalWrite(PIN_ACT_IN3, LOW);
        digitalWrite(PIN_ACT_IN4, LOW);
        return;
    }

#if INVERT_ACTUATOR
    bool tmp = extend;
    extend   = retract;
    retract  = tmp;
#endif

    digitalWrite(PIN_ACT_IN3, extend  ? HIGH : LOW);
    digitalWrite(PIN_ACT_IN4, retract ? HIGH : LOW);
}

// Immediate, unsmoothed shutdown. Used at boot, on controller loss, and
// wherever an emergency stop is required — this path always bypasses the
// slew-rate limiter.
static void enterSafeState() {
    ledcWrite(LEDC_CH_LEFT, 0);
    ledcWrite(LEDC_CH_RIGHT, 0);

    digitalWrite(PIN_AIN1, LOW);
    digitalWrite(PIN_AIN2, LOW);
    digitalWrite(PIN_BIN1, LOW);
    digitalWrite(PIN_BIN2, LOW);

    digitalWrite(PIN_ACT_IN3, LOW);
    digitalWrite(PIN_ACT_IN4, LOW);

    digitalWrite(PIN_STBY, LOW);

    g_leftCurrent    = 0;
    g_rightCurrent   = 0;
    g_driversEnabled = false;
}

// ─── Control helpers ─────────────────────────────────────────────────────────
static int applyDeadzone(int value) {
    return (abs(value) < STICK_DEADZONE) ? 0 : value;
}

static int stickToPwm(int stick) {
    int magnitude = map(abs(stick), 0, STICK_MAX, 0, MAX_PWM);
    return (stick < 0) ? -magnitude : magnitude;
}

// Bounded, deterministic slew: at most SLEW_STEP_PWM counts of change per call.
static int applySlew(int current, int target) {
    int delta = target - current;
    if (delta >  SLEW_STEP_PWM) delta =  SLEW_STEP_PWM;
    if (delta < -SLEW_STEP_PWM) delta = -SLEW_STEP_PWM;
    return current + delta;
}

// ─── Bluepad32 callbacks ─────────────────────────────────────────────────────
// These run in the context of the task that calls BP32.update(), i.e. the
// Bluetooth task below.
static void onControllerConnected(ControllerPtr ctl) {
    if (g_controller == nullptr) {
        g_controller = ctl;
        Serial.println("[BT] Controller connected");
    } else {
        Serial.println("[BT] Additional controller ignored");
    }
}

static void onControllerDisconnected(ControllerPtr ctl) {
    if (g_controller == ctl) {
        g_controller = nullptr;
        publishCommand(NEUTRAL_COMMAND);
        Serial.println("[BT] Controller disconnected — entering safe state");
    }
}

// ─── Test mode ───────────────────────────────────────────────────────────────
// Runs inside the control task, so it remains the single writer of all outputs.
#if TEST_MODE
static void runTestMode() {
    Serial.println();
    Serial.println("################################################################");
    Serial.println("#                    TEST MODE IS ENABLED                      #");
    Serial.println("#                                                              #");
    Serial.println("#  The motors and the actuator are about to move on their own.  #");
    Serial.println("#                                                              #");
    Serial.println("#   - Secure the robot and lift the drive wheels clear.         #");
    Serial.println("#   - Keep hands clear of the actuator and the flipper arm.     #");
    Serial.println("#   - Make sure the actuator cannot hit its end stops or any    #");
    Serial.println("#     part of the chassis at full stroke.                       #");
    Serial.println("#   - Verify the 5 V and 12 V rails before powering drivers.    #");
    Serial.println("#   - Use a current-limited bench supply if you have one.       #");
    Serial.println("#                                                              #");
    Serial.println("#  Remove power NOW if the robot is not secured.                #");
    Serial.println("################################################################");

    for (int i = 10; i > 0; i--) {
        Serial.printf("[TEST] Starting in %d s...\n", i);
        delay(1000);
    }

    digitalWrite(PIN_STBY, HIGH);
    delay(50);

    const int testPwm = 150;

    Serial.println("[TEST] Left motor forward");
    driveMotor(LEDC_CH_LEFT, PIN_AIN1, PIN_AIN2, testPwm, INVERT_LEFT_MOTOR);
    delay(1000);
    driveMotor(LEDC_CH_LEFT, PIN_AIN1, PIN_AIN2, 0, false);
    delay(500);

    Serial.println("[TEST] Left motor backward");
    driveMotor(LEDC_CH_LEFT, PIN_AIN1, PIN_AIN2, -testPwm, INVERT_LEFT_MOTOR);
    delay(1000);
    driveMotor(LEDC_CH_LEFT, PIN_AIN1, PIN_AIN2, 0, false);
    delay(500);

    Serial.println("[TEST] Right motor forward");
    driveMotor(LEDC_CH_RIGHT, PIN_BIN1, PIN_BIN2, testPwm, INVERT_RIGHT_MOTOR);
    delay(1000);
    driveMotor(LEDC_CH_RIGHT, PIN_BIN1, PIN_BIN2, 0, false);
    delay(500);

    Serial.println("[TEST] Right motor backward");
    driveMotor(LEDC_CH_RIGHT, PIN_BIN1, PIN_BIN2, -testPwm, INVERT_RIGHT_MOTOR);
    delay(1000);
    driveMotor(LEDC_CH_RIGHT, PIN_BIN1, PIN_BIN2, 0, false);
    delay(500);

    Serial.println("[TEST] Actuator direction A (short pulse)");
    driveActuator(true, false);
    delay(500);
    driveActuator(false, false);
    delay(500);

    Serial.println("[TEST] Actuator direction B (short pulse)");
    driveActuator(false, true);
    delay(500);
    driveActuator(false, false);

    enterSafeState();

    Serial.println("[TEST] Done. If a motor or the actuator ran the wrong way,");
    Serial.println("[TEST] set INVERT_LEFT_MOTOR / INVERT_RIGHT_MOTOR / INVERT_ACTUATOR.");
}
#endif

// ─── Core 0: Bluetooth task ──────────────────────────────────────────────────
// Owns the controller object. Copies inputs into a DriveCommand and publishes
// it; never touches motor hardware.
static void bluetoothTask(void* param) {
    (void)param;

    for (;;) {
        BP32.update();

        DriveCommand cmd = NEUTRAL_COMMAND;
        ControllerPtr ctl = g_controller;

        if (ctl != nullptr && ctl->isConnected() && ctl->isGamepad()) {
            cmd.connected  = true;
            cmd.stickY     = (int16_t)(-ctl->axisY());   // stick up = forward
            cmd.stickRX    = (int16_t)(ctl->axisRX());
            cmd.turbo      = ctl->y();
            cmd.actExtend  = ctl->r1();
            cmd.actRetract = ctl->l1();
        }

        publishCommand(cmd);
        vTaskDelay(pdMS_TO_TICKS(BT_POLL_MS));
    }
}

// ─── Core 1: motor and actuator control task ─────────────────────────────────
static void motorTask(void* param) {
    (void)param;

    // From here on this task is the single runtime writer of every output.
    // setup() already drove them low at boot; re-assert the safe state so this
    // task's own bookkeeping (smoothed values, driver-enabled flag) starts from
    // a known state.
    enterSafeState();

#if TEST_MODE
    runTestMode();
    Serial.println("[TEST] Entering normal drive mode...");
#endif

    TickType_t lastWake = xTaskGetTickCount();

#if INPUT_DEBUG_MODE
    uint32_t lastLogMs   = 0;
    bool     lastTurbo   = false;
    bool     lastExtend  = false;
    bool     lastRetract = false;
#endif

    for (;;) {
        DriveCommand cmd = consumeCommand();

        if (!cmd.connected) {
            // Failsafe: immediate stop, no smoothing, driver in standby.
            if (g_driversEnabled || g_leftCurrent != 0 || g_rightCurrent != 0) {
                enterSafeState();
            }
#if INPUT_DEBUG_MODE
            lastTurbo = lastExtend = lastRetract = false;
#endif
            vTaskDelayUntil(&lastWake, pdMS_TO_TICKS(CONTROL_PERIOD_MS));
            continue;
        }

        // Freshly (re)connected: start from a known-neutral state before the
        // driver is taken out of standby, so no stale command can move us.
        if (!g_driversEnabled) {
            g_leftCurrent  = 0;
            g_rightCurrent = 0;
            driveMotor(LEDC_CH_LEFT,  PIN_AIN1, PIN_AIN2, 0, false);
            driveMotor(LEDC_CH_RIGHT, PIN_BIN1, PIN_BIN2, 0, false);
            driveActuator(false, false);
            digitalWrite(PIN_STBY, HIGH);
            g_driversEnabled = true;
        }

        int stickY  = applyDeadzone(cmd.stickY);
        int stickRX = applyDeadzone(cmd.stickRX);

        // Tank-style mixing
        int left  = constrain(stickY + stickRX, -STICK_MAX, STICK_MAX);
        int right = constrain(stickY - stickRX, -STICK_MAX, STICK_MAX);

        int leftTarget  = stickToPwm(left);
        int rightTarget = stickToPwm(right);

        if (cmd.turbo) {
            // Bypass command smoothing and immediately apply the current
            // joystick command. This is not a boost: at half stick it produces
            // half output, just without the slew limit.
            g_leftCurrent  = leftTarget;
            g_rightCurrent = rightTarget;
        } else {
            g_leftCurrent  = applySlew(g_leftCurrent,  leftTarget);
            g_rightCurrent = applySlew(g_rightCurrent, rightTarget);
        }

        driveMotor(LEDC_CH_LEFT,  PIN_AIN1, PIN_AIN2, g_leftCurrent,  INVERT_LEFT_MOTOR);
        driveMotor(LEDC_CH_RIGHT, PIN_BIN1, PIN_BIN2, g_rightCurrent, INVERT_RIGHT_MOTOR);
        driveActuator(cmd.actExtend, cmd.actRetract);

#if INPUT_DEBUG_MODE
        if (cmd.turbo != lastTurbo) {
            Serial.println(cmd.turbo ? "[IN] Y pressed (smoothing bypassed)"
                                     : "[IN] Y released");
            lastTurbo = cmd.turbo;
        }
        if (cmd.actExtend != lastExtend) {
            Serial.println(cmd.actExtend ? "[IN] R1 pressed" : "[IN] R1 released");
            lastExtend = cmd.actExtend;
        }
        if (cmd.actRetract != lastRetract) {
            Serial.println(cmd.actRetract ? "[IN] L1 pressed" : "[IN] L1 released");
            lastRetract = cmd.actRetract;
        }

        uint32_t now = millis();
        if (now - lastLogMs >= 250) {
            lastLogMs = now;
            Serial.printf("[IN] Y=%d RX=%d | Lout=%d Rout=%d | act=%s\n",
                          stickY, stickRX, g_leftCurrent, g_rightCurrent,
                          (cmd.actExtend == cmd.actRetract) ? "stop"
                              : (cmd.actExtend ? "A" : "B"));
        }
#endif

        vTaskDelayUntil(&lastWake, pdMS_TO_TICKS(CONTROL_PERIOD_MS));
    }
}

// ─── Setup ───────────────────────────────────────────────────────────────────
void setup() {
    Serial.begin(115200);
    Serial.println();
    Serial.println("[GHOST] Drive system starting...");

    pinMode(PIN_AIN1, OUTPUT);
    pinMode(PIN_AIN2, OUTPUT);
    pinMode(PIN_BIN1, OUTPUT);
    pinMode(PIN_BIN2, OUTPUT);
    pinMode(PIN_STBY, OUTPUT);
    pinMode(PIN_ACT_IN3, OUTPUT);
    pinMode(PIN_ACT_IN4, OUTPUT);

    // One-time boot initialisation: explicitly drive every control output to its
    // safe state rather than trusting the reset values of the output latches.
    // A watchdog reset, a soft restart or a bootloader entry does not
    // necessarily leave those latches clear, and STBY floating or high with
    // stale direction bits could twitch the motors before the control task
    // starts. This runs before either task is created; from then on motorTask
    // is the sole runtime writer of these outputs.
    digitalWrite(PIN_STBY, LOW);        // driver in standby first
    digitalWrite(PIN_AIN1, LOW);
    digitalWrite(PIN_AIN2, LOW);
    digitalWrite(PIN_BIN1, LOW);
    digitalWrite(PIN_BIN2, LOW);
    digitalWrite(PIN_ACT_IN3, LOW);
    digitalWrite(PIN_ACT_IN4, LOW);

    ledcSetup(LEDC_CH_LEFT,  PWM_FREQ, PWM_RES_BITS);
    ledcAttachPin(PIN_PWMA,  LEDC_CH_LEFT);
    ledcSetup(LEDC_CH_RIGHT, PWM_FREQ, PWM_RES_BITS);
    ledcAttachPin(PIN_PWMB,  LEDC_CH_RIGHT);

    // Both PWM channels explicitly at zero duty.
    ledcWrite(LEDC_CH_LEFT,  0);
    ledcWrite(LEDC_CH_RIGHT, 0);

    publishCommand(NEUTRAL_COMMAND);

    BP32.setup(&onControllerConnected, &onControllerDisconnected);
#if RESET_BT_KEYS_ON_BOOT
    Serial.println("[BT] Clearing stored Bluetooth keys — re-pairing required");
    BP32.forgetBluetoothKeys();
#endif

    xTaskCreatePinnedToCore(bluetoothTask, "BT",    4096, NULL, 1, NULL, 0);
    xTaskCreatePinnedToCore(motorTask,     "Motor", 4096, NULL, 2, NULL, 1);

    Serial.println("[GHOST] Ready. Motors stay disabled until a controller connects.");
}

void loop() {
    // All work happens in the two pinned tasks above. Idle here rather than
    // deleting the Arduino loop task.
    vTaskDelay(pdMS_TO_TICKS(1000));
}