Annu JohnsAchinth P MSapnil MAbhinav Krishna
Published

Bolt :The quadruped robotic dog

BOLT is a modular quadruped robotic scout that enters hazardous environments first

ExpertWork in progressOver 10 days695
Bolt :The quadruped robotic dog

Things used in this project

Hardware components

Jetson Orin Nano Developer Kit
NVIDIA Jetson Orin Nano Developer Kit
×1
Eaglepower 90 kv bldc motor
×1
mks odrive mini
×1
Grove - Temperature, Humidity, Pressure and Gas Sensor (BME680)
Seeed Studio Grove - Temperature, Humidity, Pressure and Gas Sensor (BME680)
×1
Grove - VOC and eCO2 Gas Sensor SGP30
Seeed Studio Grove - VOC and eCO2 Gas Sensor SGP30
×1
Grove - Oxygen Sensor(ME2-O2-Ф20)
Seeed Studio Grove - Oxygen Sensor(ME2-O2-Ф20)
×1
Grove - Multichannel Gas Sensor v2
Seeed Studio Grove - Multichannel Gas Sensor v2
×1
Grove - I2C Hub
Seeed Studio Grove - I2C Hub
×1
Intel RealSense Camera
Intel RealSense Camera
×1
Raggibb RPLIDAR S2E
×1
Seeed Studio SenseCAP Card Tracker T1000-E for Meshtastic
×1
Seeed Studio Hazard Response Mission Pack - All-in-one Compact Kit, Open-source AIoT Solution, Meshtastic-powered, No/Low-code
×1
Seeed Studio Wio Tracker L1 Lite
×1

Software apps and online services

Arduino IDE
Arduino IDE
MQTT
MQTT
Fusion
Autodesk Fusion
Node-RED
Node-RED
bambu labs
VS Code
Microsoft VS Code
Robot Operating System
ROS Robot Operating System

Hand tools and fabrication machines

3D Printer (generic)
3D Printer (generic)
Soldering iron (generic)
Soldering iron (generic)
Solder Wire, Lead Free
Solder Wire, Lead Free

Story

Read more

Custom parts and enclosures

BOLT CAD FILE

Schematics

BOLT ARCHITECTURE

Code

Code to find and set the hip motors to standing position.

Arduino
// ============================================================
//  HIP STANDING POSITION ONLY
//
//  Current encoder position is assumed as HOME ZERO.
//
//  Commands:
//    g  -> Go to standing position
//    m  -> Measure offset from current home
//    p  -> Read encoder position + velocity
//    i  -> Force IDLE
//    s  -> Stop motor
//    ?  -> Menu
// ============================================================

#include <Arduino.h>
#include <FlexCAN_T4.h>
#include <ODriveTeensyCAN.h>

ODriveTeensyCAN odriveCAN(500000);

// ── Target axis ──────────────────────────────────────────────
const int FL_HIP_AXIS = 2;

// ── Home direction ───────────────────────────────────────────
const float HOME_DIRECTION = 1.0f;

// ── Standing position ────────────────────────────────────────
const float STANDING_OFFSET   = 0.7284f;

const float STAND_VELOCITY    = 1.0f;
const float STAND_CURRENT_LIM = 6.0f;

// ── State ────────────────────────────────────────────────────
bool  homeComplete = false;
float homeZeroPos  = 0.0f;

// ============================================================
//  Read encoder position AND velocity
// ============================================================
bool readEncoderPosVel(int axisId, float &outPos, float &outVel, unsigned long timeout = 200) {

  CAN_message_t flushMsg;
  while (odriveCAN.ReadMsg(flushMsg)) { }

  odriveCAN.GetPositionVelocity(axisId);

  uint32_t expectedId = ((uint32_t)axisId << 5) | 0x009;
  unsigned long deadline = millis() + timeout;

  while (millis() < deadline) {

    CAN_message_t inMsg;

    if (!odriveCAN.ReadMsg(inMsg)) continue;

    if (inMsg.id == expectedId && inMsg.len >= 8) {

      memcpy(&outPos, &inMsg.buf[0], sizeof(float));
      memcpy(&outVel, &inMsg.buf[4], sizeof(float));

      return true;
    }
  }

  return false;
}

// ============================================================
//  Position only
// ============================================================
bool readEncoderPos(int axisId, float &outPos, unsigned long timeout = 200) {

  float vel;
  return readEncoderPosVel(axisId, outPos, vel, timeout);
}

// ============================================================
//  Force IDLE
// ============================================================
void forceIdle() {

  odriveCAN.SetVelocity(FL_HIP_AXIS, 0.0f, 0.0f);

  delay(50);

  odriveCAN.RunState(FL_HIP_AXIS, 1);

  delay(100);

  Serial.println(F(">> Axis forced to IDLE."));
}

// ============================================================
//  Stop motor
// ============================================================
void stopMotor() {

  odriveCAN.SetVelocity(FL_HIP_AXIS, 0.0f, 0.0f);

  Serial.println(F(">> Velocity set to 0."));
}

// ============================================================
//  Move to target position
// ============================================================
bool moveToPosition(float target, float speed, float currentLim) {

  float pos;

  if (!readEncoderPos(FL_HIP_AXIS, pos)) {

    Serial.println(F("!!! Can't read position"));

    return false;
  }

  float distance  = target - pos;
  float direction = (distance > 0) ? 1.0f : -1.0f;

  if (fabs(distance) < 0.01f) {

    Serial.println(F("  Already at target."));

    return true;
  }

  Serial.print(F("  Moving "));
  Serial.print(distance, 4);
  Serial.println(F(" turns"));

  odriveCAN.ClearErrors(FL_HIP_AXIS);

  delay(50);

  odriveCAN.SetLimits(FL_HIP_AXIS, speed + 0.5f, currentLim);

  delay(50);

  odriveCAN.SetControllerModes(FL_HIP_AXIS, 2, 1);

  delay(50);

  odriveCAN.RunState(FL_HIP_AXIS, 8);

  delay(200);

  float vel = speed * direction;

  odriveCAN.SetVelocity(FL_HIP_AXIS, vel, 0.0f);

  unsigned long timeout = millis() + 10000;

  const float closeTolerance = 0.02f;

  while (millis() < timeout) {

    delay(50);

    if (!readEncoderPos(FL_HIP_AXIS, pos)) continue;

    float remaining = fabs(target - pos);

    // Slow down near target
    if (remaining < closeTolerance * 3) {

      odriveCAN.SetVelocity(
        FL_HIP_AXIS,
        speed * 0.3f * direction,
        0.0f
      );
    }

    // Reached target
    if (remaining < closeTolerance) {

      odriveCAN.SetVelocity(FL_HIP_AXIS, 0.0f, 0.0f);

      delay(100);

      // Hold position
      odriveCAN.SetControllerModes(FL_HIP_AXIS, 3, 3);

      delay(50);

      odriveCAN.SetPosition(FL_HIP_AXIS, target);

      delay(50);

      odriveCAN.RunState(FL_HIP_AXIS, 8);

      delay(200);

      odriveCAN.SetPosition(FL_HIP_AXIS, target);

      float finalPos;

      if (readEncoderPos(FL_HIP_AXIS, finalPos)) {

        Serial.print(F("  Arrived at: "));
        Serial.print(finalPos, 4);
        Serial.println(F(" turns"));
      }

      return true;
    }
  }

  odriveCAN.SetVelocity(FL_HIP_AXIS, 0.0f, 0.0f);

  delay(50);

  odriveCAN.RunState(FL_HIP_AXIS, 1);

  Serial.println(F("  !!! Move timed out"));

  return false;
}

// ============================================================
//  Go to standing position
// ============================================================
void goToStanding() {

  if (!homeComplete) {

    Serial.println(F("!!! Home not initialized"));

    return;
  }

  float standingPos =
    homeZeroPos - (HOME_DIRECTION * STANDING_OFFSET);

  Serial.println(F(""));

  Serial.print(F("  Home zero: "));
  Serial.println(homeZeroPos, 4);

  Serial.print(F("  Offset:    "));
  Serial.println(STANDING_OFFSET, 4);

  Serial.print(F("  Target:    "));
  Serial.println(standingPos, 4);

  if (moveToPosition(
        standingPos,
        STAND_VELOCITY,
        STAND_CURRENT_LIM)) {

    Serial.println(F("  >> AT STANDING POSITION <<"));

  } else {

    Serial.println(F("  !!! Failed"));

    forceIdle();
  }
}

// ============================================================
//  Measure offset from current home
// ============================================================
void measureOffset() {

  if (!homeComplete) {

    Serial.println(F("!!! Home not initialized"));

    return;
  }

  float pos;

  if (!readEncoderPos(FL_HIP_AXIS, pos)) {

    Serial.println(F("!!! Can't read encoder"));

    return;
  }

  float offset = fabs(pos - homeZeroPos);

  Serial.println(F(""));

  Serial.print(F("  Home zero:   "));
  Serial.println(homeZeroPos, 4);

  Serial.print(F("  Current pos: "));
  Serial.println(pos, 4);

  Serial.print(F("  Difference:  "));
  Serial.println(pos - homeZeroPos, 4);

  Serial.println(F(""));

  Serial.print(F("  >>> STANDING_OFFSET = "));
  Serial.print(offset, 4);
  Serial.println(F(" <<<"));

  Serial.print(F("  ("));

  Serial.print(offset * 360.0f / 9.0f, 2);

  Serial.println(F(" deg at joint)"));
}

// ============================================================
//  Menu
// ============================================================
void printMenu() {

  Serial.println(F(""));
  Serial.println(F("--- Commands ---"));

  Serial.println(F("  g -> Go to standing"));
  Serial.println(F("  m -> Measure offset"));
  Serial.println(F("  p -> Read position + velocity"));
  Serial.println(F("  s -> Stop motor"));
  Serial.println(F("  i -> Force IDLE"));
  Serial.println(F("  ? -> Menu"));

  Serial.println(F(""));

  Serial.print(F("  STANDING_OFFSET: "));
  Serial.println(STANDING_OFFSET, 4);

  if (homeComplete) {

    Serial.print(F("  Home zero:       "));
    Serial.println(homeZeroPos, 4);
  }

  Serial.println(F(""));
}

// ============================================================
//  SETUP
// ============================================================
void setup() {

  Serial.begin(115200);

  delay(2500);

  Serial.println(F(""));
  Serial.println(F("================================================"));
  Serial.println(F("  HIP STANDING CONTROL"));
  Serial.println(F("  Current position = HOME ZERO"));
  Serial.println(F("================================================"));

  delay(1000);

  forceIdle();

  delay(200);

  // Assume CURRENT position as HOME
  float pos, vel;

  if (readEncoderPosVel(FL_HIP_AXIS, pos, vel)) {

    homeZeroPos  = pos;
    homeComplete = true;

    Serial.print(F(">> HOME ZERO SET TO: "));
    Serial.println(homeZeroPos, 4);

    Serial.print(F(">> Velocity: "));
    Serial.println(vel, 4);

  } else {

    Serial.println(F("!!! Cannot read encoder"));
  }

  printMenu();

  Serial.println(F(">> Press 'g' for standing"));
}

// ============================================================
//  LOOP
// ============================================================
void loop() {

  if (Serial.available() > 0) {

    char cmd = Serial.read();

    switch (cmd) {

      case 'g':
      case 'G':
        goToStanding();
        break;

      case 'm':
      case 'M':
        measureOffset();
        break;

      case 'p':
      case 'P': {

        float pos, vel;

        if (readEncoderPosVel(FL_HIP_AXIS, pos, vel)) {

          Serial.print(F(">> Pos: "));
          Serial.print(pos, 4);

          Serial.print(F("  Vel: "));
          Serial.print(vel, 4);

          Serial.println(F(" turns/sec"));

          if (homeComplete) {

            Serial.print(F("   ("));
            Serial.print(pos - homeZeroPos, 4);
            Serial.println(F(" from home zero)"));
          }

        } else {

          Serial.println(F("!!! Read failed"));
        }

        break;
      }

      case 's':
      case 'S':
        stopMotor();
        break;

      case 'i':
      case 'I':
        forceIdle();
        break;

      case '?':
        printMenu();
        break;

      case '\n':
      case '\r':
      case ' ':
        break;

      default:

        Serial.print(F(">> Unknown: '"));
        Serial.print(cmd);
        Serial.println(F("'"));

        break;
    }
  }
}

code to find and set knee and abad motors to standing position.

Arduino
// ============================================================
//  KNEE AND ABAD HOMING + STANDING POSITION  (v7)
//
//  VELOCITY-BASED stall detection.
//
//  Why: belt slips at the stop, so encoder position keeps
//  changing — position-freeze detection fails. But even with
//  belt slip, actual velocity drops when the knee hits the
//  stop (the belt can't slip at full commanded speed).
//
//  Detection method:
//    - Command constant velocity toward the stop
//    - Read encoder velocity from ODrive (same CAN frame
//      as position: endpoint 0x009 returns pos + vel)
//    - When |actual_vel| drops below VEL_STALL_FRACTION of
//      |commanded_vel| for STALL_WINDOW_MS, that's a stall
//
//  Commands (Serial Monitor at 115200):
//    h  -> Home (find stop, set zero, back off)
//    g  -> Go to standing position
//    m  -> Measure offset from home zero
//    p  -> Read encoder position + velocity
//    i  -> Force IDLE
//    s  -> Stop motor
//    r  -> Repeatability summary
//    ?  -> Menu
// ============================================================

#include <Arduino.h>
#include <FlexCAN_T4.h>
#include <ODriveTeensyCAN.h>

ODriveTeensyCAN odriveCAN(500000);

// ── Target axis ──────────────────────────────────────────────
const int FL_KNEE_AXIS = 18;

// ── HOME DIRECTION ───────────────────────────────────────────
const float HOME_DIRECTION =1.0f;

// ── Homing parameters ────────────────────────────────────────
const float HOME_VELOCITY      = 0.3f;   // turns/sec
const float HOME_CURRENT_LIM   = 1.5f;   // amps — low to be gentle
const unsigned long HOME_TIMEOUT_MS = 10000;
const int   POLL_INTERVAL_MS   = 50;

// ── Velocity-based stall detection ───────────────────────────
// When actual velocity drops below this fraction of commanded
// velocity, we consider the motor stalled. Belt slip will slow
// things down but not maintain full speed.
//
// Example: commanding 0.3 t/s, fraction = 0.3
//   -> stall if actual < 0.09 t/s for STALL_WINDOW_MS
const float VEL_STALL_FRACTION = 0.3f;   // 30% of commanded speed
const int   STALL_WINDOW_MS    = 300;    // must stay slow this long

// Also require minimum travel before checking stall, so we
// don't false-trigger during initial acceleration.
const float MIN_TRAVEL_BEFORE_STALL = 0.05f;  // turns

// ── Back-off ─────────────────────────────────────────────────
const float BACKOFF_TURNS      = 0.10f;

// ── Standing position ────────────────────────────────────────
// >>> SET THIS AFTER CALIBRATION (press 'm') <<<
const float STANDING_OFFSET    = 0.7115f;
const bool  AUTO_STAND         = false;
const float STAND_VELOCITY     = 1.0f;
const float STAND_CURRENT_LIM  = 6.0f;

// ── State ────────────────────────────────────────────────────
int   homingAttempts = 0;
float capturedPositions[10];
bool  homeComplete   = false;
float homeZeroPos    = 0.0f;

// ============================================================
//  Read encoder position AND velocity via CAN
//
//  Endpoint 0x009 returns both pos_estimate and vel_estimate
//  as two floats (8 bytes total).
// ============================================================
bool readEncoderPosVel(int axisId, float &outPos, float &outVel, unsigned long timeout = 200) {
  CAN_message_t flushMsg;
  while (odriveCAN.ReadMsg(flushMsg)) { /* drain */ }

  odriveCAN.GetPositionVelocity(axisId);

  uint32_t expectedId = ((uint32_t)axisId << 5) | 0x009;
  unsigned long deadline = millis() + timeout;

  while (millis() < deadline) {
    CAN_message_t inMsg;
    if (!odriveCAN.ReadMsg(inMsg)) continue;
    if (inMsg.id == expectedId && inMsg.len >= 8) {
      // pos_estimate: bytes 0-3, vel_estimate: bytes 4-7
      memcpy(&outPos, &inMsg.buf[0], sizeof(float));
      memcpy(&outVel, &inMsg.buf[4], sizeof(float));
      return true;
    }
  }
  return false;
}

// Convenience: position only
bool readEncoderPos(int axisId, float &outPos, unsigned long timeout = 200) {
  float vel;
  return readEncoderPosVel(axisId, outPos, vel, timeout);
}

// ============================================================
//  Force axis to IDLE
// ============================================================
void forceIdle() {
  odriveCAN.SetVelocity(FL_KNEE_AXIS, 0.0f, 0.0f);
  delay(50);
  odriveCAN.RunState(FL_KNEE_AXIS, 1);
  delay(100);
  Serial.println(F(">> Axis forced to IDLE."));
}

// ============================================================
//  Stop motor
// ============================================================
void stopMotor() {
  odriveCAN.SetVelocity(FL_KNEE_AXIS, 0.0f, 0.0f);
  Serial.println(F(">> Velocity set to 0."));
}

// ============================================================
//  Velocity-based back-off, then IDLE
// ============================================================
void backoffAndIdle(float homingVel) {
  float reverseVel = -homingVel;
  Serial.print(F("  Reversing at "));
  Serial.print(reverseVel, 3);
  Serial.println(F(" turns/sec..."));

  odriveCAN.SetVelocity(FL_KNEE_AXIS, reverseVel, 0.0f);

  unsigned long reverseMs = (unsigned long)((BACKOFF_TURNS / HOME_VELOCITY) * 1000.0f);
  reverseMs = max(reverseMs, 200UL);
  reverseMs = min(reverseMs, 1500UL);

  float startPos;
  readEncoderPos(FL_KNEE_AXIS, startPos);

  delay(reverseMs);

  odriveCAN.SetVelocity(FL_KNEE_AXIS, 0.0f, 0.0f);
  delay(100);

  float endPos;
  if (readEncoderPos(FL_KNEE_AXIS, endPos)) {
    float travel = fabs(endPos - startPos);
    Serial.print(F("  Back-off travel: "));
    Serial.print(travel, 4);
    Serial.println(F(" turns"));
    if (travel < 0.005f) {
      Serial.println(F("  *** WARNING: barely moved! ***"));
    }
  }

  odriveCAN.RunState(FL_KNEE_AXIS, 1);
  delay(100);
  Serial.println(F("  Motor IDLE."));
}

// ============================================================
//  Move to a target position
// ============================================================
bool moveToPosition(float target, float speed, float currentLim) {
  float pos;
  if (!readEncoderPos(FL_KNEE_AXIS, pos)) {
    Serial.println(F("!!! Can't read position — abort"));
    return false;
  }

  float distance = target - pos;
  float direction = (distance > 0) ? 1.0f : -1.0f;

  if (fabs(distance) < 0.01f) {
    Serial.println(F("  Already at target."));
    return true;
  }

  Serial.print(F("  Moving "));
  Serial.print(distance, 4);
  Serial.println(F(" turns to target"));

  odriveCAN.ClearErrors(FL_KNEE_AXIS);
  delay(50);
  odriveCAN.SetLimits(FL_KNEE_AXIS, speed + 0.5f, currentLim);
  delay(50);
  odriveCAN.SetControllerModes(FL_KNEE_AXIS, 2, 1);
  delay(50);
  odriveCAN.RunState(FL_KNEE_AXIS, 8);
  delay(200);

  float vel = speed * direction;
  odriveCAN.SetVelocity(FL_KNEE_AXIS, vel, 0.0f);

  unsigned long timeout = millis() + 10000;
  float closeTolerance = 0.02f;

  while (millis() < timeout) {
    delay(POLL_INTERVAL_MS);
    if (!readEncoderPos(FL_KNEE_AXIS, pos)) continue;

    float remaining = fabs(target - pos);

    if (remaining < closeTolerance * 3) {
      odriveCAN.SetVelocity(FL_KNEE_AXIS, speed * 0.3f * direction, 0.0f);
    }

    if (remaining < closeTolerance) {
      odriveCAN.SetVelocity(FL_KNEE_AXIS, 0.0f, 0.0f);
      delay(100);
      odriveCAN.SetControllerModes(FL_KNEE_AXIS, 3, 3);
      delay(50);
      odriveCAN.SetPosition(FL_KNEE_AXIS, target);
      delay(50);
      odriveCAN.RunState(FL_KNEE_AXIS, 8);
      delay(200);
      odriveCAN.SetPosition(FL_KNEE_AXIS, target);

      float finalPos;
      if (readEncoderPos(FL_KNEE_AXIS, finalPos)) {
        Serial.print(F("  Arrived at: "));
        Serial.print(finalPos, 4);
        Serial.println(F(" turns"));
      }
      return true;
    }
  }

  odriveCAN.SetVelocity(FL_KNEE_AXIS, 0.0f, 0.0f);
  delay(50);
  odriveCAN.RunState(FL_KNEE_AXIS, 1);
  Serial.println(F("  !!! Move timed out"));
  return false;
}

// ============================================================
//  HOMING — velocity-based stall detection
// ============================================================
bool attemptHoming() {
  homingAttempts++;
  homeComplete = false;

  Serial.println(F(""));
  Serial.println(F("================================================"));
  Serial.print  (F("  HOMING ATTEMPT #"));
  Serial.println(homingAttempts);
  Serial.println(F("================================================"));

  float startPos;
  if (!readEncoderPos(FL_KNEE_AXIS, startPos)) {
    Serial.println(F("!!! Can't read encoder — abort"));
    return false;
  }
  Serial.print(F("  Starting position: "));
  Serial.print(startPos, 4);
  Serial.println(F(" turns"));

  // Set up
  odriveCAN.ClearErrors(FL_KNEE_AXIS);
  delay(50);
  odriveCAN.SetLimits(FL_KNEE_AXIS, 2.0f, HOME_CURRENT_LIM);
  delay(50);
  odriveCAN.SetControllerModes(FL_KNEE_AXIS, 2, 1);
  delay(50);
  odriveCAN.RunState(FL_KNEE_AXIS, 8);
  delay(200);

  // Command velocity toward the stop
  float vel = HOME_VELOCITY * HOME_DIRECTION;
  float commandedSpeed = HOME_VELOCITY;  // absolute value
  float stallSpeed = commandedSpeed * VEL_STALL_FRACTION;

  Serial.print(F("  Commanding: "));
  Serial.print(vel, 3);
  Serial.println(F(" turns/sec"));
  Serial.print(F("  Stall if |vel| < "));
  Serial.print(stallSpeed, 3);
  Serial.print(F(" turns/sec for "));
  Serial.print(STALL_WINDOW_MS);
  Serial.println(F(" ms"));

  odriveCAN.SetVelocity(FL_KNEE_AXIS, vel, 0.0f);

  // Monitor velocity for stall
  Serial.println(F("  [time]  pos      vel      status"));

  unsigned long stallStart = 0;
  bool stallTimerOn = false;
  unsigned long loopStart = millis();
  unsigned long timeout   = millis() + HOME_TIMEOUT_MS;
  int printCounter = 0;

  while (millis() < timeout) {
    delay(POLL_INTERVAL_MS);

    float pos, actualVel;
    if (!readEncoderPosVel(FL_KNEE_AXIS, pos, actualVel)) continue;

    float absVel = fabs(actualVel);
    float travel = fabs(pos - startPos);
    bool enoughTravel = (travel >= MIN_TRAVEL_BEFORE_STALL);
    bool velLow = (absVel < stallSpeed);

    // Print every ~200ms
    printCounter++;
    if (printCounter >= 4) {
      printCounter = 0;
      unsigned long elapsed = millis() - loopStart;
      Serial.print(F("  ["));
      Serial.print(elapsed);
      Serial.print(F("]  "));
      Serial.print(pos, 4);
      Serial.print(F("  "));
      Serial.print(actualVel, 4);
      Serial.print(F("  "));

      if (!enoughTravel) {
        Serial.println(F("accelerating..."));
      } else if (velLow && stallTimerOn) {
        Serial.print(F("** SLOW ** "));
        Serial.print((unsigned long)(millis() - stallStart));
        Serial.println(F("ms"));
      } else {
        Serial.println(F("moving"));
      }
    }

    // Only check stall after minimum travel (avoid false trigger during accel)
    if (!enoughTravel) {
      stallTimerOn = false;
      continue;
    }

    if (velLow) {
      if (!stallTimerOn) {
        stallTimerOn = true;
        stallStart = millis();
      }

      if (millis() - stallStart > (unsigned long)STALL_WINDOW_MS) {
        // ===== STALL DETECTED =====
        Serial.println(F(""));
        Serial.println(F("  >>> STALL DETECTED <<<"));
        Serial.print(F("  Velocity at stall: "));
        Serial.print(actualVel, 4);
        Serial.println(F(" turns/sec"));

        // Stop immediately
        odriveCAN.SetVelocity(FL_KNEE_AXIS, 0.0f, 0.0f);
        delay(200);

        // Capture home zero
        float stallPos;
        if (!readEncoderPos(FL_KNEE_AXIS, stallPos)) {
          stallPos = pos;
        }

        homeZeroPos = stallPos;
        homeComplete = true;

        Serial.println(F(""));
        Serial.print(F("  *** HOME ZERO: "));
        Serial.print(homeZeroPos, 4);
        Serial.println(F(" turns ***"));
        Serial.print(F("  Total travel: "));
        Serial.print(fabs(stallPos - startPos), 4);
        Serial.println(F(" turns"));

        if (homingAttempts <= 10) {
          capturedPositions[homingAttempts - 1] = stallPos;
        }

        // Back off
        Serial.println(F(""));
        Serial.println(F("  --- Backing off ---"));
        backoffAndIdle(vel);

        // Report
        float finalPos;
        if (readEncoderPos(FL_KNEE_AXIS, finalPos)) {
          float clearance = fabs(finalPos - stallPos);
          Serial.print(F("  Position:  "));
          Serial.print(finalPos, 4);
          Serial.println(F(" turns"));
          Serial.print(F("  Clearance: "));
          Serial.print(clearance, 4);
          Serial.print(F(" turns ("));
          Serial.print(clearance * 360.0f / 9.0f, 2);
          Serial.println(F(" deg)"));
        }

        Serial.println(F(""));

        if (AUTO_STAND && STANDING_OFFSET != 0.0f) {
          Serial.println(F("  --- Auto-moving to standing ---"));
          goToStanding();
        } else if (STANDING_OFFSET == 0.0f) {
          Serial.println(F("  CALIBRATION MODE:"));
          Serial.println(F("    1. 'i' to go idle"));
          Serial.println(F("    2. Move leg to standing by hand"));
          Serial.println(F("    3. 'm' to measure offset"));
        } else {
          Serial.println(F("  Press 'g' to go to standing."));
        }

        return true;
      }
    } else {
      // Velocity recovered — reset timer
      stallTimerOn = false;
    }
  }

  // Timeout
  Serial.println(F(""));
  Serial.println(F("!!! TIMEOUT — stall never detected"));
  Serial.println(F("    The motor ran for the full timeout period."));
  Serial.println(F("    Possible causes:"));
  Serial.println(F("    - Belt slipping too fast (try lower HOME_CURRENT_LIM)"));
  Serial.println(F("    - VEL_STALL_FRACTION too low (actual vel stays above threshold)"));
  Serial.println(F("    - Motor never reached the stop"));
  Serial.println(F(""));
  Serial.println(F("    Look at the velocity values above:"));
  Serial.println(F("    - If vel stayed near commanded speed -> stop not reached"));
  Serial.println(F("    - If vel dropped but not below threshold -> raise VEL_STALL_FRACTION"));

  odriveCAN.SetVelocity(FL_KNEE_AXIS, 0.0f, 0.0f);
  delay(50);
  odriveCAN.RunState(FL_KNEE_AXIS, 1);
  Serial.println(F(">> IDLE after timeout."));
  return false;
}

// ============================================================
//  Go to standing position
// ============================================================
void goToStanding() {
  if (!homeComplete) {
    Serial.println(F("!!! Home first (press 'h')"));
    return;
  }
  if (STANDING_OFFSET == 0.0f) {
    Serial.println(F("!!! STANDING_OFFSET is 0 — calibrate first ('m')"));
    return;
  }

  float standingPos = homeZeroPos - (HOME_DIRECTION * STANDING_OFFSET);

  Serial.println(F(""));
  Serial.print(F("  Home zero: ")); Serial.println(homeZeroPos, 4);
  Serial.print(F("  Offset:    ")); Serial.println(STANDING_OFFSET, 4);
  Serial.print(F("  Target:    ")); Serial.println(standingPos, 4);

  if (moveToPosition(standingPos, STAND_VELOCITY, STAND_CURRENT_LIM)) {
    Serial.println(F("  >> AT STANDING POSITION <<"));
  } else {
    Serial.println(F("  !!! Failed"));
    forceIdle();
  }
}

// ============================================================
//  Measure offset from home zero
// ============================================================
void measureOffset() {
  if (!homeComplete) {
    Serial.println(F("!!! Home first ('h')"));
    return;
  }

  float pos;
  if (!readEncoderPos(FL_KNEE_AXIS, pos)) {
    Serial.println(F("!!! Can't read encoder"));
    return;
  }

  float offset = fabs(pos - homeZeroPos);

  Serial.println(F(""));
  Serial.print(F("  Home zero:    ")); Serial.println(homeZeroPos, 4);
  Serial.print(F("  Current pos:  ")); Serial.println(pos, 4);
  Serial.print(F("  Difference:   ")); Serial.println(pos - homeZeroPos, 4);
  Serial.println(F(""));
  Serial.print(F("  >>> STANDING_OFFSET = "));
  Serial.print(offset, 4);
  Serial.println(F(" <<<"));
  Serial.print(F("  ("));
  Serial.print(offset * 360.0f / 9.0f, 2);
  Serial.println(F(" deg at joint)"));
  Serial.println(F(""));
  Serial.println(F("  Put this value in STANDING_OFFSET and re-flash."));
}

// ============================================================
//  Repeatability
// ============================================================
void printRepeatability() {
  int n = min(homingAttempts, 10);
  if (n < 2) {
    Serial.println(F(">> Need at least 2 homings."));
    return;
  }

  float mn = capturedPositions[0], mx = capturedPositions[0];
  float sum = 0.0f;
  for (int i = 0; i < n; i++) {
    if (capturedPositions[i] < mn) mn = capturedPositions[i];
    if (capturedPositions[i] > mx) mx = capturedPositions[i];
    sum += capturedPositions[i];
  }
  float spread = mx - mn;

  Serial.println(F(""));
  Serial.println(F("--- Repeatability ---"));
  for (int i = 0; i < n; i++) {
    Serial.print(F("  #")); Serial.print(i+1);
    Serial.print(F(": ")); Serial.println(capturedPositions[i], 4);
  }
  Serial.print(F("  Spread: ")); Serial.print(spread, 4);
  Serial.print(F(" turns (")); Serial.print(spread * 360.0f / 9.0f, 2);
  Serial.println(F(" deg)"));

  if (spread < 0.02f) Serial.println(F("  EXCELLENT"));
  else if (spread < 0.05f) Serial.println(F("  ACCEPTABLE"));
  else Serial.println(F("  POOR"));
}

// ============================================================
//  Menu
// ============================================================
void printMenu() {
  Serial.println(F(""));
  Serial.println(F("--- Commands ---"));
  Serial.println(F("  h -> Home"));
  Serial.println(F("  g -> Go to standing"));
  Serial.println(F("  m -> Measure offset"));
  Serial.println(F("  p -> Read position + velocity"));
  Serial.println(F("  s -> Stop motor"));
  Serial.println(F("  i -> Force IDLE"));
  Serial.println(F("  r -> Repeatability"));
  Serial.println(F("  ? -> Menu"));
  Serial.println(F(""));
  Serial.print(F("  HOME_DIRECTION:      ")); Serial.println(HOME_DIRECTION);
  Serial.print(F("  HOME_VELOCITY:       ")); Serial.print(HOME_VELOCITY); Serial.println(F(" t/s"));
  Serial.print(F("  HOME_CURRENT_LIM:    ")); Serial.print(HOME_CURRENT_LIM); Serial.println(F(" A"));
  Serial.print(F("  VEL_STALL_FRACTION:  ")); Serial.println(VEL_STALL_FRACTION);
  Serial.print(F("  STALL_WINDOW_MS:     ")); Serial.println(STALL_WINDOW_MS);
  Serial.print(F("  STANDING_OFFSET:     ")); Serial.println(STANDING_OFFSET, 4);
  if (homeComplete) {
    Serial.print(F("  Home zero:           ")); Serial.println(homeZeroPos, 4);
  }
  Serial.println(F(""));
}

// ============================================================
//  SETUP
// ============================================================
void setup() {
  Serial.begin(115200);
  delay(2500);

  Serial.println(F(""));
  Serial.println(F("================================================"));
  Serial.println(F("  KNEE HOMING (v7) — velocity stall detection"));
  Serial.println(F("  Target: FL knee (axis 4)"));
  Serial.println(F("================================================"));

  delay(1500);
  forceIdle();
  delay(200);

  float pos, vel;
  if (readEncoderPosVel(FL_KNEE_AXIS, pos, vel)) {
    Serial.print(F(">> Position: ")); Serial.print(pos, 4);
    Serial.print(F("  Velocity: ")); Serial.println(vel, 4);
  } else {
    Serial.println(F("!!! Cannot read encoder"));
  }

  printMenu();

  if (STANDING_OFFSET == 0.0f) {
    Serial.println(F(">> CALIBRATION MODE"));
    Serial.println(F(">>  h -> home, i -> idle, move leg, m -> measure"));
  } else {
    Serial.println(F(">> h -> home, g -> standing"));
  }
}

// ============================================================
//  LOOP
// ============================================================
void loop() {
  if (Serial.available() > 0) {
    char cmd = Serial.read();

    switch (cmd) {
      case 'h': case 'H':
        attemptHoming();
        break;

      case 'g': case 'G':
        goToStanding();
        break;

      case 'm': case 'M':
        measureOffset();
        break;

      case 'p': case 'P': {
        float pos, vel;
        if (readEncoderPosVel(FL_KNEE_AXIS, pos, vel)) {
          Serial.print(F(">> Pos: ")); Serial.print(pos, 4);
          Serial.print(F("  Vel: ")); Serial.print(vel, 4);
          Serial.println(F(" turns/sec"));
          if (homeComplete) {
            Serial.print(F("   (")); Serial.print(pos - homeZeroPos, 4);
            Serial.println(F(" from home zero)"));
          }
        } else {
          Serial.println(F("!!! Read failed"));
        }
        break;
      }

      case 's': case 'S': stopMotor(); break;
      case 'i': case 'I': forceIdle(); break;
      case 'r': case 'R': printRepeatability(); break;
      case '?': printMenu(); break;
      case '\n': case '\r': case ' ': break;

      default:
        Serial.print(F(">> Unknown: '")); Serial.print(cmd);
        Serial.println(F("' — '?'"));
        break;
    }
  }
}

final code

Arduino
// ============================================================
//  TOPS Quadruped — MERGED FIRMWARE
//
//  Boot flow:
//    1. Press 'f' (or 'a'/'j'/'k') -> stand-up sequence from
//       program 2 (hardstop homing on ABAD+KNEE, capture on HIP)
//       Robot ends in its mechanical standing pose, each motor
//       holding state[i].homeZeroPos - dir*standingOffset[i].
//
//    2. Press 'r' -> bridge into IK-driven motion (program 1):
//       - back-compute offsetFL/BL/FR/BR[] from the standing
//         encoder turns so program-1's IK home pose matches
//         the standing pose the robot is currently in
//       - reconfigure all 12 axes into POSITION + POS_FILTER
//         control with program-1's gains
//       - now 'w','T','F','B','L','R','C','P','D','V','N' work
//
//    3. Press 'i' to idle, 'r' to re-bridge after re-homing.
//
//  IMPORTANT: Place HIPs at max-limit before 'f' or 'j' so the
//  captured zero position is consistent with the offsets used
//  in standingOffset[].
//
//  Axis IDs (unchanged from both programs):
//    FL: 0(abad), 2(hip),  4(knee)
//    BL:12(abad),14(hip), 16(knee)
//    FR: 6(abad), 8(hip), 10(knee)
//    BR:18(abad),20(hip), 22(knee)
// ============================================================

#include <Arduino.h>
#include <FlexCAN_T4.h>
#include <ODriveTeensyCAN.h>
#include "QIK.h"

ODriveTeensyCAN odriveCAN(500000);

// =============================================================
//  ====================  STAND-UP SECTION  ====================
//  (copied/adapted from program 2 — finds mechanical zeros)
// =============================================================

enum JointType { ABAD, HIP, KNEE };

struct MotorConfig {
  int   axisId;
  float homeDirection;
  JointType type;
  const char* name;
};

// FIXED ORDER — standingOffset[] is indexed by this array
MotorConfig motors[] = {
  // ABAD (hardstop homing)
  { 12, -1.0f, ABAD, "ABAD FL (12)" },   // 0   ** axis 12 is BL in program 1
  {  0, +1.0f, ABAD, "ABAD RL (0)"  },   // 1   ** axis 0  is FL in program 1
  {  6, -1.0f, ABAD, "ABAD FR (6)"  },   // 2   ** axis 6  is FR in program 1
  { 18, +1.0f, ABAD, "ABAD RR (18)" },   // 3   ** axis 18 is BR in program 1
  // HIP (capture current pos)
  {  2, +1.0f, HIP,  "HIP  FL (2)"  },   // 4
  { 14, +1.0f, HIP,  "HIP  RL (14)" },   // 5
  {  8, -1.0f, HIP,  "HIP  FR (8)"  },   // 6
  { 20, -1.0f, HIP,  "HIP  RR (20)" },   // 7
  // KNEE (hardstop homing)
  {  4, -1.0f, KNEE, "KNEE FL (4)"  },   // 8
  { 16, -1.0f, KNEE, "KNEE RL (16)" },   // 9
  { 10, +1.0f, KNEE, "KNEE FR (10)" },   // 10
  { 22, +1.0f, KNEE, "KNEE RR (22)" }    // 11
};
const int NUM_MOTORS = sizeof(motors) / sizeof(motors[0]);  // 12

// Per-leg standing offsets (turns from home zero)
float standingOffset[12] = {
  // ABAD: FL,    BL,     FR,    BR
       0.9785f, 1.0352f, 0.9626f, 0.8485f,
  // HIP:  FL,    BL,     FR,    BR
       0.7284f, 0.7284f, 0.7284f, 0.7284f,
  // KNEE: FL,    BL,     FR,    BR
       1.9718f, 1.9718f, 1.9718f, 1.9718f
};

// Stand-up tuning
const float HOME_VELOCITY          = 0.3f;
const float HOME_CURRENT_LIM       = 3.0f;
const unsigned long HOME_TIMEOUT_MS= 20000;
const int   POLL_INTERVAL_MS       = 50;
const float VEL_STALL_FRACTION     = 0.5f;
const int   STALL_WINDOW_MS        = 300;
const float MIN_TRAVEL_BEFORE_STALL= 0.05f;
const float BACKOFF_TURNS          = 0.10f;
const float STAND_VELOCITY         = 1.0f;
const float STAND_CURRENT_LIM      = 6.0f;

struct MotorState {
  bool  homeComplete;
  float homeZeroPos;
  float standingTurns;   // homeZeroPos - dir*standingOffset (set after stand)
  bool  standComplete;
};
MotorState mstate[12];

// ============================================================
//  Low-level CAN reads (from program 2)
// ============================================================
bool readEncoderPosVel(int axisId, float &outPos, float &outVel, unsigned long timeout = 200) {
  CAN_message_t flushMsg;
  while (odriveCAN.ReadMsg(flushMsg)) { /* drain */ }
  odriveCAN.GetPositionVelocity(axisId);
  uint32_t expectedId = ((uint32_t)axisId << 5) | 0x009;
  unsigned long deadline = millis() + timeout;
  while (millis() < deadline) {
    CAN_message_t inMsg;
    if (!odriveCAN.ReadMsg(inMsg)) continue;
    if (inMsg.id == expectedId && inMsg.len >= 8) {
      memcpy(&outPos, &inMsg.buf[0], sizeof(float));
      memcpy(&outVel, &inMsg.buf[4], sizeof(float));
      return true;
    }
  }
  return false;
}
bool readEncoderPos(int axisId, float &outPos, unsigned long timeout = 200) {
  float vel;
  return readEncoderPosVel(axisId, outPos, vel, timeout);
}

void forceIdleAxis(int axisId) {
  odriveCAN.SetVelocity(axisId, 0.0f, 0.0f);
  delay(20);
  odriveCAN.RunState(axisId, 1);
  delay(50);
}
void forceIdleAll() {
  Serial.println(F(">> Forcing all axes IDLE..."));
  for (int i = 0; i < NUM_MOTORS; i++) forceIdleAxis(motors[i].axisId);
  Serial.println(F(">> All IDLE."));
}
void stopAll() {
  for (int i = 0; i < NUM_MOTORS; i++)
    odriveCAN.SetVelocity(motors[i].axisId, 0.0f, 0.0f);
  Serial.println(F(">> All velocities set to 0."));
}

void backoffAndIdle(int axisId, float homingVel) {
  odriveCAN.SetVelocity(axisId, -homingVel, 0.0f);
  unsigned long reverseMs = (unsigned long)((BACKOFF_TURNS / HOME_VELOCITY) * 1000.0f);
  reverseMs = max(reverseMs, 200UL);
  reverseMs = min(reverseMs, 1500UL);
  delay(reverseMs);
  odriveCAN.SetVelocity(axisId, 0.0f, 0.0f);
  delay(50);
  odriveCAN.RunState(axisId, 1);
  delay(50);
}

// ============================================================
//  Hardstop homing (ABAD + KNEE)
// ============================================================
bool homeMotorHardstop(int motorIdx) {
  MotorConfig &m = motors[motorIdx];
  mstate[motorIdx].homeComplete = false;

  Serial.println(F(""));
  Serial.print(F("  HOMING (hardstop): ")); Serial.println(m.name);

  float startPos;
  if (!readEncoderPos(m.axisId, startPos)) {
    Serial.print(F("!!! Can't read encoder on ")); Serial.println(m.name);
    return false;
  }
  Serial.print(F("  Start pos: ")); Serial.println(startPos, 4);

  odriveCAN.ClearErrors(m.axisId);                   delay(30);
  odriveCAN.SetLimits(m.axisId, 2.0f, HOME_CURRENT_LIM); delay(30);
  odriveCAN.SetControllerModes(m.axisId, 2, 1);      delay(30);
  odriveCAN.RunState(m.axisId, 8);                   delay(150);

  float vel = HOME_VELOCITY * m.homeDirection;
  float stallSpeed = HOME_VELOCITY * VEL_STALL_FRACTION;
  odriveCAN.SetVelocity(m.axisId, vel, 0.0f);

  unsigned long stallStart = 0;
  bool stallTimerOn = false;
  bool reachedSpeed = false;
  unsigned long loopStart = millis();
  unsigned long timeout   = millis() + HOME_TIMEOUT_MS;
  int printCounter = 0;

  while (millis() < timeout) {
    delay(POLL_INTERVAL_MS);
    float pos, actualVel;
    if (!readEncoderPosVel(m.axisId, pos, actualVel)) continue;

    float absVel = fabs(actualVel);
    float travel = fabs(pos - startPos);
    bool enoughTravel = (travel >= MIN_TRAVEL_BEFORE_STALL);
    bool velLow = (absVel < stallSpeed);
    if (absVel > HOME_VELOCITY * 0.7f) reachedSpeed = true;

    printCounter++;
    if (printCounter >= 4) {
      printCounter = 0;
      unsigned long elapsed = millis() - loopStart;
      Serial.print(F("  [")); Serial.print(elapsed); Serial.print(F("]  "));
      Serial.print(pos, 4); Serial.print(F("  ")); Serial.print(actualVel, 4); Serial.print(F("  "));
      if (!enoughTravel)         Serial.println(F("accel"));
      else if (!reachedSpeed)    Serial.println(F("waiting"));
      else if (velLow && stallTimerOn) { Serial.print(F("SLOW ")); Serial.println((unsigned long)(millis() - stallStart)); }
      else                       Serial.println(F("moving"));
    }

    if (!enoughTravel || !reachedSpeed) { stallTimerOn = false; continue; }

    if (velLow) {
      if (!stallTimerOn) { stallTimerOn = true; stallStart = millis(); }
      if (millis() - stallStart > (unsigned long)STALL_WINDOW_MS) {
        Serial.println(F("  >>> STALL <<<"));
        odriveCAN.SetVelocity(m.axisId, 0.0f, 0.0f);
        delay(150);
        float stallPos;
        if (!readEncoderPos(m.axisId, stallPos)) stallPos = pos;
        mstate[motorIdx].homeZeroPos  = stallPos;
        mstate[motorIdx].homeComplete = true;
        Serial.print(F("  HOME ZERO: ")); Serial.println(stallPos, 4);
        backoffAndIdle(m.axisId, vel);
        return true;
      }
    } else {
      stallTimerOn = false;
    }
  }

  Serial.print(F("!!! TIMEOUT homing ")); Serial.println(m.name);
  odriveCAN.SetVelocity(m.axisId, 0.0f, 0.0f);
  delay(30);
  odriveCAN.RunState(m.axisId, 1);
  return false;
}

// ============================================================
//  HIP capture-home
// ============================================================
bool captureHipHomeZero(int motorIdx) {
  MotorConfig &m = motors[motorIdx];
  mstate[motorIdx].homeComplete = false;
  forceIdleAxis(m.axisId);
  delay(80);
  float pos, vel;
  Serial.print(F("  ")); Serial.print(m.name); Serial.print(F(": "));
  if (!readEncoderPosVel(m.axisId, pos, vel)) {
    Serial.println(F("READ FAIL"));
    return false;
  }
  mstate[motorIdx].homeZeroPos  = pos;
  mstate[motorIdx].homeComplete = true;
  Serial.print(F("home zero = ")); Serial.println(pos, 4);
  return true;
}

float computeStandingTarget(int motorIdx) {
  MotorConfig &m = motors[motorIdx];
  return mstate[motorIdx].homeZeroPos - (m.homeDirection * standingOffset[motorIdx]);
}

// ============================================================
//  Move a group of motors to standing
// ============================================================
bool moveGroupToStanding(const int* motorIndices, int count) {
  Serial.println(F("  MOVING GROUP TO STANDING"));
  for (int i = 0; i < count; i++) {
    int idx = motorIndices[i];
    if (!mstate[idx].homeComplete) {
      Serial.print(F("!!! ")); Serial.print(motors[idx].name); Serial.println(F(" not homed"));
      return false;
    }
  }

  float targets[12], startPos[12], directions[12];
  bool  done[12];

  for (int i = 0; i < count; i++) {
    int idx = motorIndices[i], ax = motors[idx].axisId;
    odriveCAN.ClearErrors(ax);                       delay(40);
    odriveCAN.SetLimits(ax, STAND_VELOCITY + 0.5f, STAND_CURRENT_LIM); delay(40);
    odriveCAN.SetControllerModes(ax, 2, 1);          delay(40);
  }
  for (int i = 0; i < count; i++) {
    int idx = motorIndices[i], ax = motors[idx].axisId;
    odriveCAN.SetVelocity(ax, 0.0f, 0.0f);           delay(20);
    odriveCAN.RunState(ax, 8);                       delay(40);
    odriveCAN.SetVelocity(ax, 0.0f, 0.0f);           delay(20);
  }
  delay(200);

  for (int i = 0; i < count; i++) {
    int idx = motorIndices[i], ax = motors[idx].axisId;
    float pos;
    if (!readEncoderPos(ax, pos)) {
      Serial.print(F("!!! Read fail on ")); Serial.println(motors[idx].name);
      for (int j = 0; j < count; j++) {
        odriveCAN.SetVelocity(motors[motorIndices[j]].axisId, 0.0f, 0.0f);
        odriveCAN.RunState(motors[motorIndices[j]].axisId, 1);
      }
      return false;
    }
    targets[i]    = computeStandingTarget(idx);
    startPos[i]   = pos;
    directions[i] = (targets[i] - pos > 0) ? 1.0f : -1.0f;
    done[i]       = (fabs(targets[i] - pos) < 0.02f);
    Serial.print(F("  ")); Serial.print(motors[idx].name);
    Serial.print(F(": pos=")); Serial.print(pos, 4);
    Serial.print(F(" target=")); Serial.println(targets[i], 4);
  }

  for (int i = 0; i < count; i++) {
    if (done[i]) continue;
    int idx = motorIndices[i];
    odriveCAN.SetVelocity(motors[idx].axisId, STAND_VELOCITY * directions[i], 0.0f);
    delay(15);
  }

  const float closeTolerance = 0.02f;
  unsigned long timeout = millis() + 12000;
  int doneCount = 0;
  for (int i = 0; i < count; i++) if (done[i]) doneCount++;

  while (millis() < timeout && doneCount < count) {
    delay(POLL_INTERVAL_MS);
    for (int i = 0; i < count; i++) {
      if (done[i]) continue;
      int idx = motorIndices[i], ax = motors[idx].axisId;
      float pos;
      if (!readEncoderPos(ax, pos)) continue;
      float signedRemaining = (targets[i] - pos) * directions[i];

      if (signedRemaining > 0 && signedRemaining < closeTolerance * 3) {
        odriveCAN.SetVelocity(ax, STAND_VELOCITY * 0.3f * directions[i], 0.0f);
      }
      if (signedRemaining <= closeTolerance) {
        odriveCAN.SetVelocity(ax, 0.0f, 0.0f);          delay(50);
        odriveCAN.SetControllerModes(ax, 3, 3);         delay(20);
        odriveCAN.SetPosition(ax, targets[i]);          delay(20);
        odriveCAN.RunState(ax, 8);                      delay(80);
        odriveCAN.SetPosition(ax, targets[i]);
        done[i] = true;
        doneCount++;
        mstate[idx].standingTurns = targets[i];   // <-- remember for bridge
        mstate[idx].standComplete = true;
        Serial.print(F("  ")); Serial.print(motors[idx].name); Serial.println(F(" arrived"));
        continue;
      }
      float traveled = fabs(pos - startPos[i]);
      float expected = fabs(targets[i] - startPos[i]);
      if (traveled > expected + 0.15f) {
        odriveCAN.SetVelocity(ax, 0.0f, 0.0f); delay(30);
        odriveCAN.RunState(ax, 1);
        done[i] = true; doneCount++;
        Serial.print(F("  !!! RUNAWAY ")); Serial.println(motors[idx].name);
      }
    }
  }
  return doneCount == count;
}

bool homeAllOfType(JointType type) {
  Serial.print(F("== HOMING TYPE "));
  Serial.println(type == ABAD ? "ABAD" : type == HIP ? "HIP" : "KNEE");
  bool allOk = true;
  for (int i = 0; i < NUM_MOTORS; i++) {
    if (motors[i].type != type) continue;
    bool ok = (type == HIP) ? captureHipHomeZero(i) : homeMotorHardstop(i);
    if (!ok) allOk = false;
    delay(type == HIP ? 80 : 300);
  }
  return allOk;
}
bool standAllOfType(JointType type) {
  int indices[12], count = 0;
  for (int i = 0; i < NUM_MOTORS; i++)
    if (motors[i].type == type) indices[count++] = i;
  return moveGroupToStanding(indices, count);
}

void runAbadSequence() { if (homeAllOfType(ABAD)) { delay(500); standAllOfType(ABAD); } }
void runHipSequence()  { if (homeAllOfType(HIP))  { delay(500); standAllOfType(HIP);  } }
void runKneeSequence() { if (homeAllOfType(KNEE)) { delay(500); standAllOfType(KNEE); } }

void runFullSequence() {
  Serial.println(F("\n######## FULL STAND-UP ########"));
  runAbadSequence(); delay(800);
  runHipSequence();  delay(800);
  runKneeSequence();
  Serial.println(F("\n>> Stand-up complete. Press 'r' to enable IK motion."));
}

bool allMotorsStanding() {
  for (int i = 0; i < NUM_MOTORS; i++)
    if (!mstate[i].standComplete) return false;
  return true;
}

// =============================================================
//  ====================  IK / MOTION SECTION  =================
//  (from program 1)
// =============================================================

QIK legFL(94.5, 180, 180);
QIK legBL(94.5, 180, 180);
QIK legFR(94.5, 180, 180);
QIK legBR(94.5, 180, 180);

// CAN axis IDs  [0]=abad [1]=hip [2]=knee
const int axisFL[3] = {  0,  2,  4 };
const int axisBL[3] = { 12, 14, 16 };
const int axisFR[3] = {  6,  8, 10 };
const int axisBR[3] = { 18, 20, 22 };

// Motor direction signs
const float dirFL[3] = { 1.0f,  1.0f,  1.0f };
const float dirBL[3] = { 1.0f,  1.0f,  1.0f };
const float dirFR[3] = { 1.0f, -1.0f, -1.0f };
const float dirBR[3] = { 1.0f, -1.0f, -1.0f };

// Encoder zero offsets (degrees) — back-computed by bridge step.
// Initial values are program-1 defaults; will be overwritten on 'r'.
float offsetFL[3] = {  55.0f,  15.0f, 70.449f };
float offsetBL[3] = { 105.0f,   5.0f, 55.449f };
float offsetFR[3] = { 130.0f,  -2.0f, 89.449f };
float offsetBR[3] = {  75.0f, -14.0f, 45.449f };

// Neutral standing position (mm)
const float normX_Front = 130.0f;
const float normX_Back  = 110.0f;
float       normYF      =  60.0f;
float       normYB      = -60.0f;
const float normZ       = 300.0f;

// Gait parameters
float stepLength = 60.0f;
float stepHeight = 90.0f;
int   resolution = 100;
int   pointDelay = 8;
// ── Crawl gait parameters ────────────────────────────────────
// Crawl is statically stable: only one leg moves at a time,
// other three remain planted forming a stable support triangle.
// Body weight is shifted away from the lifting leg before lift,
// then back over the new foot after plant.
float crawlStepLength    = 50.0f;   // forward foot travel per step (mm)
float crawlStepHeight    = 70.0f;   // foot lift height (mm)
float crawlWeightShift   = 25.0f;   // body shift away from lifting leg (mm)
int   crawlSwingTimeMs   = 600;     // time for one leg's swing
int   crawlShiftTimeMs   = 400;     // time for weight shifts
int   crawlPlantDwellMs  = 200;     // pause after planting foot
int   crawlSteps         = 30;      // interpolation points per phase

// Step parameters
int   stepOffsetMm = 60;
int   stepLiftMm   = 50;
int   stepTimeMs   = 150;
int   danceTimeMs  = 15;

// Per-joint gains
float posGain[3]           = { 70.0f, 60.0f, 60.0f };
float velGain[3]           = {  0.2f,  0.2f,  0.2f };
float velIntegratorGain[3] = {  2.0f,  2.0f,  2.0f };
const char* jointName[3]   = { "ABAD", "HIP ", "KNEE" };

enum RobotState {
  STATE_IDLE,
  STATE_PRESTAND,        // awaiting 'f' (boot or after re-idle)
  STATE_STOOD,           // stand-up done, awaiting 'r' to bridge
  STATE_CLOSED_LOOP,
  STATE_HOMING,
  STATE_STANDING,
  STATE_WALKING,
  STATE_STEPPING
};
RobotState robotState = STATE_PRESTAND;

float homeTurnsFL[3] = {0,0,0};
float homeTurnsBL[3] = {0,0,0};
float homeTurnsFR[3] = {0,0,0};
float homeTurnsBR[3] = {0,0,0};

float currentTurnsFL[3] = {0,0,0};
float currentTurnsBL[3] = {0,0,0};
float currentTurnsFR[3] = {0,0,0};
float currentTurnsBR[3] = {0,0,0};

int           gaitStep     = 0;
int           gaitTotal    = 0;
unsigned long lastStepTime = 0;
int gaitStepPos = 0;

// ============================================================
//  Helpers
// ============================================================
bool isIKValid(float angleDeg) {
  if (isnan(angleDeg) || isinf(angleDeg)) return false;
  if (angleDeg < -175.0f || angleDeg > 175.0f) return false;
  return true;
}
float angleToTurns(int joint, float angleDeg, const float offset[], const float dir[]) {
  return ((angleDeg - offset[joint]) / 360.0f * 9.0f) * dir[joint];
}

// ============================================================
//  Bridge: back-compute offset[] so program-1's IK home pose
//  matches the encoder turns the robot is currently holding.
//
//  Standing pose (encoder turns) was stored in mstate[i].standingTurns.
//  Program-1 IK at neutral foot position produces neutralAngle[i].
//  We want: ((neutralAngle - offset)/360 * 9) * dir == standingTurns
//        => offset = neutralAngle - (standingTurns / dir) * 360 / 9
// ============================================================
int mstateIndexFor(int axisId) {
  for (int i = 0; i < NUM_MOTORS; i++)
    if (motors[i].axisId == axisId) return i;
  return -1;
}

void calibrateOffsetForLeg(QIK &leg,
                           float nX, float nY, float nZ,
                           const int axis[3], const float dir[3],
                           float offset[3], const char *name)
{
  float ang[3];
  ang[0] = leg.getTheta(0, nX, nY, nZ);
  ang[1] = leg.getTheta(1, nX, nY, nZ);
  ang[2] = leg.getTheta(2, nX, nY, nZ);

  Serial.print(F(">> Bridge ")); Serial.println(name);
  for (int j = 0; j < 3; j++) {
    int mi = mstateIndexFor(axis[j]);
    if (mi < 0 || !mstate[mi].standComplete) {
      Serial.print(F("   !! axis ")); Serial.print(axis[j]);
      Serial.println(F(" not in standing pose — keeping old offset"));
      continue;
    }
    float standingTurns = mstate[mi].standingTurns;
    float newOffset = ang[j] - (standingTurns / dir[j]) * 360.0f / 9.0f;
    Serial.print(F("   joint ")); Serial.print(jointName[j]);
    Serial.print(F(": ang=")); Serial.print(ang[j], 2);
    Serial.print(F(" standTurns=")); Serial.print(standingTurns, 4);
    Serial.print(F(" oldOff=")); Serial.print(offset[j], 3);
    Serial.print(F(" -> newOff=")); Serial.println(newOffset, 3);
    offset[j] = newOffset;
  }
}

void bridgeFromStandupToIK() {
  calibrateOffsetForLeg(legFL, normX_Front, normYF, normZ, axisFL, dirFL, offsetFL, "FL");
  calibrateOffsetForLeg(legBL, normX_Back,  normYB, normZ, axisBL, dirBL, offsetBL, "BL");
  calibrateOffsetForLeg(legFR, normX_Front, normYF, normZ, axisFR, dirFR, offsetFR, "FR");
  calibrateOffsetForLeg(legBR, normX_Back,  normYB, normZ, axisBR, dirBR, offsetBR, "BR");
}

// ============================================================
//  Gains
// ============================================================
void applyGainJoint(int j) {
  const int axes[4] = { axisFL[j], axisBL[j], axisFR[j], axisBR[j] };
  for (int k = 0; k < 4; k++) {
    odriveCAN.SetPositionGain(axes[k], posGain[j]);                          delay(10);
    odriveCAN.SetVelocityGains(axes[k], velGain[j], velIntegratorGain[j]);   delay(10);
  }
  Serial.print(F(">> ")); Serial.print(jointName[j]);
  Serial.print(F("  posGain="));    Serial.print(posGain[j],           3);
  Serial.print(F("  velGain="));    Serial.print(velGain[j],           4);
  Serial.print(F("  velIntGain=")); Serial.println(velIntegratorGain[j], 4);
}
void applyGainsAll() { for (int j = 0; j < 3; j++) applyGainJoint(j); }

void printGains() {
  Serial.println(F("  Joint   posGain   velGain   velIntGain"));
  for (int j = 0; j < 3; j++) {
    Serial.print(F("  ")); Serial.print(jointName[j]);
    Serial.print(F("    "));  Serial.print(posGain[j],           5);
    Serial.print(F("     ")); Serial.print(velGain[j],           5);
    Serial.print(F("     ")); Serial.println(velIntegratorGain[j], 5);
  }
}

// ============================================================
//  Send foot position
// ============================================================
void sendFootPos(QIK &leg,
                 const int axis[], const float offset[], const float dir[],
                 float currentTurns[],
                 float Xf, float Yf, float Zf)
{
  float abad = leg.getTheta(0, Xf, Yf, Zf);
  float hip  = leg.getTheta(1, Xf, Yf, Zf);
  float knee = leg.getTheta(2, Xf, Yf, Zf);
  if (!isIKValid(abad) || !isIKValid(hip) || !isIKValid(knee)) return;
  float turns[3];
  turns[0] = angleToTurns(0, abad, offset, dir);
  turns[1] = angleToTurns(1, hip,  offset, dir);
  turns[2] = angleToTurns(2, knee, offset, dir);
  for (int j = 0; j < 3; j++) {
    odriveCAN.SetPosition(axis[j], turns[j]);
    currentTurns[j] = turns[j];
  }
}
void sendFL(float X, float Y, float Z) { sendFootPos(legFL, axisFL, offsetFL, dirFL, currentTurnsFL, X, Y, Z); }
void sendBL(float X, float Y, float Z) { sendFootPos(legBL, axisBL, offsetBL, dirBL, currentTurnsBL, X, Y, Z); }
void sendFR(float X, float Y, float Z) { sendFootPos(legFR, axisFR, offsetFR, dirFR, currentTurnsFR, X, Y, Z); }
void sendBR(float X, float Y, float Z) { sendFootPos(legBR, axisBR, offsetBR, dirBR, currentTurnsBR, X, Y, Z); }
void sendAll(float XF, float YF, float XB, float YB, float Z) {
  sendFL(XF, YF, Z); sendFR(XF, YF, Z); sendBL(XB, YB, Z); sendBR(XB, YB, Z);
}

// ============================================================
//  Compute IK home turns for each leg
// ============================================================
bool computeHomeLeg(QIK &leg, float nX, float nY, float nZ,
                    const float offset[], const float dir[],
                    float homeTurns[3], const char *name)
{
  float abad = leg.getTheta(0, nX, nY, nZ);
  float hip  = leg.getTheta(1, nX, nY, nZ);
  float knee = leg.getTheta(2, nX, nY, nZ);
  if (!isIKValid(abad) || !isIKValid(hip) || !isIKValid(knee)) {
    Serial.print(F("!!! IK invalid for ")); Serial.println(name);
    return false;
  }
  homeTurns[0] = angleToTurns(0, abad, offset, dir);
  homeTurns[1] = angleToTurns(1, hip,  offset, dir);
  homeTurns[2] = angleToTurns(2, knee, offset, dir);
  Serial.print(F(">> ")); Serial.print(name); Serial.print(F(" HOME — abad="));
  Serial.print(homeTurns[0], 3); Serial.print(F("  hip=")); Serial.print(homeTurns[1], 3);
  Serial.print(F("  knee=")); Serial.println(homeTurns[2], 3);
  return true;
}
void recomputeAllHomes() {
  computeHomeLeg(legFL, normX_Front, normYF, normZ, offsetFL, dirFL, homeTurnsFL, "FL");
  computeHomeLeg(legBL, normX_Back,  normYB, normZ, offsetBL, dirBL, homeTurnsBL, "BL");
  computeHomeLeg(legFR, normX_Front, normYF, normZ, offsetFR, dirFR, homeTurnsFR, "FR");
  computeHomeLeg(legBR, normX_Back,  normYB, normZ, offsetBR, dirBR, homeTurnsBR, "BR");
}

// ============================================================
//  Read encoder positions (all 12)
// ============================================================
void readAllEncoderPositions() {
  const int allAxes[12] = {
    axisFL[0], axisFL[1], axisFL[2],
    axisBL[0], axisBL[1], axisBL[2],
    axisFR[0], axisFR[1], axisFR[2],
    axisBR[0], axisBR[1], axisBR[2]
  };
  float encoderPos[24]; bool received[24];
  for (int i = 0; i < 24; i++) { encoderPos[i] = 0.0f; received[i] = false; }
  for (int i = 0; i < 12; i++) { odriveCAN.GetPositionVelocity(allAxes[i]); delay(5); }
  unsigned long deadline = millis() + 200;
  while (millis() < deadline) {
    CAN_message_t inMsg;
    if (!odriveCAN.ReadMsg(inMsg)) continue;
    if ((inMsg.id & 0x1F) == 0x009) {
      uint8_t nodeId = (inMsg.id >> 5) & 0x3F;
      if (nodeId < 24) {
        EncoderEstimatesMsg_t est;
        odriveCAN.GetPositionVelocityResponse(est, inMsg);
        encoderPos[nodeId] = est.posEstimate;
        received[nodeId]   = true;
      }
    }
  }
  for (int j = 0; j < 3; j++) {
    currentTurnsFL[j] = received[axisFL[j]] ? encoderPos[axisFL[j]] : homeTurnsFL[j];
    currentTurnsBL[j] = received[axisBL[j]] ? encoderPos[axisBL[j]] : homeTurnsBL[j];
    currentTurnsFR[j] = received[axisFR[j]] ? encoderPos[axisFR[j]] : homeTurnsFR[j];
    currentTurnsBR[j] = received[axisBR[j]] ? encoderPos[axisBR[j]] : homeTurnsBR[j];
  }
}

// ============================================================
//  S-curve home move
// ============================================================
void safeHomeToAll() {
  float startFL[3], startBL[3], startFR[3], startBR[3];
  for (int j = 0; j < 3; j++) {
    startFL[j] = currentTurnsFL[j];
    startBL[j] = currentTurnsBL[j];
    startFR[j] = currentTurnsFR[j];
    startBR[j] = currentTurnsBR[j];
  }
  const int homeSteps = 600;
  const int homeDelay = 15;
  for (int step = 0; step <= homeSteps; step++) {
    float t = (float)step / (float)homeSteps;
    float ts = 0.5f * (1.0f - cosf(t * PI));
    for (int j = 0; j < 3; j++) {
      odriveCAN.SetPosition(axisFL[j], startFL[j] + (homeTurnsFL[j] - startFL[j]) * ts);
      odriveCAN.SetPosition(axisBL[j], startBL[j] + (homeTurnsBL[j] - startBL[j]) * ts);
      odriveCAN.SetPosition(axisFR[j], startFR[j] + (homeTurnsFR[j] - startFR[j]) * ts);
      odriveCAN.SetPosition(axisBR[j], startBR[j] + (homeTurnsBR[j] - startBR[j]) * ts);
    }
    delay(homeDelay);
    if (step % 60 == 0) Serial.print('.');
  }
  Serial.println();
  for (int j = 0; j < 3; j++) {
    currentTurnsFL[j] = homeTurnsFL[j];
    currentTurnsBL[j] = homeTurnsBL[j];
    currentTurnsFR[j] = homeTurnsFR[j];
    currentTurnsBR[j] = homeTurnsBR[j];
  }
}

// ============================================================
//  Closed-loop init in POSITION + POS_FILTER (program-1 mode)
// ============================================================
void initIKClosedLoop() {
  Serial.println(F(">> Entering POSITION control on all 12 axes..."));
  const int allAxes[12] = {
    axisFL[0], axisFL[1], axisFL[2],
    axisBL[0], axisBL[1], axisBL[2],
    axisFR[0], axisFR[1], axisFR[2],
    axisBR[0], axisBR[1], axisBR[2]
  };
  for (int i = 0; i < 12; i++) {
    odriveCAN.ClearErrors(allAxes[i]);              delay(60);
    odriveCAN.SetControllerModes(allAxes[i], 3, 3); delay(60); // POSITION + POS_FILTER
    odriveCAN.RunState(allAxes[i], 8);              delay(80);
  }
  applyGainsAll();
  robotState = STATE_CLOSED_LOOP;
}

// ============================================================
//  Heartbeat monitor
// ============================================================
bool isKnownAxis(uint8_t nodeId) {
  for (int j = 0; j < 3; j++)
    if (nodeId == axisFL[j] || nodeId == axisBL[j] ||
        nodeId == axisFR[j] || nodeId == axisBR[j]) return true;
  return false;
}
void checkHeartbeat() {
  CAN_message_t inMsg;
  while (odriveCAN.ReadMsg(inMsg)) {
    if ((inMsg.id & 0x1F) != 0x01) continue;
    HeartbeatMsg_t hb;
    odriveCAN.Heartbeat(hb, inMsg);
    if (hb.axisError != 0) {
      uint8_t nodeId = (inMsg.id >> 5) & 0x3F;
      if (!isKnownAxis(nodeId)) continue;
      robotState  = STATE_CLOSED_LOOP;
      gaitStep    = 0;
      gaitStepPos = 0;
      Serial.print(F("!!! ERROR on axis ")); Serial.print(nodeId);
      Serial.print(F(" : 0x")); Serial.println(hb.axisError, HEX);
      Serial.println(F(">> Press 'r' to re-arm, then 'e' to re-home."));
    }
  }
}

// ============================================================
//  Square-foot trot trajectory
// ============================================================
// Ground-press: extra mm pressed past normZ during stance so the
// planted diagonal actually pushes into the floor, lifting the
// chassis enough for the swinging pair to clear the ground.
// ============================================================
//  Square-foot trot trajectory — with chassis lift
// ============================================================
//
// Trajectory uses two key heights:
//   stanceZ = normZ + chassisLift   (supporting legs extended, body rises)
//   swingZ  = normZ + chassisLift - stepHeight   (foot clears ground)
//
// Ground contact happens at stanceZ (foot pushes body up against gravity).
// Swing height is stepHeight ABOVE the ground contact point.
//
const float chassisLift = 35.0f;   // body lifts 25mm during stance

void computeSquareTraj(int phase, int total, float baseY, float &outY, float &outZ) {
  int quarter = total / 4;
  float half  = stepLength * 0.5f;
  
  const float stanceZ = normZ + chassisLift;             // body-lifting Z
  const float swingZ  = normZ + chassisLift - stepHeight; // foot clearance Z

  if (phase < quarter) {
    // Phase 1: Lift-off — foot rises from stance Z up to swing Z
    float t = (float)phase / (float)quarter;
    outY = baseY - half;
    outZ = stanceZ - (stanceZ - swingZ) * t;  // smoothly interpolate
  }
  else if (phase < total / 2) {
    // Phase 2: Swing forward at peak height
    float t = (float)(phase - quarter) / (float)quarter;
    outY = (baseY - half) + stepLength * t;
    outZ = swingZ;
  }
  else if (phase < total / 2 + quarter) {
    // Phase 3: Touch-down — foot lowers back to stance Z
    float t = (float)(phase - total / 2) / (float)quarter;
    outY = baseY + half;
    outZ = swingZ + (stanceZ - swingZ) * t;
  }
  else {
    // Phase 4: Stance — foot at stance Z (body lifted, foot in ground contact)
    float t = (float)(phase - (total / 2 + quarter)) / (float)quarter;
    outY = (baseY + half) - stepLength * t;
    outZ = stanceZ;
  }
}
void tickGait() {
  if (robotState != STATE_WALKING) return;
  unsigned long now = millis();
  if (now - lastStepTime < (unsigned long)pointDelay) return;
  lastStepTime = now;
  int halfway = gaitTotal / 2;
  int phaseA = gaitStep % gaitTotal;
  int phaseB = (gaitStep + halfway) % gaitTotal;
  float Yf_A, Zf_A, Yf_B, Zf_B;
  computeSquareTraj(phaseA, gaitTotal, normYF, Yf_A, Zf_A);
  computeSquareTraj(phaseB, gaitTotal, normYF, Yf_B, Zf_B);
  float dA = Yf_A - normYF;
  float dB = Yf_B - normYF;
  sendFL(normX_Front, normYF + dA, Zf_A);
  sendBR(normX_Back,  normYB + dA, Zf_A);
  sendFR(normX_Front, normYF + dB, Zf_B);
  sendBL(normX_Back,  normYB + dB, Zf_B);
  gaitStep++;
  if (gaitStep >= gaitTotal) gaitStep = 0;
}

void tickGaitStep() {
  const int LAND_STEPS = 20;
  int total = resolution;
  int halfway = total / 2;
  for (int i = 0; i < total; i++) {
    int phaseA = gaitStepPos % total;
    int phaseB = (gaitStepPos + halfway) % total;
    float Yf_A, Zf_A, Yf_B, Zf_B;
    computeSquareTraj(phaseA, total, normYF, Yf_A, Zf_A);
    computeSquareTraj(phaseB, total, normYF, Yf_B, Zf_B);
    float dA = Yf_A - normYF, dB = Yf_B - normYF;
    sendFL(normX_Front, normYF + dA, Zf_A);
    sendBR(normX_Back,  normYB + dA, Zf_A);
    sendFR(normX_Front, normYF + dB, Zf_B);
    sendBL(normX_Back,  normYB + dB, Zf_B);
    delay(pointDelay);
    gaitStepPos++;
    if (gaitStepPos >= total) gaitStepPos = 0;
  }
  {
    int phaseA = gaitStepPos % total;
    int phaseB = (gaitStepPos + halfway) % total;
    float Yf_A, Zf_A_end, Yf_B, Zf_B_end;
    computeSquareTraj(phaseA, total, normYF, Yf_A, Zf_A_end);
    computeSquareTraj(phaseB, total, normYF, Yf_B, Zf_B_end);
    float dA = Yf_A - normYF, dB = Yf_B - normYF;
    for (int k = 0; k <= LAND_STEPS; k++) {
      float t = (float)k / (float)LAND_STEPS;
      float ts = 0.5f * (1.0f - cosf(t * PI));
      float ZA = Zf_A_end + (normZ - Zf_A_end) * ts;
      float ZB = Zf_B_end + (normZ - Zf_B_end) * ts;
      sendFL(normX_Front, normYF + dA, ZA);
      sendBR(normX_Back,  normYB + dA, ZA);
      sendFR(normX_Front, normYF + dB, ZB);
      sendBL(normX_Back,  normYB + dB, ZB);
      delay(pointDelay);
    }
  }
  sendAll(normX_Front, normYF, normX_Back, normYB, normZ);
  gaitStepPos = 0;
}

// ============================================================
//  Discrete diagonal trot step
// ============================================================
void stepDir(char dir) {
  float offY   = (dir == 'F') ?  (float)stepOffsetMm
               : (dir == 'B') ? -(float)stepOffsetMm : 0.0f;
  float offX   = (dir == 'R') ?  (float)stepOffsetMm
               : (dir == 'L') ? -(float)stepOffsetMm : 0.0f;
  float offX_L = (dir == 'C') ? -(float)stepOffsetMm : offX;
  float offX_R = (dir == 'C') ?  (float)stepOffsetMm : offX;
  float liftZ  = normZ - (float)stepLiftMm;
  int   t      = stepTimeMs;

  sendFL(normX_Front,          normYF,        liftZ);
  sendBR(normX_Back,           normYB,        liftZ);
  sendFR(normX_Front,          normYF,        normZ);
  sendBL(normX_Back,           normYB,        normZ);
  delay(t);
  sendFL(normX_Front + offX_L, normYF + offY, liftZ);
  sendBR(normX_Back  + offX_R, normYB + offY, liftZ);
  sendFR(normX_Front,          normYF,        normZ);
  sendBL(normX_Back,           normYB,        normZ);
  delay(t);
  sendFL(normX_Front + offX_L, normYF + offY, normZ);
  sendBR(normX_Back  + offX_R, normYB + offY, normZ);
  sendFR(normX_Front,          normYF,        normZ);
  sendBL(normX_Back,           normYB,        normZ);
  delay(t);
  sendFL(normX_Front + offX_L, normYF + offY, normZ);
  sendBR(normX_Back  + offX_R, normYB + offY, normZ);
  sendFR(normX_Front,          normYF,        liftZ);
  sendBL(normX_Back,           normYB,        liftZ);
  delay(t);
  sendFL(normX_Front + offX_L, normYF + offY, normZ);
  sendBR(normX_Back  + offX_R, normYB + offY, normZ);
  sendFR(normX_Front + offX_R, normYF + offY, liftZ);
  sendBL(normX_Back  + offX_L, normYB + offY, liftZ);
  delay(t);
  sendFL(normX_Front + offX_L, normYF + offY, normZ);
  sendBR(normX_Back  + offX_R, normYB + offY, normZ);
  sendFR(normX_Front + offX_R, normYF + offY, normZ);
  sendBL(normX_Back  + offX_L, normYB + offY, normZ);
  delay(t);
}

void inPlaceTrot() {
  int t = 95;
  sendFL(normX_Front, normYF, normZ - 80);
  sendBR(normX_Back,  normYB, normZ - 80);
  sendFR(normX_Front, normYF, normZ);
  sendBL(normX_Back,  normYB, normZ);
  delay(t);
  sendAll(normX_Front, normYF, normX_Back, normYB, normZ);
  delay(t);
  sendFL(normX_Front, normYF, normZ);
  sendBR(normX_Back,  normYB, normZ);
  sendFR(normX_Front, normYF, normZ - 80);
  sendBL(normX_Back,  normYB, normZ - 80);
  delay(t);
  sendAll(normX_Front, normYF, normX_Back, normYB, normZ);
  delay(t);
}

// ============================================================
//  CRAWL GAIT — Statically stable, one leg at a time
//
//  Leg ordering (canonical crawl sequence):
//    FR -> BL -> FL -> BR
//
//  Each leg's cycle:
//    1. Shift body weight away from lifting leg (CoM into support triangle)
//    2. Lift, swing forward, plant the leg
//    3. Pause briefly (let load transfer to new foot)
//    4. Next leg (or end cycle if last)
//
//  All foot positions are kept relative to a "stance offset" that
//  shifts to keep CoM over the support triangle of the 3 grounded legs.
// ============================================================

// Per-leg crawl identifier: 0=FL, 1=BL, 2=FR, 3=BR
enum CrawlLeg { CR_FL = 0, CR_BL = 1, CR_FR = 2, CR_BR = 3 };

// Helper: send all 4 legs at an offset stance with one leg's foot at custom position
void sendCrawlStance(int liftLegIdx,
                     float liftX, float liftY, float liftZ,
                     float bodyShiftX, float bodyShiftY)
{
  // Compute each foot's target. The "lifting" leg uses its custom position.
  // The three "planted" legs are offset by -bodyShiftX/Y, which physically
  // shifts the body in the +bodyShiftX/Y direction relative to the ground.
  
  // FL (front-left)
  if (liftLegIdx == CR_FL) {
    sendFL(liftX, liftY, liftZ);
  } else {
    sendFL(normX_Front - bodyShiftX, normYF - bodyShiftY, normZ);
  }
  
  // BL (back-left)
  if (liftLegIdx == CR_BL) {
    sendBL(liftX, liftY, liftZ);
  } else {
    sendBL(normX_Back - bodyShiftX, normYB - bodyShiftY, normZ);
  }
  
  // FR (front-right)
  if (liftLegIdx == CR_FR) {
    sendFR(liftX, liftY, liftZ);
  } else {
    sendFR(normX_Front - bodyShiftX, normYF - bodyShiftY, normZ);
  }
  
  // BR (back-right)
  if (liftLegIdx == CR_BR) {
    sendBR(liftX, liftY, liftZ);
  } else {
    sendBR(normX_Back - bodyShiftX, normYB - bodyShiftY, normZ);
  }
}

// Determine body shift direction needed to lift a given leg.
// Returns shift X and Y that moves CoM away from the leg.
void getShiftForLeg(int legIdx, float &shiftX, float &shiftY) {
  // Shift toward the diagonal opposite leg's quadrant
  switch (legIdx) {
    case CR_FL:  // lifting front-left -> shift back-right
      shiftX = +crawlWeightShift;
      shiftY = -crawlWeightShift;
...

This file has been truncated, please download it to see its full contents.

ROS WORKSPACEE

Credits

Annu Johns
1 project • 2 followers
ECE student building real-world IoT and embedded solutions, bridging hardware with scalable and reliable software systems.
Achinth P M
0 projects • 3 followers
Sapnil M
1 project • 8 followers
Iot enthusiast and always willing and ready to work on new and exciting projects
Abhinav Krishna
9 projects • 57 followers
Maker | IoT Enthusiast | Electronics hobbyist

Comments

Related channels and tags
  • Autodesk
  • Intel
  • Microsoft
  • MQTT
  • Node-RED
  • NVIDIA
  • Robotics
  • Robotics
  • ROS
  • Seeed Studio