Firmware Setup & Code Guide

You can program the ESP32-S3 microcontroller using the standard Arduino IDE.

Learning embedded robotics requires understanding the fundamentals rather than treating your code as an opaque black box. This guide gives you the foundational architecture and modular building blocks needed to write and understand your robotics code from the ground up:

1. Autonomous Architecture & Motion Design: How a mobile robot structures acceleration, state transitions, step generation, and closed-loop feedback.
2. Modular Building Blocks: Verified C++ code snippets for each individual hardware subsystem (steppers, encoders, gyro, power monitoring, safety buttons) that you can adapt and assemble into your own program.

1. Arduino IDE Setup

1. Board Manager: Install the official esp32 board package by Espressif (Tools > Board > Boards Manager). ESP32 core version 3.3.x is recommended.
2. Select Board: Choose ESP32S3 Dev Module.
3. USB Cable: Plug your USB-C cable into the UART port (typically the left port) on the ESP32-S3 DevKit for code uploading and Serial Monitor output.
4. Recommended Settings:
   • USB CDC On Boot: Enabled
   • Flash Size: 8MB (or matching your specific DevKit flash)
5. Libraries: If your DevKit has an addressable RGB status LED, you can install Adafruit NeoPixel from Sketch > Include Library > Manage Libraries. Standard libraries (Wire, Preferences, BLEDevice) ship directly with the ESP32 core.

2. Pin Definitions

Here are the onboard pin assignments for the EVDrive PCB:

// Left Motor (Motor A)
const int PIN_M1_DIR  = 4;
const int PIN_M1_STEP = 5;
const int PIN_M1_EN   = 6;  // LOW = ON, HIGH = OFF

// Right Motor (Motor B)
const int PIN_M2_DIR  = 46;
const int PIN_M2_STEP = 9;
const int PIN_M2_EN   = 14; // LOW = ON, HIGH = OFF

// Sensors (I2C)
const int PIN_I2C1_SDA = 1;  // MPU-6050 & Encoder A (J1 - Right Wheel)
const int PIN_I2C1_SCL = 2;
const int PIN_I2C2_SDA = 41; // Encoder B (J2 - Left Wheel)
const int PIN_I2C2_SCL = 42;

// Buttons, Switches & Battery Sense
const int PIN_START_BTN = 7;  // Start Button SW5 (Active LOW)
const int PIN_WL_SW     = 15; // Wireless Switch SW4 (LOW when ON)
const int PIN_VOLT_ADC  = 13; // Battery voltage divider (11:1)
const int PIN_WL_LED    = 16; // Wireless Status LED D1
const int PIN_USER_LED  = 40; // General Status LED D5

Notes:

• The Reset button (SW6) is wired directly to the ESP32 CHIP_PU (EN) pin. Pressing it reboots the chip in hardware; it is not readable as a software GPIO.
• If your ESP32-S3 DevKit includes an onboard addressable RGB LED (WS2812), it is typically wired to GPIO 48. Because GPIO 48 is also broken out on expansion header J6, keep this in mind if you connect external sensors to pin 48.
• Distance Calibration: Do not rely on theoretical wheel math. Tire compression under chassis weight alters effective wheel diameter. Always calibrate your wheel rollout empirically as described in Calibration & Tuning.

3. Autonomous Motion Architecture & State Machine Design

An Electric Vehicle sketch must execute the required two-stage run: accelerate straight forward from the Start Line, push the non-optional water bottle past the Bottle Line, pause, reverse toward the Target Point, and stop with the rear Measurement Point directly over the mark, all with zero remote control during the run.

State Machine

A finite state machine is the cleanest way to structure your loop:

IDLE -> FORWARD -> PAUSE -> REVERSE -> DONE

• IDLE: Motors are disarmed or holding at the start line. Waiting for a start button press (SW5).
• FORWARD: Accelerate forward, track straight using gyro/encoder feedback, and advance until the bottle is pushed past the line.
• PAUSE: Halts forward motion for a settling delay (e.g. 300 ms to 500 ms) while chassis inertia dissipates.
• REVERSE: Inverts motor direction pins, accelerates backward toward the Target Point, and decelerates to a stop.
• DONE: Motors hold position until retrieved. An unsharpened pencil press or manual disarm command frees the wheels.

Step Generation & Timing

To ensure smooth motor motion:

• Hardware Timers or Interval Scheduling: Instead of relying on blocking delayMicroseconds() calls that stutter when background tasks run, higher-performance sketches utilize ESP32 hardware timers or non-blocking micros intervals to schedule STEP pulses.
• Independent Motor Control: Generating step pulses independently for the left and right motors allows you to dynamically adjust individual wheel velocities for steering corrections.

Speed Profiles & Acceleration Ramps

• Preventing Launch Slip: Suddenly commanding full speed will cause the drive wheels to spin on gym floors. Always ramp velocity smoothly over an acceleration period (e.g. 1.0 to 2.0 seconds).
• Smooth Deceleration: Rather than stopping abruptly, calculate remaining distance from your encoders and decelerate smoothly ($v = \sqrt{2 \cdot a \cdot d_\text{remaining}}$) so the vehicle rolls gently to a halt without skidding.

Leg Termination Conditions

Relying solely on a timer to end a run causes large distance variations as battery voltage or floor grip fluctuates. The most reliable trigger to end a movement leg is accumulated encoder distance.

Closed-Loop Steering Architecture

To prevent the car from veering off-line:

1. Gyro Heading Correction: Proportional feedback derived from the integrated Z-axis angular rate counters rotational yaw drift.
2. Encoder Differential Trim: Monitoring the difference in wheel rotations ($\Delta \text{ticks} = \text{Ticks}_L - \text{Ticks}_R$) corrects for small mechanical drag imbalances over long distances.
3. Mutual Exclusion: When the gyro is actively correcting an angular heading disturbance, attenuate encoder differential trim so the two feedback systems do not fight each other.

Flash Memory Persistence

You can use the ESP32's built-in Preferences library to save calibrated values (such as encoder ticks-per-meter or steering gains) directly into non-volatile flash memory, ensuring they persist across reboots.

4. Arming & Disarming the Motors

The TMC2209 driver enable pins are active LOW:

• LOW: Energizes motor coils with holding torque.
• HIGH: Cuts coil current, allowing the wheels to spin freely by hand (essential during push-roll calibration).

void setMotorsArmed(bool arm) {
  digitalWrite(PIN_M1_EN, arm ? LOW : HIGH);
  digitalWrite(PIN_M2_EN, arm ? LOW : HIGH);
}

5. Voltage Sag Check on Motor Arming

When stepper drivers engage holding torque, resting current draw increases. If measured battery rail voltage drops significantly upon arming (e.g. dropping by more than 1.5V to 2.0V):

• Primary Cause (Depleted Batteries): Excessive voltage sag or low bus voltage almost always indicates that the battery pack is depleted or has high internal cell resistance. Always recharge batteries or swap in fresh cells before altering driver current settings.
• Secondary Cause (Driver Vref Set Too High): If batteries are verified fully charged, high voltage sag indicates that the TMC2209 driver trimpot Vref is set too high, drawing excessive coil current.

float getBatteryVoltage() {
  uint32_t mv = analogReadMilliVolts(PIN_VOLT_ADC);
  return (mv / 1000.0) * 11.0; // 100k/10k divider ratio (11:1)
}

void checkArmingVoltageSag() {
  float vIdle = getBatteryVoltage();
  setMotorsArmed(true);
  delay(100);
  float vArmed = getBatteryVoltage();

  // If voltage drops significantly under resting holding torque
  if ((vIdle - vArmed) > 1.5) {
    Serial.println("WARNING: Severe voltage sag detected on motor arming!");
    Serial.println("Verify battery charge level first. If fully charged, lower driver Vref.");
    setMotorsArmed(false); // Disarm to prevent over-discharging the battery
  }
}

6. Moving the Motors & Speed Ramping

Because the motors face opposite directions across the chassis, one motor's direction pin must be inverted. On the EVDrive PCB, forward = left DIR HIGH and right DIR LOW:

void setDirection(bool forward) {
  // Left (Motor A) forward is HIGH, Right (Motor B) forward is LOW
  digitalWrite(PIN_M1_DIR, forward ? HIGH : LOW);
  digitalWrite(PIN_M2_DIR, forward ? LOW : HIGH);
}

// Basic drive step loop with an acceleration ramp to prevent tire slip
void driveStepsWithRamp(int totalSteps, int startDelayUs, int cruiseDelayUs) {
  int rampSteps = 400; // Number of steps to ramp up speed

  for (int i = 0; i < totalSteps; i++) {
    int currentDelay = cruiseDelayUs;
    if (i < rampSteps) {
      currentDelay = map(i, 0, rampSteps, startDelayUs, cruiseDelayUs);
    }

    digitalWrite(PIN_M1_STEP, HIGH);
    digitalWrite(PIN_M2_STEP, HIGH);
    delayMicroseconds(2);
    digitalWrite(PIN_M1_STEP, LOW);
    digitalWrite(PIN_M2_STEP, LOW);
    delayMicroseconds(currentDelay);
  }
}

7. Stepper Motor Acoustic Alerts

You can generate audible beeps using the stepper motors without moving the chassis by rapidly oscillating the direction pin at acoustic frequencies. The motor coils act like an acoustic transducer:

void beep(int frequencyHz, int durationMs) {
  int halfPeriodUs = 1000000 / (frequencyHz * 2);
  unsigned long endTime = millis() + durationMs;
  bool dir = false;

  setMotorsArmed(true);

  while (millis() < endTime) {
    dir = !dir;
    digitalWrite(PIN_M1_DIR, dir ? HIGH : LOW);
    digitalWrite(PIN_M2_DIR, dir ? HIGH : LOW);

    digitalWrite(PIN_M1_STEP, HIGH);
    digitalWrite(PIN_M2_STEP, HIGH);
    delayMicroseconds(2);
    digitalWrite(PIN_M1_STEP, LOW);
    digitalWrite(PIN_M2_STEP, LOW);

    delayMicroseconds(halfPeriodUs);
  }
}

void playDoneBeeps() {
  beep(1200, 100);
  delay(50);
  beep(1600, 150);
}

8. Reading the AS5600 Magnetic Encoders

The AS5600 encoders communicate over I2C at address 0x36. The right wheel encoder connects to Wire (pins 1 & 2 on header J1), while the left wheel encoder connects to Wire1 (pins 41 & 42 on header J2):

#include <Wire.h>

void setupEncoders() {
  Wire.begin(PIN_I2C1_SDA, PIN_I2C1_SCL);
  Wire1.begin(PIN_I2C2_SDA, PIN_I2C2_SCL);
}

// Read raw 12-bit angle (0 to 4095)
uint16_t readAngle(TwoWire &bus) {
  bus.beginTransmission(0x36);
  bus.write(0x0C); // Raw angle register
  if (bus.endTransmission(false) != 0) return 0;
  bus.requestFrom(0x36, 2);
  if (bus.available() < 2) return 0;
  return ((bus.read() << 8) | bus.read()) & 0x0FFF;
}

• Encoder Direction: Because the motors are mounted mirror-image, one encoder counts downward during forward travel. You can invert the left encoder accumulator in code (LEFT_ENC_INVERT = true) so both wheels report positive travel.
• Angle Wrap-Around: The raw register rolls over between 4095 and 0. Calculate signed angular deltas between consecutive reads to track cumulative position.

9. Push Rollout Calibration Routine

Disarming the motor drivers allows you to measure true wheel rollout by pushing the vehicle along an exact measured distance:

void runPushCalibration() {
  setMotorsArmed(false); // Disarm holding torque for free-wheeling
  Serial.println("Motors disarmed. Push the car forward 1.0 meter...");

  uint16_t lastAngleL = readAngle(Wire1);
  uint16_t lastAngleR = readAngle(Wire);
  long totalTicksL = 0;
  long totalTicksR = 0;

  // Accumulate counts until the start button is pressed
  while (digitalRead(PIN_START_BTN) == HIGH) {
    uint16_t angleL = readAngle(Wire1);
    uint16_t angleR = readAngle(Wire);

    int diffL = angleL - lastAngleL;
    if (diffL > 2048) diffL -= 4096;
    else if (diffL < -2048) diffL += 4096;
    totalTicksL += abs(diffL);
    lastAngleL = angleL;

    int diffR = angleR - lastAngleR;
    if (diffR > 2048) diffR -= 4096;
    else if (diffR < -2048) diffR += 4096;
    totalTicksR += abs(diffR);
    lastAngleR = angleR;

    delay(5);
  }

  long avgTicks = (totalTicksL + totalTicksR) / 2;
  Serial.printf("Calibration complete. Measured ticks per meter: %ld\n", avgTicks);
  beep(1500, 200);
}

10. Reading the Gyro & Heading Steering

The MPU-6050 IMU communicates over Wire (I2C address 0x68). Some third-party breakouts report 0x74 from the WHO_AM_I register (0x75); accept both addresses.

Reading Angular Rate (Gyro Z):

int16_t readGyroZ() {
  Wire.beginTransmission(0x68);
  Wire.write(0x47); // Gyro Z high byte register
  Wire.endTransmission(false);
  Wire.requestFrom(0x68, 2);
  if (Wire.available() < 2) return 0;
  return (Wire.read() << 8) | Wire.read();
}

Stationary Bias Nulling & Sign Convention

Stationary gyros exhibit a small DC rate offset. Sample at least 40 stationary readings immediately before each launch and subtract the average from raw readings.

On standard chassis orientations, positive heading indicates the front of the vehicle is turned LEFT (counter-clockwise). Check your sensor orientation manually: rotate the chassis left by hand and verify that heading increases positively.

Closed-Loop Heading Correction

float heading = 0.0; // Integrated angular heading. Positive = nose turned left.

// In your motion loop:
int leftDelay = 800;   // Base microsecond step delay
int rightDelay = 800;
int correctionTrim = 25; // Tuning gain: adjustment applied per unit drift

// Nose turned left (heading > 0.3): slow the right wheel to steer back right
if (heading > 0.3) {
  rightDelay += correctionTrim;
}
// Nose turned right (heading < -0.3): slow the left wheel to steer back left
else if (heading < -0.3) {
  leftDelay += correctionTrim;
}

11. Buttons, Wireless Switch & Mode Control

void setupPins() {
  pinMode(PIN_START_BTN, INPUT_PULLUP);
  pinMode(PIN_WL_SW, INPUT_PULLUP);
  pinMode(PIN_WL_LED, OUTPUT);
  pinMode(PIN_USER_LED, OUTPUT);
}

// Check wireless switch and update status LED
bool isWirelessOn() {
  bool on = (digitalRead(PIN_WL_SW) == LOW);
  digitalWrite(PIN_WL_LED, on ? HIGH : LOW);
  return on;
}

// Mode Interlock Notice
void checkSafetyBeforeRun() {
  if (isWirelessOn()) {
    Serial.println("NOTICE: Wireless switch is ON. Set switch to OFF and reset for isolated offline run.");
    for (int i = 0; i < 3; i++) {
      digitalWrite(PIN_WL_LED, LOW);
      beep(1400, 150);
      digitalWrite(PIN_WL_LED, HIGH);
      delay(100);
    }
  }
}

Next Steps

• For full parameter tuning, distance factor math, and gain optimization, see Calibration & Tuning.
• For chassis customization and mounting hardware, see Customization & Hardware.
• For Bluetooth service specifications and wireless setup, see Wireless & Bluetooth Setup.
• For the browser interface and real-time graphing, see Web Bluetooth & Dashboard.