Phase 1 — Walk. Deep dive.

Install the development environment, create an empty firmware project, and upload the microcontroller equivalent of 'hello world' — a blinking LED plus a serial print. If this works, every other task is just more code on top.

~80% of 'the code doesn't upload' problems come from a broken toolchain, bad USB cable, missing driver, or wrong COM port — NOT the code. You need a baseline that proves the chain works so when real bugs appear later, you know to look at the code, not the setup.

• VS Code installed (https://code.visualstudio.com).
• An ESP32 DevKit V1 board (classic ESP32, not S3 — Bluepad32 needs BT Classic).
• A DATA-capable USB cable (charge-only cables cause silent upload failures).
• Mac / Windows / Linux — PlatformIO runs on all three.

1. STEP 1
   Install the PlatformIO extension in VS Code

   Open VS Code → Extensions panel (⇧⌘X) → search 'PlatformIO IDE' → Install. First install downloads the Python-based core (~200 MB) and can take 3–10 min. Let it finish; you'll see a 'PlatformIO: Home' tab when done.

   EXPECT: The PlatformIO icon (ant head) appears in the VS Code activity bar (left strip).
2. STEP 2
   Install the CP2102 / CH340 USB-serial driver (macOS / Windows)

   Most DevKit V1 boards use the Silicon Labs CP2102. On macOS you need the VCP driver from Silicon Labs. On Windows, plug the board in — if it shows as a yellow-bang in Device Manager, install the CP2102 or CH340 driver matching the onboard chip. Linux recognises both natively.

   EXPECT: A new /dev/cu.SLAB_USBtoUART (macOS) or COMn (Windows) appears when you plug the board in.
3. STEP 3
   Create a new PlatformIO project

   PIO Home → New Project. Name: hexa-firmware. Board: 'Espressif ESP32 Dev Module'. Framework: Arduino. Location: anywhere outside OneDrive/iCloud sync folders (those break uploads).

   EXPECT: VS Code opens a folder with platformio.ini, src/main.cpp, and a lib/ folder.
4. STEP 4
   Edit platformio.ini

   Replace the generated file with the config from docs/04_SOFTWARE_PLAN.md. Key settings: monitor_speed = 115200, upload_speed = 921600, and the lib_deps block that pulls in the Adafruit PCA9685 driver.

   [env:esp32dev]
   platform = espressif32
   board = esp32doit-devkit-v1
   framework = arduino
   monitor_speed = 115200
   upload_speed = 921600
   lib_deps =
     adafruit/Adafruit PWM Servo Driver Library@^3.0.0

   EXPECT: PlatformIO will re-index and silently download the library on first build.
5. STEP 5
   Write a minimal main.cpp

   Blink GPIO 2 (the blue onboard LED) and print a banner to Serial.

   #include <Arduino.h>
   
   void setup() {
     Serial.begin(115200);
     pinMode(2, OUTPUT);
     delay(200);
     Serial.println("=== HEXA FIRMWARE BOOT ===");
   }
   
   void loop() {
     digitalWrite(2, HIGH);
     delay(500);
     digitalWrite(2, LOW);
     delay(500);
     Serial.print(".");
   }

   EXPECT: Compiles cleanly.
6. STEP 6
   Build, upload, open the serial monitor

   Click the ✓ (Build) icon in the PlatformIO toolbar. Then → (Upload). When you see 'Hard resetting via RTS pin…', click the plug icon (Serial Monitor). Some DevKit V1s need you to HOLD the BOOT button briefly while upload starts.

   EXPECT: Blue LED blinks at 1 Hz. Serial monitor shows 'HEXA FIRMWARE BOOT' followed by dots.

Get the two PCA9685 servo driver boards talking to the ESP32 over I²C, confirm both show up at addresses 0x40 and 0x41, and make servo #0 physically move to its mechanical centre.

The PCA9685 is the bridge between your code and every servo. If I²C doesn't scan clean or the PWM frequency is wrong, nothing downstream works. Getting one servo to centre proves the entire signal chain (code → I²C → driver → PWM → servo).

• T1 complete (board boots, serial works).
• One MG996R servo with horn OFF (don't install horns until after calibration).
• Both PCA9685 boards soldered to their headers (or bought pre-assembled).
• Buck converter set to ~5.5 V (measure with multimeter!) OR a bench PSU at 5.5 V, 1 A limit.
• DANGER: never power PCA9685 V+ from the ESP32's 5 V pin. One servo under load will brown out the board.

1. STEP 1
   Bridge the A0 solder-jumper on the SECOND PCA9685 only

   Each PCA9685 has six address-select pads (A0..A5). Default = 0x40. Bridge A0 on driver #2 with a small solder blob → it becomes 0x41. Leave the first one alone. Bridge a wrong one and you'll clash with the IMU (0x68) or the camera's EEPROM.

   EXPECT: Visibly shiny solder over the A0 pad on driver #2. None of the A1..A5 pads touched.
2. STEP 2
   Wire the I²C daisy-chain

   ESP32 GPIO 21 → SDA of driver #1 → SDA of driver #2. ESP32 GPIO 22 → SCL of driver #1 → SCL of driver #2. ESP32 3V3 → VCC of both drivers (logic power — low current). ESP32 GND → GND of both drivers. Keep these wires short: <20 cm each.

   EXPECT: Both drivers' green power LED lights when ESP32 is powered.
3. STEP 3
   Wire servo power — SEPARATELY

   Buck converter + output → V+ pin on driver #1 → V+ pin on driver #2. Buck converter − output → GND of driver #1 → GND of driver #2 → ESP32 GND (all grounds MUST be common). Put a 470 µF–1000 µF electrolytic cap across V+/GND near driver #1.

   EXPECT: With no servos plugged in, V+ reads 5.3–5.6 V at both drivers.
4. STEP 4
   Plug ONE servo into channel 0 of driver #1

   Signal = orange (or yellow), +V = red, GND = brown (or black). Double-check the pin order on your driver silkscreen — some have GND on the outside, some inside.

   EXPECT: Servo doesn't move yet. No buzz.
5. STEP 5
   Run an I²C scan on boot

   Add this to setup() after Serial.begin:

   #include <Wire.h>
   void i2cScan() {
     Wire.begin(21, 22);
     Wire.setClock(400000);
     Serial.println("I2C scan:");
     for (uint8_t a = 1; a < 127; a++) {
       Wire.beginTransmission(a);
       if (Wire.endTransmission() == 0) Serial.printf("  0x%02X found\n", a);
     }
   }

   EXPECT: Boot-time serial log: '0x40 found' and '0x41 found'.
6. STEP 6
   Initialise at 50 Hz, set servo 0 to centre

   Use the Adafruit PCA9685 library. 50 Hz is the MG996R's native frame rate. Centre = ~1500 µs pulse = tick count 307 at 50 Hz / 12-bit.

   #include <Adafruit_PWMServoDriver.h>
   
   Adafruit_PWMServoDriver drv0(0x40);
   uint16_t usToTicks(uint16_t us) {
     return (uint16_t)((us * 4096UL) / 20000UL);
   }
   
   void setup() {
     Serial.begin(115200);
     Wire.begin(21, 22);
     drv0.begin();
     drv0.setPWMFreq(50);
     drv0.setPWM(0, 0, usToTicks(1500)); // centre
   }

   EXPECT: Servo flicks to centre position within one frame (<20 ms) and holds there.
7. STEP 7
   Wrap it behind a setAngle API

   Deg 0..180 maps to ~500..2500 µs on a typical analog servo. Clamp at the ends to avoid gear strain.

   void setAngle(uint8_t ch, uint8_t deg) {
     deg = constrain(deg, 0, 180);
     uint16_t us = map(deg, 0, 180, 500, 2500);
     if (ch < 16) drv0.setPWM(ch, 0, usToTicks(us));
     else drv1.setPWM(ch - 16, 0, usToTicks(us));
   }

   EXPECT: setAngle(0, 0) → hard-left. setAngle(0, 180) → hard-right. setAngle(0, 90) → centre.

Write the math that turns 'put the foot at this XYZ point' into 'coxa=this angle, femur=that, tibia=that'. Then prove it with a unit test that runs ON YOUR LAPTOP (not on the robot), using PlatformIO's native test environment.

IK is the brain of the whole walker. Get it right once, the rest of the robot is just sequencing. Test it NATIVELY — if you only discover a sign error in IK by watching a leg punch the floor, you've already stripped a gear.

• T2 complete.
• Measured link lengths (coxa, femur, tibia) with calipers. For Antdroid defaults: coxa ≈ 28 mm, femur ≈ 83 mm, tibia ≈ 121 mm — but MEASURE yours.
• Paper + pencil (you will draw the triangle).
• A willingness to re-read high-school trigonometry (atan2, law of cosines).

1. STEP 1
   Draw the leg on paper

   Top view: coxa rotates around the vertical axis; its angle = atan2(y, x). Side view: the femur and tibia form a triangle — given foot radius r and depth z, solve the triangle with the law of cosines. Write the diagram + label link lengths L1, L2, L3 before you touch code.

2. STEP 2
   Port the analytical IK from rasheeddo

   The rasheeddo/hexapod_dev_esp32 repo has a clean closed-form IK you can literally copy. Adjust signs if your body frame defines +Z as UP vs. DOWN.

   struct Angles { float coxa, femur, tibia; bool reachable; };
   
   Angles solveIK(float x, float y, float z,
                  float L1, float L2, float L3) {
     Angles a{};
     a.coxa = atan2f(y, x);
     float r = sqrtf(x*x + y*y) - L1;
     float d = sqrtf(r*r + z*z);
     if (d > (L2 + L3) || d < fabsf(L2 - L3)) {
       a.reachable = false;
       return a;
     }
     float alpha = atan2f(z, r);
     float beta  = acosf((L2*L2 + d*d - L3*L3) / (2*L2*d));
     a.femur = alpha + beta;
     a.tibia = acosf((L2*L2 + L3*L3 - d*d) / (2*L2*L3));
     a.reachable = true;
     return a;
   }

3. STEP 3
   Add the matching FK

   FK is the other direction: plug angles in, get (x, y, z) out. Good for sanity testing (IK then FK should round-trip).

4. STEP 4
   Set up a native (desktop) test env in platformio.ini

   Running unit tests on your laptop is ~100× faster than flashing to hardware for every fix.

   [env:native]
   platform = native
   test_framework = unity
   build_flags = -std=gnu++17
   test_build_src = yes

5. STEP 5
   Write the round-trip test

   Pick 10 varied points within the reachable envelope. Run IK → FK. Expect recovery within 0.1 mm.

   void test_ik_fk_roundtrip() {
     const float pts[][3] = {{150, 0, -100}, {100, 80, -120},
                             {50, -50, -80}};
     for (auto& p : pts) {
       auto a = solveIK(p[0], p[1], p[2], L1, L2, L3);
       TEST_ASSERT_TRUE(a.reachable);
       float x, y, z; solveFK(a, x, y, z);
       TEST_ASSERT_FLOAT_WITHIN(0.1, p[0], x);
       TEST_ASSERT_FLOAT_WITHIN(0.1, p[1], y);
       TEST_ASSERT_FLOAT_WITHIN(0.1, p[2], z);
     }
   }

6. STEP 6
   Run the test

   Terminal in VS Code: pio test -e native. You get a pass/fail summary in under 2 seconds.

   EXPECT: 3 tests pass. If one fails with 'Expected 150 got -150', you have a sign error — probably in atan2 arg order or the z convention.
7. STEP 7
   Check reachability explicitly

   Commanding an unreachable point should return reachable=false, not NaN angles. NaNs will propagate and do wild things at PWM time.

Add a serial command that forces every servo to neutral 90° and lets you adjust per-servo trim + direction flags until the robot looks mechanically straight. Save those values to ESP32 flash so they survive reboots.

No matter how careful you are, servo horns can ONLY install in 6° or 15° increments (25-tooth vs 24-tooth splines). So mechanical neutral ≠ 90° electrical. Trim is how you reconcile this. Without calibration, IK gives correct ANGLES but the leg physically isn't where the code thinks it is — and the walker will drift or fall.

• T2 complete (servo bus works).
• Robot assembled and MOUNTED ON A STAND. Feet must not touch the floor.
• One person at a keyboard, one hand on the EN (reset) button.
• Every servo labelled (leg 0..5, joint coxa/femur/tibia) before calibration.

1. STEP 1
   Implement the CAL serial protocol

   On receiving 'CAL\n': every servo goes to 90° − trim, with direction applied. While in CAL mode, accept commands: 'T <idx> <deg>' to adjust trim, 'D <idx>' to flip direction, 'SAVE' to write NVS, 'Q' to exit.

   // Pseudo-protocol:
   // CAL         → enter calibration hold
   // T 3 -5      → servo 3 trim = −5°
   // D 3         → flip servo 3 direction
   // SAVE        → commit trims + dirs to NVS
   // Q           → exit CAL; resume normal control

2. STEP 2
   Wire trim + direction into setAngle

   Inside setAngle(), apply direction first, then trim. Clamp the final value to [0, 180] to avoid mechanical stall.

   int8_t SERVO_DIR[18] = { /* +1 or −1 */ };
   float  SERVO_TRIM[18] = { /* −15.0 .. +15.0 deg */ };
   
   void setAngle(uint8_t ch, float deg) {
     float d = 90.0f + SERVO_DIR[ch] * (deg - 90.0f) + SERVO_TRIM[ch];
     d = constrain(d, 0.0f, 180.0f);
     /* ... PWM write ... */
   }

3. STEP 3
   Persist trim + dir to NVS using Preferences

   ESP32's Preferences library wraps NVS in a simple key/value API.

   #include <Preferences.h>
   Preferences prefs;
   void saveCal() {
     prefs.begin("cal", false);
     prefs.putBytes("trim", SERVO_TRIM, sizeof(SERVO_TRIM));
     prefs.putBytes("dir",  SERVO_DIR,  sizeof(SERVO_DIR));
     prefs.end();
   }
   void loadCal() {
     prefs.begin("cal", true);
     prefs.getBytes("trim", SERVO_TRIM, sizeof(SERVO_TRIM));
     prefs.getBytes("dir",  SERVO_DIR,  sizeof(SERVO_DIR));
     prefs.end();
   }

4. STEP 4
   Walk through every leg, joint by joint

   For each leg (0..5), for each joint (coxa, femur, tibia): command 90°. If it moves to the wrong side, issue 'D'. Then adjust 'T' by ±1° until the joint is mechanically straight. Coxa parallel to ground, femur straight out horizontal, tibia pointing straight down. Print each value as you go — that's your backup.

5. STEP 5
   SAVE and power-cycle

   Issue SAVE. Power the board off, then on. Re-enter CAL. Verify every joint comes up in the same position. If it drifts, loadCal() isn't being called in setup().

   EXPECT: All 18 joints hold their calibrated pose from cold boot.
6. STEP 6
   Export a backup as JSON

   Print the calibration as JSON on serial with a 'DUMP' command. Paste that into a git-committed calibration_backup.json file. If the NVS ever wipes (flash erase, bad partition), you have the values.

Combine IK (T3) + servo bus (T2) + calibration (T4) into a tripod gait. On a stand, all 6 feet should trace smooth arcs. Tripod A (legs 0, 2, 4) and Tripod B (legs 1, 3, 5) should alternate cleanly.

This is the 'it walks' moment. Every prior task has been plumbing. Tripod is the simplest stable gait because a triangle is always balanced — other gaits are harder because they require inertia or body shifting.

• T3 (IK) tests passing.
• T4 calibration saved.
• Robot on stand, feet hanging free.
• Fire extinguisher / LiPo bag nearby (we're now running servos continuously).

1. STEP 1
   Parametrise the gait

   Expose: cycleMs (full swing+stance duration), strideMm (step length), swingHeightMm (arc height), bodyHeightMm (nominal z), duty (fraction of cycle spent in stance). Start conservative: cycleMs=600, stride=40, swingHeight=25, bodyHeight=90, duty=0.5.

2. STEP 2
   Generate one leg's trajectory

   Swing phase: foot traces a cycloid from (x0−stride/2, −bodyHeight) up over (x0, −bodyHeight + swingHeight) to (x0+stride/2, −bodyHeight). Stance: foot slides straight back from (x0+stride/2, −bodyHeight) to (x0−stride/2, −bodyHeight). Feed each (x, y, z) into solveIK() → setAngle().

   // Normalised phase t in [0,1) within cycle.
   // Stance for t < duty; swing for t >= duty.
   // Cycloidal swing (smooth start/end):
   //   s = (t - duty) / (1 - duty);
   //   x = -stride/2 + stride * s;
   //   z = -bodyH + swingH * sin(PI * s);

3. STEP 3
   Phase-offset the two tripods

   Tripod A (legs 0, 2, 4) uses phase t. Tripod B (legs 1, 3, 5) uses phase (t + 0.5) mod 1. That means whenever A is swinging, B is in stance, and vice versa — the body is always on three feet.

4. STEP 4
   Handle 'walk in place'

   When commanded stride = 0, swing height stays but x delta is zero. Feet lift and drop in place — a great visual test on the stand.

5. STEP 5
   Run on the stand first

   Issue 'WALK 0.0' (stride 0). Watch all 6 feet. Each should lift exactly once per cycle with clean arcs. Tripod A and B should alternate visibly. If any foot doesn't lift at all, its femur direction or trim is wrong.

6. STEP 6
   Timer task, not loop()

   Run the gait update in a FreeRTOS task or esp_timer at a fixed 50 Hz. Don't rely on loop() — it's whatever-your-code-allows Hz and will jitter.

   // In setup():
   // esp_timer_create_args_t tArgs = {.callback=&gaitTick, ...};
   // esp_timer_start_periodic(timer, 20000); // 20 ms = 50 Hz

Monitor the LiPo voltage through a simple resistor divider into an ADC pin, and halt all servo motion when voltage drops below 6.6 V (3.3 V per cell). Resume above 7.0 V. Beep a buzzer + blink an LED on low voltage.

Discharging a LiPo below 3.0 V/cell permanently damages it — a single deep-discharge event and the pack is scrap (R600+ lost). Below 2.5 V/cell LiPos can puff, vent, or catch fire. Battery monitor is the single most-important safety feature after the fuse.

• T1 complete.
• 2S LiPo charged (nominal 7.4 V, full 8.4 V).
• Two 10 kΩ ±1% resistors (one 10k/10k divider → ADC sees ≤ ½ × battery voltage, safely under 3.3 V).
• Soldering iron, heat-shrink.
• A buzzer from the BOM OR the low-voltage alarm as backup.

1. STEP 1
   Build a 10k / 10k voltage divider

   From LiPo+ → 10 kΩ → ADC pin (GPIO 34) AND from same node → 10 kΩ → GND. At 8.4 V battery, ADC sees 4.2 V — TOO HIGH for ESP32. Use 20k/10k instead, OR 10k/4.7k — something that keeps ADC input < 3.0 V at 8.4 V input. Calculate: ADC_V = V_bat × (R2 / (R1 + R2)).

   EXPECT: At 8.4 V input, ADC input ≤ 3.0 V. At 6.0 V input, ADC input ≤ 2.2 V.
2. STEP 2
   Verify with multimeter BEFORE connecting to ESP32

   Connect the divider to the LiPo (inside fireproof bag, fuse in circuit). Measure the divider midpoint. Must match your calculation ± 0.1 V. Only after this passes, connect to GPIO 34.

3. STEP 3
   Read ADC and convert back to battery voltage

   ESP32 ADC is 12-bit (0..4095) → 0..3.3 V reference. Scale by the divider ratio to recover V_battery.

   const float DIV_RATIO = (10000.0f + 10000.0f) / 10000.0f; // for 10k/10k
   // or use DIV_RATIO = (20000 + 10000) / 10000 = 3.0 for a 20k/10k
   float readBattery() {
     int raw = analogRead(34);
     float v_adc = (raw / 4095.0f) * 3.3f;
     return v_adc * DIV_RATIO;
   }

   EXPECT: readBattery() returns ±0.2 V of the multimeter reading.
4. STEP 4
   Apply moving average (ADC is noisy)

   A 16-sample moving average over ~160 ms smooths the ADC enough to make hysteresis reliable.

   float filteredV() {
     static float buf[16] = {0};
     static int i = 0;
     buf[i] = readBattery();
     i = (i + 1) & 15;
     float s = 0; for (float x : buf) s += x;
     return s / 16;
   }

5. STEP 5
   Implement hysteresis cutoff

   Two thresholds, not one. Halt at ≤ 6.6 V. Only re-arm above ≥ 7.0 V. State is sticky — you need a deliberate recovery.

   enum BatteryState { OK, LOW };
   BatteryState bs = OK;
   bool gaitEnabled() {
     float v = filteredV();
     if (bs == OK  && v <= 6.60f) bs = LOW;
     if (bs == LOW && v >= 7.00f) bs = OK;
     return bs == OK;
   }

6. STEP 6
   Feedback: buzzer + GPIO 2 LED

   On LOW: pulse GPIO 2 fast (100 ms on / 100 ms off), drive a buzzer pin HIGH with a 1 kHz square wave. Keep the standalone LVC buzzer ALSO plugged in — redundancy is cheap.

7. STEP 7
   Test with a bench PSU (not the LiPo!)

   Sweep the PSU from 8.0 V down to 6.0 V while watching the serial log. Motion halts at 6.6 V. Sweep back up — doesn't resume until 7.0 V. Beep + blink align with the halt.

   EXPECT: No resume chatter near the threshold. Serial logs state transitions.

Extend the tripod gait state machine to also support ripple (one leg at a time, 6 × 1/6-phase) and wave (one leg at a time, slower, more stable). Let the operator switch between them via serial command or later, via the dashboard.

Tripod is fast but commits all weight to a triangle — unstable on uneven ground. Wave gait is slower but keeps 5/6 feet planted at all times — useful for carrying payloads or climbing. Shipping all three is low-effort and impressive in demos.

• T5 complete (tripod working).
• Understanding of gait phase tables (see docs 06).

1. STEP 1
   Generalise phase to a 6-entry table

   Each leg has a phase offset in [0, 1). Tripod: [0, 0.5, 0, 0.5, 0, 0.5]. Ripple: [0, 1/6, 2/6, 3/6, 4/6, 5/6] — but in the insect-inspired order 0, 3, 1, 4, 2, 5 (opposite leg pairs alternate). Wave: [0, 1/6, 2/6, 3/6, 4/6, 5/6] in sequential leg order but with duty = 5/6 (each leg swings 1/6 of cycle, planted 5/6).

2. STEP 2
   Make duty (stance fraction) a per-gait parameter

   Tripod: duty = 0.5. Ripple: duty = 2/3. Wave: duty = 5/6. Everything else in the gait function stays the same — just a different phase table + duty.

3. STEP 3
   Add a serial 'GAIT <name>' command

   'GAIT TRIPOD' / 'GAIT RIPPLE' / 'GAIT WAVE'. Switching mid-walk should finish the current cycle before applying the new table (cleaner transition).

4. STEP 4
   Latency: don't reset phase on switch

   If you reset phase to 0 on switch, all legs snap to a new pose. Let phase keep advancing; the new phase table takes effect on the next cycle.

5. STEP 5
   Stability sanity check

   For each gait, at every moment in the cycle, the centre of mass projection should fall inside the support polygon formed by the planted feet. For tripod that's a triangle (most fragile). For wave it's always a pentagon (most stable).