ESP32 Drone Firmware: Motor, Tilt and Balance Code

Upload these three sketches in order with the Arduino IDE (ESP32 board package). First test each motor, then read the tilt sensor, then let the ESP32 balance the drone.

Check before the next step

• Motor test: each motor spins in the right order
• Tilt test: roll and pitch change when you tip the board
• Balance test: props off, tip drone and feel the low side speed up

Safety: Props off for all three tests on your desk.

// Step 1: spin each motor for 1 second. TAKE THE PROPS OFF.
const int motorPins[4] = {25, 26, 27, 14};  // FL, FR, RL, RR

void setup() {
  for (int i = 0; i < 4; i++) {
    ledcAttach(motorPins[i], 20000, 8);  // 20 kHz PWM, speed 0-255
    ledcWrite(motorPins[i], 0);
  }
}

void loop() {
  for (int i = 0; i < 4; i++) {
    ledcWrite(motorPins[i], 60);   // gentle test speed
    delay(1000);
    ledcWrite(motorPins[i], 0);
    delay(500);
  }
  delay(3000);
}

// Step 2: read roll and pitch from the MPU6050.
#include <Wire.h>
const int MPU = 0x68;

void setup() {
  Serial.begin(115200);
  Wire.begin(21, 22);                 // SDA, SCL
  Wire.beginTransmission(MPU);
  Wire.write(0x6B); Wire.write(0);    // wake sensor up
  Wire.endTransmission();
}

void loop() {
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);                   // accelerometer data starts here
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6);
  int16_t ax = Wire.read() << 8 | Wire.read();
  int16_t ay = Wire.read() << 8 | Wire.read();
  int16_t az = Wire.read() << 8 | Wire.read();

  float roll  = atan2(ay, az) * 180 / PI;
  float pitch = atan2(-ax, sqrt((float)ay * ay + (float)az * az)) * 180 / PI;
  Serial.printf("Roll: %.1f  Pitch: %.1f\n", roll, pitch);
  delay(100);
}

// Step 3: simple P balancer. It speeds up the motors on the low side.
#include <Wire.h>
const int MPU = 0x68;
const int motorPins[4] = {25, 26, 27, 14};  // FL, FR, RL, RR
float Kp = 1.5;    // proportional gain
int base = 110;    // test throttle (props OFF!)

void setMotor(int i, float speed) {
  ledcWrite(motorPins[i], constrain((int)speed, 0, 255));
}

void setup() {
  Wire.begin(21, 22);
  for (int i = 0; i < 4; i++) ledcAttach(motorPins[i], 20000, 8);
}

void loop() {
  // read roll from MPU6050...
  float roll = 0; // sensor value
  float error = 0 - roll; // target is 0° level
  float correction = error * Kp;

  setMotor(0, base + correction); // Left motors
  setMotor(2, base + correction);
  setMotor(1, base - correction); // Right motors
  setMotor(3, base - correction);
  delay(10);
}

See this step in 3D