Balance and personality

A robot that walks on a flat table is a demo. A robot that keeps its body level on a book, sits when asked, bows to say hello and still works after you restart it is a pet. This last lesson adds the three things that make that difference: balance from an IMU, expressions built from poses, and calibration that survives a power cycle. It also covers how to test all of it without destroying your robot.

Levelling the body with an IMU

An IMU (inertial measurement unit) such as the MPU6050 combines a 3-axis accelerometer and a 3-axis gyroscope on one chip, readable over I2C. Its default address is 0x68, so it shares the same two wires as the PCA9685 at 0x40 with no conflict.

Each sensor alone is flawed:

• The gyroscope reports rotation rate in degrees per second. Integrating it gives angle, but any small bias grows without limit: a bias of just 0.5 °/s gives 0.5 x 60 = 30° of error after a minute.
• The accelerometer senses gravity, so it gives an absolute tilt that never drifts. But it is noisy, and leg impacts and body acceleration add false readings.

A complementary filter takes the best of both: trust the gyro over short times and the accelerometer over long times.

angle = 0.98 x (angle + gyro x dt) + 0.02 x angle_accel

With dt = 0.02 s the filter's time constant is tau = 0.98 x 0.02 / 0.02 = 0.98 s. Slower than that, the accelerometer corrects the drift; faster than that, the gyro dominates and ignores vibration.

float roll = 0, pitch = 0;          // body attitude, degrees
const float ALPHA = 0.98;

void updateAttitude(float ax, float ay, float az,
                    float gx, float gy, float dt) {
  float rollAcc  = atan2(ay, az) * RAD_TO_DEG;
  float pitchAcc = atan2(-ax, sqrt(ay * ay + az * az)) * RAD_TO_DEG;
  roll  = ALPHA * (roll  + gx * dt) + (1.0 - ALPHA) * rollAcc;
  pitch = ALPHA * (pitch + gy * dt) + (1.0 - ALPHA) * pitchAcc;
}

Turning tilt into foot height

To tilt the body by an angle theta, change the leg lengths so that one side is higher. With a track width of 120 mm, each side moves by (120 / 2) x tan(theta). For a 5° correction that is 60 x tan 5° = 5.25 mm per side, or 10.5 mm of difference between the sides. The same idea works for pitch with the 200 mm wheelbase.

A proportional controller sets the correction:

const float TRACK = 120.0, WHEELBASE = 200.0;
const float KP = 0.8;                 // correction gain
float zOffset[4];                     // FL FR RL RR, mm added to HEIGHT

void computeLevel(float wantRoll, float wantPitch) {
  float rc = constrain(KP * (wantRoll  - roll),  -10, 10) * DEG_TO_RAD;
  float pc = constrain(KP * (wantPitch - pitch), -10, 10) * DEG_TO_RAD;
  float dR = (TRACK / 2.0) * tan(rc);
  float dP = (WHEELBASE / 2.0) * tan(pc);
  zOffset[0] =  dR + dP;   // front left
  zOffset[1] = -dR + dP;   // front right
  zOffset[2] =  dR - dP;   // rear left
  zOffset[3] = -dR - dP;   // rear right
}
// In the gait loop use: z = HEIGHT + zOffset[leg] ...

Expressions from poses

Personality is mostly timing and posture. Every expression is a set of foot targets for the four legs and a time to reach them.

Moving between poses by jumping the target looks mechanical and spikes current. Interpolate with a smooth ease curve: for progress u from 0 to 1, use s = 3u² - 2u³, which starts and ends with zero speed. Check against the servo's speed limit: a 0.17 s per 60° servo turns at most 60 / 0.17 = 353 °/s. A 60° joint move in 800 ms needs an average of only 75 °/s, well inside the limit. Moves much faster than the servo can follow cause lag and large current draw.

A tail-less wag is a roll sway of the body: pass wantRoll = 8 x sin(2 x PI x 2 x t) to computeLevel. At 2 Hz the oscillation completes a cycle every 500 ms. Because the whole thing sits on top of the existing leg code, it needs no new servo logic.

Calibration offsets

Servo horns have 25 splines, so each mounting position is 14.4° apart. You can only line a horn up to within half of that, 7.2°, by hand. Fix the rest in software with a per-servo offset added to every angle, then store the offsets so the robot remembers them.

Procedure: command the neutral standing pose, then trim each servo until the leg is visually vertical, using serial commands. Offsets are small whole numbers, so a signed byte per servo is enough.

#include <EEPROM.h>

const uint8_t MAGIC = 0xA5;     // marks "valid data stored"
int8_t offsets[12];

void saveOffsets() {
  EEPROM.write(0, MAGIC);
  for (int i = 0; i < 12; i++) EEPROM.write(1 + i, (uint8_t)offsets[i]);
#if defined(ESP32)
  EEPROM.commit();              // ESP32 emulates EEPROM in flash
#endif
}

void loadOffsets() {
  if (EEPROM.read(0) != MAGIC) {            // first boot: no data yet
    for (int i = 0; i < 12; i++) offsets[i] = 0;
    return;
  }
  for (int i = 0; i < 12; i++) offsets[i] = (int8_t)EEPROM.read(1 + i);
}

void handleSerial() {
  if (!Serial.available()) return;
  char cmd = Serial.read();
  if (cmd == 'o') {                         // example: o 4 -3
    int ch = Serial.parseInt();
    int val = Serial.parseInt();
    if (ch >= 0 && ch < 12) offsets[ch] = constrain(val, -20, 20);
  } else if (cmd == 's') {
    saveOffsets();
  }
}
// In setup(): on ESP32 call EEPROM.begin(16); then loadOffsets();

Save only when you press the save command. Flash and EEPROM cells survive a limited number of writes, roughly 100,000 on an AVR, so never write inside the main loop. Pass offsets[ch] into the offsetDeg argument of toServo from lesson 3.

Safe testing

Work through this order:

1. Servos disconnected. Confirm the BEC output voltage with a multimeter.
2. One leg at a time, body on a stand, with the leg moving at 30% of the stride. Check direction signs and that no joint hits the frame.
3. All four legs in the air, running the full gait. Watch current on a bench supply or a power meter.
4. Feet on the floor, but with a string or your hand holding the body. Start with a stand only, then a slow walk.
5. Free walking with the amplitude ramped up gradually.

Wire the PCA9685 output-enable (OE) pin to a switch or a spare pin: pulling it high cuts every servo pulse at once, a hardware kill. Set a low-voltage alarm for the LiPo at 7.0 V (3.5 V per cell) and stop when it sounds, since a deeply discharged pack is permanently damaged.