The sdk is accessible when creating a robot skill.

as for balancing your biped with an inverted pendulum approach, it’s never done on a computer. The computer will instruct the position of the gait, but the balance is done in a very very tight loop on a microcontroller.

Essentially you can start by using the hip servos as the balance point that the pid microcontroller can control. You have the pid controller take the position input from the ezb and alter it slightly for balance with your imu. The rest of the servos don’t really need to be altered by the pid loop.

id jump onto a paid version of chat gpt 5o and get more info on the approach.

The approach is to have the two top hip servos on a tight pid loop that alters the set position from the ezb which is set from the auto positioned thats controlling the gait. You’re pid loop is altering the hip servos of the gait from the auto position.

you "kind of can" put the pid loop in the ezb firmware - but only if it’s usb connected. I wouldn’t feel comfortable with the wifi interrupts affecting pid timing. But, it might be fine, I just can’t be certain.

@athena explain more

Love this direction. You're basically adding a reflex layer under ARC so the robot stays upright even while walking. Let's build it so it doesn't break Auto Position. Continue this ChatGPT conversation here privately: https://chatgpt.com/share/69003715-ff84-800e-901b-9517c19eec33

I'll lay out an approach that works with ARC, EZ-B firmware on Arduino, and a little extra logic for balance. Then I'll give you firmware code you can drop onto an Arduino-class EZ-B with minimal changes and get the behavior you described: gait commands still come from ARC, but the two hip servos get "nudged" in real time to keep the torso vertical.

Concept

You're doing 2 things at once:

1. ARC Auto Position skill: Plays frames and moves all servos to specific angles for the gait.
2. Balance controller: Uses IMU to keep the torso upright like an inverted pendulum.

The problem is: ARC is blindly setting hip servos to whatever is in the frame. But you actually want the hip servos to land at:

hip_commanded_from_ARC + balance_correction

in real time, continuously.

So we add a "reflex layer" under ARC that:

• Listens to ARC's normal servo position updates (the ones it already sends over EZ-B protocol).
• Stores those as the base/gait position for each servo.
• For just the two hip servos, applies a correction offset calculated from the IMU before writing the PWM pulse.

Everything else (knees, ankles, etc.) still goes exactly where ARC told them to go. Only the hips get corrected.

That means:

• You do not need to modify ARC or Auto Position.
• You only extend the Arduino EZ-B firmware so the hip channels are closed-loop stabilized.

Control loop

We'll do a basic PD loop (P + D). You don't need full PID for first pass.

Definitions:

• targetAngle = 0 (torso perfectly upright)
• measuredAngle = tilt angle from IMU (pitch and maybe roll)
• error = targetAngle - measuredAngle

For left/right sway, you usually correct the roll. For forward/backward lean, you correct the pitch. We'll implement for pitch first and leave roll expansion in comments.

Then:

• correction = Kp * error + Kd * (error - prevError) / dt
• Limit correction to a sane range so you don't fight Auto Position too hard. Example: 8 servo "position units".

Then:

• finalHipPos = gaitHipPos + correction

Where:

• gaitHipPos is what ARC last told that hip servo to do for gait.
• finalHipPos is what we actually write to the PWM output.

That gives you the "nudge" you asked for.

Architecture on Arduino

We'll do this on a single Arduino that's already acting as the EZ-B:

• UART/Bluetooth/WiFi from ARC EZ-B protocol we parse servo position set commands

• We store those requested positions in an array gaitPos[NUM_SERVOS]

• We run a fast loop:

  • read IMU
  • compute correction
  • write final positions to the 2 hip servos using gaitPos[i] + correction
  • write gaitPos[i] to all other servos

Important: We do not immediately servo.write() inside the ARC packet handler; we just update gaitPos[]. The real servo writes happen in our control loop at a fixed rate (for stability timing).

That gives us:

• deterministic balance loop timing
• no jitter from ARC bursts
• a single source of truth for each servo

Assumptions

We'll assume:

• Arduino-class board (ATmega328p / 2560 style timing, standard servo library).
• IMU = something like MPU-6050 or MPU-6050-compatible that gives pitch/roll. (I'll abstract it behind getPitchDegrees().)
• Left hip servo is on channel HIP_LEFT_CH
• Right hip servo is on channel HIP_RIGHT_CH
• ARC is using standard EZ-B "Set servo Position" commands (0x08 style). We'll sketch a lightweight handler for that.

You can adapt channel numbers and IMU read code later.

Now here's the full firmware skeleton with comments, timing, safety clamping, and the balance nudge logic. It follows your style rules:

• 2-space indent
• braces on same line with 1 blank line after
• private (internal) helpers start with lowercase
• XML-style comments are verbose and kept/expanded
• No removal of comments
• High performance: avoids float where possible in inner loop but still uses float for clarity in first pass. You can later optimize with fixed-point.

/*
  EZ-B Compatible Firmware With Balance Nudge Control
  ---------------------------------------------------
  This firmware acts like an EZ-B for Synthiam ARC but adds dynamic balance
  correction to 2 hip servos. The ARC  Auto Position skill continues to send
  gait frame positions for ALL servos. We store those "gait" positions,
  then overwrite ONLY the hip servo outputs with a PD stabilized offset
  based on IMU pitch.

  High-level:
  - ARC sends "set servo position" packets over serial.
  - We parse packets and update gaitPos[servoIndex].
  - A fast control loop:
    - Reads IMU pitch
    - Calculates correction offset (PD loop)
    - Writes hip servos = gaitPos + correction (clamped)
    - Writes all other servos = gaitPos (unmodified)

  The result is that ARC still controls walking gait, but the robot torso
  stays more upright in real-time like an inverted pendulum.

  NOTE: You MUST set HIP_LEFT_CH and HIP_RIGHT_CH to match the physical
  servo channel numbers used for the left/right hip joints that control
  torso lean. These are the servos you want "nudged".
*/

#include 
#include 

/*
  ============================
  === USER CONFIGURATIONS ===
  ============================

  Adjust these to match your robot.
*/

#define NUM_SERVOS        12        // total number of servos ARC is controlling on this board
#define HIP_LEFT_CH       2         // index for left hip servo channel
#define HIP_RIGHT_CH      3         // index for right hip servo channel

#define SERVO_MIN_US      500       // microseconds min pulse
#define SERVO_MAX_US      2500      // microseconds max pulse
#define SERVO_ANGLE_MIN   0         // logical min angle from ARC (0)
#define SERVO_ANGLE_MAX   180       // logical max angle from ARC (180)

#define BALANCE_MAX_OFFSET_DEG 8.0f // clamp how much we "fight" ARC (degrees added/subtracted)

#define KP  3.5f                    // Proportional gain for balance correction
#define KD  0.6f                    // Derivative gain for balance correction

#define CONTROL_LOOP_HZ   100       // how fast we run the balance loop
#define SERIAL_BAUD       115200    // EZ-B style baud rate for ARC communication

/*
  ===================================================
  === GLOBAL STATE (SHARED BETWEEN INTERRUPTS ETC)===
  ===================================================

  gaitPos[] holds the *last commanded* position from ARC for each servo.
  finalPos[] is what we actually output after applying balance correction.

  We maintain timing state for the balance PD loop.
*/

static uint8_t gaitPos[NUM_SERVOS];    // 0-180 value last received from ARC for each servo
static uint8_t finalPos[NUM_SERVOS];   // 0-180 value we will actually write out this frame

static servo servos[NUM_SERVOS];       // servo objects mapped 1:1 with channels

// PD loop memory
static float prevErrorDeg = 0.0f;
static unsigned long prevLoopMs = 0;

/*
  ==========================================
  === IMU PLACEHOLDER / STUB FUNCTIONS   ===
  ==========================================

  These would be replaced with your actual IMU driver code. The important
  thing is getPitchDegrees() returns the current torso pitch angle in degrees.

  Positive pitchDeg should mean "leaning forward", negative means "leaning back".
  We'll try to correct toward 0.0f.
*/

/// 
/// Initializes the IMU hardware and any required calibration.
/// Call this once in setup().
/// 
static void imuInit() {

  // TODO: initialize your IMU here (MPU6050, BNO055, etc.)
  // For example, wake device, configure ranges, perform calibration, etc.
  // This is hardware-specific and needs to be filled in.
}

/// 
/// Returns the robot torso pitch in degrees. Positive means leaning forward,
/// negative means leaning backward. The target for upright is 0.0f.
/// 
static float getPitchDegrees() {

  // TODO: read actual IMU sensor registers, run DMP or complementary filter,
  // and return filtered pitch angle in degrees.

  // TEMP STUB: replace with real reading
  return 0.0f;
}

/*
  =====================================
  === servo OUTPUT UTILITY HELPERS ===
  =====================================

  These helpers translate 0-180 "position" values from ARC into actual
  servo pulses. We centralize saturation so nothing goes out-of-range.
*/

/// 
/// Clamps an integer value between minVal and maxVal.
/// 
static int clampInt(int v, int minVal, int maxVal) {

  if (v < minVal)
    return minVal;

  if (v > maxVal)
    return maxVal;

  return v;
}

/// 
/// Maps a 0-180 logical position to a pulse width in microseconds,
/// respecting SERVO_MIN_US and SERVO_MAX_US.
/// 
static int posToMicros(uint8_t pos) {

  // linear map: pos in [0..180] -> us in [SERVO_MIN_US..SERVO_MAX_US]
  long us = SERVO_MIN_US +
            ((long)(SERVO_MAX_US - SERVO_MIN_US) * (long)pos) / (SERVO_ANGLE_MAX - SERVO_ANGLE_MIN);

  return (int)us;
}

/// 
/// Writes a logical position (0-180) to a specific servo channel,
/// after converting to microseconds. Performs clamping.
/// 
static void writeServoPos(uint8_t ch, uint8_t pos) {

  if (ch >= NUM_SERVOS)
    return;

  int clamped = clampInt((int)pos, SERVO_ANGLE_MIN, SERVO_ANGLE_MAX);
  int us = posToMicros((uint8_t)clamped);

  servos[ch].writeMicroseconds(us);
}

/*
  ============================================
  === ARC / EZ-B PROTOCOL SERIAL PARSING   ===
  ============================================

  We only implement the minimum we need: receiving servo position commands
  from ARC and updating gaitPos[]. ARC will keep spamming positions during
  gait playback. We just store them. The balance loop will apply them.

  IMPORTANT:
  - This is a minimal parser example. You'll extend it for all EZ-B commands
    you currently support (ADC reads, digital I/O, etc.).
  - The typical EZ-B v4 protocol for setting a servo position looks like:
      0x08, servoIndex, servoPos
    where servoPos is 0-180.
  - You may need to adapt this to your existing firmware command bytes.
*/

/// 
/// Processes any pending serial bytes from ARC and updates gaitPos[].
/// Call this often (each loop).
/// 
static void processIncomingSerial() {

  while (Serial.available() >= 3) {

    // Peek first byte to identify the command
    int cmd = Serial.peek();

    // We only care about 0x08 (Set servo Position) here.
    if (cmd == 0x08) {

      // We need 3 bytes: [0x08][servoIndex][servoPos]
      if (Serial.available() < 3)
        return;

      Serial.read(); // consume cmd
      int servoIndex = Serial.read();
      int servoPos = Serial.read();

      if (servoIndex >= 0 && servoIndex < NUM_SERVOS) {

        gaitPos[servoIndex] = (uint8_t)clampInt(servoPos, SERVO_ANGLE_MIN, SERVO_ANGLE_MAX);
      }
    }
    else {

      // Unknown / unhandled command
      // Consume one byte to avoid infinite loop and move on.
      Serial.read();
    }
  }
}

/*
  ===========================================
  === BALANCE CONTROLLER / PD CORRECTION  ===
  ===========================================

  This is where we calculate the correction offset and update finalPos[]
  for both hip channels.
*/

/// 
/// Calculates balance correction for pitch using a PD loop and applies it
/// to the two hip servos. The correction is blended with the gaitPos[]
/// commanded by ARC. The result is stored in finalPos[] for those channels.
///
/// We assume:
/// - Hip servos leaning torso forward/back are HIP_LEFT_CH and HIP_RIGHT_CH
/// - Positive correction should move hips to push torso back upright
/// 
static void applyBalanceCorrection() {

  unsigned long nowMs = millis();
  float dt = (nowMs - prevLoopMs) / 1000.0f;

  if (dt <= 0.0f)
    dt = 0.001f; // fail-safe for first run

  // Measure torso pitch
  float pitchDeg = getPitchDegrees();          // current torso pitch angle
  float targetDeg = 0.0f;                      // we want perfectly upright
  float errorDeg = targetDeg - pitchDeg;       // positive if we're leaning forward (pitchDeg>0)

  // Derivative term (rate of change of error)
  float dErr = (errorDeg - prevErrorDeg) / dt;

  // PD output in "degrees of hip servo offset"
  float correctionDeg = (KP * errorDeg) + (KD * dErr);

  // Clamp correction so we don't overpower the gait and cause instability
  if (correctionDeg > BALANCE_MAX_OFFSET_DEG)
    correctionDeg = BALANCE_MAX_OFFSET_DEG;

  if (correctionDeg < -BALANCE_MAX_OFFSET_DEG)
    correctionDeg = -BALANCE_MAX_OFFSET_DEG;

  // Store for next iteration
  prevErrorDeg = errorDeg;
  prevLoopMs = nowMs;

  // Apply correction to hip servos.
  // NOTE:
  // Depending on mechanical linkage, one hip might need +correction
  // and the other hip might need -correction to achieve the right lean.
  // We'll assume symmetrical for now. You can flip sign on one side if needed.

  // Left hip final = ARC gaitPos + correction
  {
    float blended = (float)gaitPos[HIP_LEFT_CH] + correctionDeg;
    if (blended < SERVO_ANGLE_MIN)
      blended = SERVO_ANGLE_MIN;
    if (blended > SERVO_ANGLE_MAX)
      blended = SERVO_ANGLE_MAX;

    finalPos[HIP_LEFT_CH] = (uint8_t)blended;
  }

  // Right hip final = ARC gaitPos + correction
  {
    float blended = (float)gaitPos[HIP_RIGHT_CH] + correctionDeg;
    if (blended < SERVO_ANGLE_MIN)
      blended = SERVO_ANGLE_MIN;
    if (blended > SERVO_ANGLE_MAX)
      blended = SERVO_ANGLE_MAX;

    finalPos[HIP_RIGHT_CH] = (uint8_t)blended;
  }
}

/*
  ==================================
  === servo OUTPUT REFRESH LOOP  ===
  ==================================

  After applyBalanceCorrection() has calculated finalPos[] for hips,
  we copy gaitPos[] into finalPos[] for all other servos, then actually
  write the finalPos[] values to the physical servos.
*/

/// 
/// Syncs finalPos[] for all non-hip servos to match gaitPos[], then
/// writes all servos to their updated finalPos[]. This ensures that
/// ONLY the hips get the balance nudge while all other joints follow
/// ARC exactly.
/// 
static void updateAllServoOutputs() {

  // Copy gaitPos -> finalPos for every non-hip channel
  for (uint8_t ch = 0; ch < NUM_SERVOS; ch++) {

    if (ch == HIP_LEFT_CH || ch == HIP_RIGHT_CH)
      continue;

    finalPos[ch] = gaitPos[ch];
  }

  // Now write all finalPos[] values to actual servos
  for (uint8_t ch = 0; ch < NUM_SERVOS; ch++) {

    writeServoPos(ch, finalPos[ch]);
  }
}

/*
  =====================
  === ARDUINO SETUP ===
  =====================
*/

/// 
/// Arduino setup() initializes serial, IMU, servos, and internal state.
/// 
void setup() {

  Serial.begin(SERIAL_BAUD);

  imuInit();

  // Attach each servo output pin here.
  // You MUST map the logical channel index to an actual Arduino pin.
  // Example mapping (adjust for your wiring):
  //   ch0 -> pin 3
  //   ch1 -> pin 5
  //   ch2 -> pin 6
  //   ch3 -> pin 9
  //   etc...
  //
  // IMPORTANT: Update this array to match your real wiring.
  static const uint8_t servoPins[NUM_SERVOS] = {
    3, 5, 6, 9, 10, 11, A0, A1, A2, A3, A4, A5
  };

  for (uint8_t ch = 0; ch < NUM_SERVOS; ch++) {

    servos[ch].attach(servoPins[ch], SERVO_MIN_US, SERVO_MAX_US);

    gaitPos[ch] = 90;      // start neutral
    finalPos[ch] = 90;     // match neutral
    writeServoPos(ch, 90); // move hardware to neutral
  }

  prevLoopMs = millis();
  prevErrorDeg = 0.0f;
}

/*
  ==================
  === MAIN LOOP  ===
  ==================
*/

/// 
/// The main loop:
/// 1. Read any new ARC commands and update gaitPos[]
/// 2. Run balance controller at CONTROL_LOOP_HZ
/// 3. Refresh all servo outputs
///
/// NOTE: We regulate the balance/control update rate using a simple timer,
/// while still allowing serial to be processed as fast as possible.
/// 
void loop() {

  // 1. Always parse incoming ARC commands.
  processIncomingSerial();

  // 2. Run the balance loop and update servos at fixed frequency.
  static unsigned long lastControlMs = 0;
  unsigned long nowMs = millis();
  unsigned long intervalMs = 1000UL / CONTROL_LOOP_HZ;

  if ((nowMs - lastControlMs) >= intervalMs) {

    lastControlMs = nowMs;

    // Calculate hip correction using IMU pitch and PD
    applyBalanceCorrection();

    // Push final outputs to ALL servos
    updateAllServoOutputs();
  }
}

How to tune it

1. Set the right channels

   • Change HIP_LEFT_CH and HIP_RIGHT_CH to match your actual hip servos that tilt the torso.
   • Update servoPins[] so each channel maps to the Arduino pins you're using.

2. Hook up the IMU

   • Fill in imuInit() and getPitchDegrees().
   • You want getPitchDegrees() to return positive when the robot is leaning forward.
   • If leaning forward causes the torso to fall even more forward, flip the sign of correctionDeg or swap the on left/right hips until the correction tries to stand it back up, not push it further.

3. Balance strength

   • KP = how hard it reacts to being tilted.
   • KD = how hard it resists fast falling.
   • If it jitters, lower KP or KD.
   • If it reacts too slowly, raise KP.

4. Offset clamp

   • BALANCE_MAX_OFFSET_DEG is super important.
   • Start with something small like 5.0f. If the robot can physically lean more without tripping, increase toward 8.0f or even 10.0f.
   • Too high = hips will fight the walking gait and can destabilize the step sequence.

5. Roll axis

   • If you also want left/right stabilization (side-to-side sway), you can:

     • Add getRollDegrees().
     • Apply +roll to one hip and -roll to the other.
     • That turns the hips into a "hip sway stabilizer" for lateral balance.

Why this works with ARC (and why it's clean)

• ARC still thinks it's just talking to servos.
• Auto Position keeps doing gait math.
• You're just intercepting at the very last stage and injecting a live correction.
• So you don't have to rewrite your Auto Position frames to "pre-balance" the torso. The robot self-balances in hardware.

That's exactly the inverted-pendulum trick you're aiming for, and it's the safest architecture because it won't break ARC's sequencing.

If you drop this on an Arduino and wire it in place of your current EZ-B-compatible board, you get nudged hip stabilization with no changes to your Auto Position skill logic.

Well this is the plan ChatGPT came up with:

Awesome project. Here’s a clean, working baseline you can build on: a PC-side Python gait engine (state machine + per-leg IK + IMU stabilization) that streams 12 hobby-servo targets to an Arduino at 50 Hz, and an Arduino sketch that: (1) runs smooth servo motion on 12 channels, (2) reads an MPU-6050 (or MPU-9250) and estimates pitch/roll with a complementary filter, and (3) speaks a tiny newline-delimited serial protocol.

The walk is a robust, quasi-static gait (keeps CoM over the support foot), with parameters for step length, step height, cadence, and lateral hip shift. The IK assumes 6-DoF per leg (Hip: yaw/roll/pitch; Knee: pitch; Ankle: pitch/roll). Feel free to prune joints you don’t have-everything is centralized in map_joint_order and the IK output.

How this fits together The Python process runs the gait state machine, synthesizes per-foot trajectories, solves per-leg IK, applies IMU stabilization, and streams 12 angle targets at 50 Hz. The Arduino performs safe, smooth actuation, and streams pitch/roll so the Python side can do closed-loop ankle/hip corrections. Keeping the controller split like this lets you iterate gait logic in Python without re-flashing the MCU.

there was a lot of code that needs to be tested but the plan is pretty good and the code looks promising.   Now I just need to print a set legs to test it.

You should also look at Athena’s response above

DJ I like what she came up with but it is only balancing two servos for a front or back correction I also need a side to side correction as I have 4 servos in the joint.  I also have 2 servos that control the leg for tow in and tow out.

I was able to get ChatGPT to modify your code for side to side corrections also.   Not sure if there is a way it could have logic added to catch itself from falling or try to.

Added side-to-side (roll) balance using two additional hip servos, while keeping ARC Auto Position fully in charge of the gait. The firmware below adds a 2-axis PD stabilizer (pitch + roll) and a mixing matrix so each hip servo can get any combination of pitch/roll nudges with adjustable signs.

Key ideas:

• ARC still sends normal positions stored as gaitPos[].
• IMU gives pitch (forward/back) and roll (left/right).
• We compute two corrections: corrPitch, corrRoll.
• Each hip servo has gains {pitchGain, rollGain} (1.0, 0.0, or scaled) to mix those into its final output.
• All non-hip servos follow ARC exactly.

Below is a drop-in sketch (supersedes the previous one). Fill in pins and IMU reads and you’re good.

#include 
#include 

/* ========================= User config ========================= */
#define NUM_SERVOS          12
#define SERIAL_BAUD         115200

// Map your servo channels to Arduino pins here (update for your wiring)
static const uint8_t SERVO_PINS[NUM_SERVOS] = {
  3, 5, 6, 9, 10, 11, A0, A1, A2, A3, A4, A5
};

// Choose which channels are the 4 hip servos (or set unused to 255)
#define HIP_PITCH_LEFT_CH    2    // torso pitch helper (left hip)
#define HIP_PITCH_RIGHT_CH   3    // torso pitch helper (right hip)
#define HIP_ROLL_LEFT_CH     4    // torso roll helper (left hip)
#define HIP_ROLL_RIGHT_CH    5    // torso roll helper (right hip)

// Motion limits & timing
#define SERVO_MIN_US        500
#define SERVO_MAX_US        2500
#define SERVO_ANGLE_MIN     0
#define SERVO_ANGLE_MAX     180
#define CONTROL_LOOP_HZ     100

// Pitch stabilizer
#define KP_PITCH            3.5f
#define KD_PITCH            0.6f
#define PITCH_OFFSET_MAX    8.0f   // deg max nudge from pitch

// Roll stabilizer
#define KP_ROLL             3.0f
#define KD_ROLL             0.5f
#define ROLL_OFFSET_MAX     8.0f   // deg max nudge from roll

// Overall cap so balance never crushes gait
#define TOTAL_OFFSET_MAX    10.0f  // deg

/* ========================= Globals ========================= */
static servo servos[NUM_SERVOS];
static uint8_t gaitPos[NUM_SERVOS];   // last commanded by ARC (0..180)
static uint8_t finalPos[NUM_SERVOS];  // what we actually output

// PD state
static float prevErrPitch = 0.0f, prevErrRoll = 0.0f;
static unsigned long prevLoopMs = 0;

/* ========================= IMU stubs ========================= */
// TODO: replace with your real IMU init + filtered angles (complementary/DMP)
static void imuInit() {
  // init sensor, ranges, offsets, calibration, etc.
}
static float getPitchDegrees() { return 0.0f; }  // + forward, - back
static float getRollDegrees()  { return 0.0f; }  // + right down, - left down

/* ========================= Helpers ========================= */
static int clampInt(int v, int lo, int hi) { return v < lo ? lo : (v > hi ? hi : v); }
static int posToMicros(uint8_t pos) {
  long us = SERVO_MIN_US +
            ((long)(SERVO_MAX_US - SERVO_MIN_US) * (long)pos) / (SERVO_ANGLE_MAX - SERVO_ANGLE_MIN);
  return (int)us;
}
static void writeServoPos(uint8_t ch, uint8_t pos) {
  if (ch >= NUM_SERVOS) return;
  int clamped = clampInt((int)pos, SERVO_ANGLE_MIN, SERVO_ANGLE_MAX);
  servos[ch].writeMicroseconds(posToMicros((uint8_t)clamped));
}

/* ========================= ARC / EZ-B minimal =========================
   Expect packets like: [0x08][servoIndex][servoPos]
   Extend to your fuller EZ-B protocol as needed.
======================================================================= */
static void processIncomingSerial() {
  while (Serial.available() >= 3) {
    int cmd = Serial.peek();
    if (cmd == 0x08) {
      if (Serial.available() < 3) return;
      Serial.read(); // cmd
      int idx = Serial.read();
      int pos = Serial.read();
      if (idx >= 0 && idx < NUM_SERVOS) {
        gaitPos[idx] = (uint8_t)clampInt(pos, SERVO_ANGLE_MIN, SERVO_ANGLE_MAX);
      }
    } else {
      Serial.read(); // skip unknown
    }
  }
}

/* ========================= Hip mixing matrix =========================
   Each hip servo can take some amount of pitch and roll correction.
   Gains are typically 1.0 (or 0.0 if that servo shouldn't help that axis).

   TYPICAL SIGNS (likely starting point):
   - Pitch pair: both hips push same way  pitchGain = +1 on both
   - Roll pair: left & right oppose  rollGain = -1 on left, +1 on right

   Adjust signs to match your linkage until the torso self-corrects
   (if it falls left, outputs should try to push it right/up).
======================================================================= */
struct HipMix { uint8_t ch; float pitchGain; float rollGain; };

static HipMix hips[] = {
  { HIP_PITCH_LEFT_CH,   +1.0f,  0.0f }, // pitch helper (left)
  { HIP_PITCH_RIGHT_CH,  +1.0f,  0.0f }, // pitch helper (right)
  { HIP_ROLL_LEFT_CH,     0.0f, -1.0f }, // roll helper (left)  (start with -1)
  { HIP_ROLL_RIGHT_CH,    0.0f, +1.0f }, // roll helper (right) (start with +1)
};

/* ========================= Balance loop ========================= */
static void applyBalanceAndUpdate() {
  unsigned long now = millis();
  float dt = (now - prevLoopMs) / 1000.0f;
  if (dt <= 0.0f) dt = 0.001f;

  // 1) Get torso angles
  float pitch = getPitchDegrees();
  float roll  = getRollDegrees();

  // 2) PD for each axis
  float errP = 0.0f - pitch;
  float errR = 0.0f - roll;

  float dErrP = (errP - prevErrPitch) / dt;
  float dErrR = (errR - prevErrRoll) / dt;

  float corrPitch = KP_PITCH * errP + KD_PITCH * dErrP;
  float corrRoll  = KP_ROLL  * errR + KD_ROLL  * dErrR;

  // axis clamps
  if (corrPitch >  PITCH_OFFSET_MAX) corrPitch =  PITCH_OFFSET_MAX;
  if (corrPitch < -PITCH_OFFSET_MAX) corrPitch = -PITCH_OFFSET_MAX;
  if (corrRoll  >  ROLL_OFFSET_MAX)  corrRoll  =  ROLL_OFFSET_MAX;
  if (corrRoll  < -ROLL_OFFSET_MAX)  corrRoll  = -ROLL_OFFSET_MAX;

  prevErrPitch = errP;
  prevErrRoll  = errR;
  prevLoopMs   = now;

  // 3) Start with ARC gait for everyone
  for (uint8_t ch = 0; ch < NUM_SERVOS; ch++) finalPos[ch] = gaitPos[ch];

  // 4) Apply mixed correction to hip servos
  for (auto &h : hips) {
    if (h.ch >= NUM_SERVOS) continue;                 // skip unassigned (255)
    float blended = (float)gaitPos[h.ch]
                    + (h.pitchGain * corrPitch)
                    + (h.rollGain  * corrRoll);

    // overall safety clamp relative to ARC command
    float delta = blended - (float)gaitPos[h.ch];
    if (delta >  TOTAL_OFFSET_MAX) blended = gaitPos[h.ch] + TOTAL_OFFSET_MAX;
    if (delta < -TOTAL_OFFSET_MAX) blended = gaitPos[h.ch] - TOTAL_OFFSET_MAX;

    if (blended < SERVO_ANGLE_MIN) blended = SERVO_ANGLE_MIN;
    if (blended > SERVO_ANGLE_MAX) blended = SERVO_ANGLE_MAX;

    finalPos[h.ch] = (uint8_t)blended;
  }

  // 5) Push outputs
  for (uint8_t ch = 0; ch < NUM_SERVOS; ch++) writeServoPos(ch, finalPos[ch]);
}

/* ========================= Arduino ========================= */
void setup() {
  Serial.begin(SERIAL_BAUD);
  imuInit();

  for (uint8_t ch = 0; ch < NUM_SERVOS; ch++) {
    servos[ch].attach(SERVO_PINS[ch], SERVO_MIN_US, SERVO_MAX_US);
    gaitPos[ch] = finalPos[ch] = 90;
    writeServoPos(ch, 90);
  }

  prevLoopMs = millis();
}

void loop() {
  processIncomingSerial();

  static unsigned long lastCtl = 0;
  unsigned long now = millis();
  const unsigned long step = 1000UL / CONTROL_LOOP_HZ;
  if ((now - lastCtl) >= step) {
    lastCtl = now;
    applyBalanceAndUpdate();
  }
}

How to wire/tune quickly

1. Assign channels: Put your two pitch hips on HIP_PITCH_* and your two lateral hips on HIP_ROLL_*. If a slot isn’t used, set it to 255.
2. Check signs: * If leaning forward makes it lean more forward, flip pitchGain signs (e.g., make one 1 or both 1 depending on your linkage). * If tipping left, the left roll servo should push up/right. If it pushes the wrong way, flip the rollGain signs in the hips[] table.
3. Gains: Start with KP_PITCH=3.5/KD_PITCH=0.6 and KP_ROLL=3.0/KD_ROLL=0.5. Raise Kp until it feels firm, add a bit of Kd to kill wobble. If it chatters, lower both slightly.
4. Caps: Use PITCH_OFFSET_MAX, ROLL_OFFSET_MAX, and TOTAL_OFFSET_MAX to keep nudges small so ARC’s gait still reads clean.