RH Circuits
← Home
Docs/Software/Firmware Setup & Code Guide

Firmware Setup & Code Guide

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

Treat these code snippets like Legos: mix, match, and snap together the parts you need to build your own custom competition sketch.

1. Arduino IDE Setup

  1. Board Manager: Install the official esp32 board package by Espressif (Tools > Board > Boards Manager).
  2. Select Board: Choose ESP32S3 Dev Module.
  3. USB Cable: Plug your USB-C cable into the LEFT port (labeled UART) on the ESP32-S3 DevKit. This port is for uploading code and using the Serial Monitor.
  4. Settings:
    • USB CDC On Boot: Enabled
    • Flash Size: 8MB (or whatever matches your DevKit)

2. Pin Definitions

Here are the verified pins on the PCB:

cpp
// 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
const int PIN_I2C1_SCL = 2;
const int PIN_I2C2_SDA = 41; // Encoder B
const int PIN_I2C2_SCL = 42;

// Buttons, Switches & Battery
const int PIN_START_BTN = 7;  // Start Button (Active LOW)
const int PIN_WL_SW     = 15; // Wireless Switch (LOW when ON)
const int PIN_VOLT_ADC  = 13; // Battery voltage divider
const int PIN_WL_LED    = 16; // Wireless LED
const int PIN_USER_LED  = 40; // Status LED

3. Arming & Disarming the Motors

The TMC2209 driver enable pin is active LOW:

  • LOW: Turns on motor holding power.
  • HIGH: Cuts power so the wheels can spin freely by hand (useful for rolling the car to test encoders).
cpp
void setMotorsArmed(bool arm) {
  digitalWrite(PIN_M1_EN, arm ? LOW : HIGH);
  digitalWrite(PIN_M2_EN, arm ? LOW : HIGH);
}

4. Lego: Voltage Sag Check on Motor Arming

CRITICAL WARNING: If your battery reads normal voltage while idle (e.g. 12V), but drops significantly when you arm the motors (e.g. down to 9V) and jumps right back up when disarmed, your stepper drivers are drawing too much current! You must turn down the Vref potentiometer on the TMC2209 modules.

Here is a snippet to test for this automatically:

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

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

  // If voltage drops by more than 1.5V under resting holding torque
  if ((vIdle - vArmed) > 1.5) {
    Serial.println("WARNING: Excessive voltage sag detected! Lower driver Vref.");
    setMotorsArmed(false); // Disarm to protect battery
  }
}

5. Moving the Motors & Speed Ramping

Because the motors face opposite directions on the chassis, one motor's direction pin needs to be inverted:

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

// 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);
  }
}

6. Beeping with the Stepper Motors

You can make the motors beep without moving the car by rapidly toggling the direction back and forth every step. The motor vibrates in place like a speaker:

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

  // Turn motors on so coils can vibrate
  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);
  }
}

// Two quick beeps on finish
void playDoneBeeps() {
  beep(1200, 100);
  delay(50);
  beep(1600, 150);
}

7. Reading the AS5600 Encoders

Encoder A is on Wire (pins 1 & 2) and Encoder B is on Wire1 (pins 41 & 42). Both use address 0x36.

cpp
#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;
}

8. Lego: Push Rollout Calibration Routine

A reliable calibration routine is to disarm the motors and count steps as you push the car exactly 1.0 meter:

cpp
void runPushCalibration() {
  setMotorsArmed(false); // Free-wheel
  Serial.println("Motors disarmed. Push the car forward 1.0 meter...");

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

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

    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("Push calibration done! Ticks per meter: %ld\n", avgTicks);
  beep(1500, 200);
}

9. Reading the Gyro & Simple Straight-Line Steering

The MPU-6050 sits on Wire (address 0x68).

Read Gyro Z (Turn Rate):

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

Simple Steering Correction:

Instead of complicated math, you can adjust your step delays based on the vehicle's heading drift:

cpp
float heading = 0; // Updated by adding gyro readings

// In your driving loop:
int leftDelay = 800;  // Base speed delay
int rightDelay = 800;

// If the car veers right (heading > 0), slow down the left wheel to pull it back straight
if (heading > 0.5) {
  leftDelay += 40;  // More delay = slower
} else if (heading < -0.5) {
  rightDelay += 40; // Veering left -> slow down right wheel
}

10. Buttons, Wireless Switch & Competition Warning

cpp
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 LED
bool isWirelessOn() {
  bool on = (digitalRead(PIN_WL_SW) == LOW);
  digitalWrite(PIN_WL_LED, on ? HIGH : LOW);
  return on;
}

// Warning: Do not run with wireless enabled in competition!
void checkSafetyBeforeRun() {
  if (isWirelessOn()) {
    Serial.println("WARNING: Wireless switch is still ON! Turn off for competition.");
    for (int i = 0; i < 3; i++) {
      digitalWrite(PIN_WL_LED, LOW);
      beep(1400, 150);
      digitalWrite(PIN_WL_LED, HIGH);
      delay(100);
    }
  }
}

Next Steps

Previous
Modifications & Rules
Next
Calibration & Tuning