Oh Sorry I Mean Self Balancing Robot

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


// Forward declarations
void setMotor(int speed, int channelForward, int channelReverse);
void stopMotors();
void driveBothMotors(int speed);
void calibrateGyro();

constexpr int MPU_SDA_PIN = 21;
constexpr int MPU_SCL_PIN = 22;
constexpr int LEFT_IN1_PIN = 25;
constexpr int LEFT_IN2_PIN = 26;
constexpr int RIGHT_IN1_PIN = 32;
constexpr int RIGHT_IN2_PIN = 33;
constexpr int DRIVER_SLEEP_PIN = 14;

constexpr int LEFT_PWM_A = 0;
constexpr int LEFT_PWM_B = 1;
constexpr int RIGHT_PWM_A = 2;
constexpr int RIGHT_PWM_B = 3;
constexpr int PWM_FREQUENCY = 20000;
constexpr int PWM_RESOLUTION = 8;
constexpr int MAX_PWM = 255;

// Tune these safely with the wheels off the floor.
constexpr float TARGET_ANGLE_DEG = 0.0f;
constexpr float KP = 20.0f;
constexpr float KD = 0.85f;
constexpr float FILTER_WEIGHT = 0.98f;
constexpr float FALL_ANGLE_DEG = 35.0f;
constexpr int MIN_DRIVE_PWM = 42;

MPU6050 mpu;
float gyroYOffset = 0.0f;
float pitchDeg = 0.0f;
unsigned long lastControlUs = 0;
bool sensorReady = false;

void setMotor(int speed, int channelForward, int channelReverse) {
  speed = constrain(speed, -MAX_PWM, MAX_PWM);
  if (speed > 0) {
    ledcWrite(channelForward, speed);
    ledcWrite(channelReverse, 0);
  } else if (speed < 0) {
    ledcWrite(channelForward, 0);
    ledcWrite(channelReverse, -speed);
  } else {
    ledcWrite(channelForward, 0);
    ledcWrite(channelReverse, 0);
  }
}

void stopMotors() {
  setMotor(0, LEFT_PWM_A, LEFT_PWM_B);
  setMotor(0, RIGHT_PWM_A, RIGHT_PWM_B);
}

void driveBothMotors(int speed) {
  if (speed != 0 && abs(speed) < MIN_DRIVE_PWM) {
    speed = speed > 0 ? MIN_DRIVE_PWM : -MIN_DRIVE_PWM;
  }
  setMotor(speed, LEFT_PWM_A, LEFT_PWM_B);
  setMotor(speed, RIGHT_PWM_A, RIGHT_PWM_B);
}

void calibrateGyro() {
  constexpr int SAMPLES = 800;
  long gyroSum = 0;
  for (int i = 0; i < SAMPLES; ++i) {
    int16_t gx, gy, gz;
    mpu.getRotation(&gx, &gy, &gz);
    gyroSum += gy;
    delay(2);
  }
  gyroYOffset = gyroSum / static_cast<float>(SAMPLES);
}

void setup() {
  Serial.begin(115200);
  Wire.begin(MPU_SDA_PIN, MPU_SCL_PIN);
  Wire.setClock(400000);

  pinMode(DRIVER_SLEEP_PIN, OUTPUT);
  digitalWrite(DRIVER_SLEEP_PIN, LOW);

  ledcSetup(LEFT_PWM_A, PWM_FREQUENCY, PWM_RESOLUTION);
  ledcSetup(LEFT_PWM_B, PWM_FREQUENCY, PWM_RESOLUTION);
  ledcSetup(RIGHT_PWM_A, PWM_FREQUENCY, PWM_RESOLUTION);
  ledcSetup(RIGHT_PWM_B, PWM_FREQUENCY, PWM_RESOLUTION);
  ledcAttachPin(LEFT_IN1_PIN, LEFT_PWM_A);
  ledcAttachPin(LEFT_IN2_PIN, LEFT_PWM_B);
  ledcAttachPin(RIGHT_IN1_PIN, RIGHT_PWM_A);
  ledcAttachPin(RIGHT_IN2_PIN, RIGHT_PWM_B);
  stopMotors();

  mpu.initialize();
  if (!mpu.testConnection()) {
    Serial.println("MPU-6050 not found: motors stay off.");
    return;
  }

  Serial.println("Hold robot upright and still: calibrating.");
  calibrateGyro();

  int16_t ax, ay, az;
  mpu.getAcceleration(&ax, &ay, &az);
  pitchDeg = atan2f(static_cast<float>(ax), static_cast<float>(az)) * 180.0f / PI;
  sensorReady = true;
  lastControlUs = micros();
  digitalWrite(DRIVER_SLEEP_PIN, HIGH);
  Serial.println("Balancing enabled.");
}

void loop() {
  if (!sensorReady) {
    digitalWrite(DRIVER_SLEEP_PIN, LOW);
    stopMotors();
    delay(100);
    return;
  }

  const unsigned long nowUs = micros();
  const float dt = (nowUs - lastControlUs) / 1000000.0f;
  if (dt < 0.004f) return;
  lastControlUs = nowUs;

  int16_t ax, ay, az, gx, gy, gz;
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  const float accelPitchDeg = atan2f(static_cast<float>(ax), static_cast<float>(az)) * 180.0f / PI;
  const float gyroRateDegPerSecond = (static_cast<float>(gy) - gyroYOffset) / 131.0f;
  pitchDeg = FILTER_WEIGHT * (pitchDeg + gyroRateDegPerSecond * dt) +
             (1.0f - FILTER_WEIGHT) * accelPitchDeg;

  if (fabsf(pitchDeg - TARGET_ANGLE_DEG) > FALL_ANGLE_DEG) {
    stopMotors();
    return;
  }

  const float error = pitchDeg - TARGET_ANGLE_DEG;
  const int command = static_cast<int>(KP * error + KD * gyroRateDegPerSecond);
  driveBothMotors(command);
}