ESP32 Self-Balancing Hoverboard

#include <Arduino.h>
#include <Wire.h>
#include <math.h>


// Forward declarations
bool writeRegister(uint8_t reg, uint8_t value);
bool readMotion(int16_t &ax, int16_t &ay, int16_t &az, int16_t &gx, int16_t &gy, int16_t &gz);
void stopMotors();
void setOneMotor(int in1, int in2, int command);
void disableForFault(const char *reason);
bool calibrateImu();

constexpr uint8_t MPU_ADDR = 0x68;
constexpr int MOTOR_A_IN1 = 25;
constexpr int MOTOR_A_IN2 = 26;
constexpr int MOTOR_B_IN1 = 27;
constexpr int MOTOR_B_IN2 = 32;
constexpr int DRIVER_SLEEP_PIN = 33;
constexpr int DRIVER_FAULT_PIN = 14;
constexpr int IMU_SDA_PIN = 21;
constexpr int IMU_SCL_PIN = 22;

constexpr float LOOP_SECONDS = 0.005f;       // 200 Hz
constexpr float COMPLEMENTARY_ALPHA = 0.985f;
constexpr float TILT_CUTOFF_DEG = 35.0f;
constexpr int MAX_PWM = 130;                 // deliberately limited for bench testing
constexpr float KP = 8.0f;
constexpr float KI = 0.15f;
constexpr float KD = 0.22f;

float pitchDeg = 0.0f;
float gyroBiasY = 0.0f;
float integral = 0.0f;
float previousError = 0.0f;
bool armed = false;
unsigned long lastLoopUs = 0;

bool writeRegister(uint8_t reg, uint8_t value) {
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(reg);
  Wire.write(value);
  return Wire.endTransmission() == 0;
}

bool readMotion(int16_t &ax, int16_t &ay, int16_t &az, int16_t &gx, int16_t &gy, int16_t &gz) {
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x3B);
  if (Wire.endTransmission(false) != 0 || Wire.requestFrom(MPU_ADDR, (uint8_t)14) != 14) return false;
  ax = (Wire.read() << 8) | Wire.read();
  ay = (Wire.read() << 8) | Wire.read();
  az = (Wire.read() << 8) | Wire.read();
  gx = (Wire.read() << 8) | Wire.read();
  gy = (Wire.read() << 8) | Wire.read();
  gz = (Wire.read() << 8) | Wire.read();
  return true;
}

void stopMotors() {
  analogWrite(MOTOR_A_IN1, 0);
  analogWrite(MOTOR_A_IN2, 0);
  analogWrite(MOTOR_B_IN1, 0);
  analogWrite(MOTOR_B_IN2, 0);
}

void setOneMotor(int in1, int in2, int command) {
  command = constrain(command, -MAX_PWM, MAX_PWM);
  if (command > 0) {
    analogWrite(in1, command);
    analogWrite(in2, 0);
  } else if (command < 0) {
    analogWrite(in1, 0);
    analogWrite(in2, -command);
  } else {
    analogWrite(in1, 0);
    analogWrite(in2, 0);
  }
}

void disableForFault(const char *reason) {
  armed = false;
  integral = 0.0f;
  stopMotors();
  digitalWrite(DRIVER_SLEEP_PIN, LOW);
  Serial.println(reason);
}

bool calibrateImu() {
  const int samples = 600;
  float gyroTotal = 0.0f;
  float pitchTotal = 0.0f;
  for (int i = 0; i < samples; ++i) {
    int16_t ax, ay, az, gx, gy, gz;
    if (!readMotion(ax, ay, az, gx, gy, gz)) return false;
    gyroTotal += gy;
    pitchTotal += atan2f((float)ax, sqrtf((float)ay * ay + (float)az * az)) * 180.0f / PI;
    delay(3);
  }
  gyroBiasY = gyroTotal / samples;
  pitchDeg = pitchTotal / samples;
  return true;
}

void setup() {
  Serial.begin(115200);
  pinMode(MOTOR_A_IN1, OUTPUT);
  pinMode(MOTOR_A_IN2, OUTPUT);
  pinMode(MOTOR_B_IN1, OUTPUT);
  pinMode(MOTOR_B_IN2, OUTPUT);
  pinMode(DRIVER_SLEEP_PIN, OUTPUT);
  pinMode(DRIVER_FAULT_PIN, INPUT); // external 10 kOhm pull-up is fitted
  stopMotors();
  digitalWrite(DRIVER_SLEEP_PIN, LOW);

  Wire.begin(IMU_SDA_PIN, IMU_SCL_PIN);
  Wire.setClock(400000);
  if (!writeRegister(0x6B, 0x00) || !writeRegister(0x1B, 0x00) || !writeRegister(0x1C, 0x00)) {
    Serial.println("IMU not detected; motors remain off.");
    return;
  }

  Serial.println("Hold the secured rig upright and still: calibrating IMU.");
  if (!calibrateImu()) {
    Serial.println("IMU read failed; motors remain off.");
    return;
  }
  if (digitalRead(DRIVER_FAULT_PIN) == LOW) {
    Serial.println("Motor-driver fault present; motors remain off.");
    return;
  }
  digitalWrite(DRIVER_SLEEP_PIN, HIGH);
  armed = true;
  lastLoopUs = micros();
  Serial.println("Armed at low power. Keep the rig restrained.");
}

void loop() {
  if (!armed) {
    delay(50);
    return;
  }

  const unsigned long now = micros();
  if ((unsigned long)(now - lastLoopUs) < 5000) return;
  float dt = (now - lastLoopUs) / 1000000.0f;
  lastLoopUs = now;
  if (dt > 0.02f) dt = LOOP_SECONDS;

  if (digitalRead(DRIVER_FAULT_PIN) == LOW) {
    disableForFault("Motor-driver fault: motors disabled.");
    return;
  }

  int16_t ax, ay, az, gx, gy, gz;
  if (!readMotion(ax, ay, az, gx, gy, gz)) {
    disableForFault("IMU communication fault: motors disabled.");
    return;
  }

  const float accelPitch = atan2f((float)ax, sqrtf((float)ay * ay + (float)az * az)) * 180.0f / PI;
  const float gyroRate = ((float)gy - gyroBiasY) / 131.0f;
  pitchDeg = COMPLEMENTARY_ALPHA * (pitchDeg + gyroRate * dt) + (1.0f - COMPLEMENTARY_ALPHA) * accelPitch;

  if (fabsf(pitchDeg) > TILT_CUTOFF_DEG) {
    disableForFault("Tilt cutoff: stand the rig upright, then reset power.");
    return;
  }

  const float error = -pitchDeg; // upright is the calibrated zero-angle target
  integral = constrain(integral + error * dt, -20.0f, 20.0f);
  const float derivative = (error - previousError) / dt;
  previousError = error;
  const int motorCommand = (int)(KP * error + KI * integral + KD * derivative);

  // Same command on both wheels: balance only. No rider/joystick motion control in this bench prototype.
  setOneMotor(MOTOR_A_IN1, MOTOR_A_IN2, motorCommand);
  setOneMotor(MOTOR_B_IN1, MOTOR_B_IN2, motorCommand);
}