ESP32 Bench
Boards: ESP32, S2, S3, C3, C6
Flash and monitor: Web Serial
← All projects

Self-levelling platform

Tilt the base and two servos tilt the plate the other way, so a marble stays put. An MPU-6050 measures the tilt and a complementary filter keeps it steady.

Intermediate45 min115200 baud
Board
Fig. 1. Pins this project uses on the ESP32-DevKitC V4, 38 pin. USB connector at the bottom.

Wiring

FromTo boardNote
MPU-6050 VCC3V3The GY-521 board also accepts 5 V, but 3V3 keeps its I2C lines at the ESP32's level
MPU-6050 GNDGND
MPU-6050 SDAGPIO 21The board has its own pull-up resistors
MPU-6050 SCLGPIO 22
Roll servo signalGPIO 18Tilts the plate left and right
Pitch servo signalGPIO 19Tilts the plate forward and back
Both servos red5 V supply +Not the ESP32's 3V3 pin. Two servos moving at once can draw an amp
Both servos brown, and supply -GNDJoin the servo supply's ground to the ESP32's GND

Tested on this board

Not run on this board yet. Treat the wiring and code as a first draft.

Firmware, Arduino sketch for ESP32 Dev Module

// Self-levelling platform: an MPU-6050 on the base measures how far it is
// tilted, and two servos tilt the plate the other way so it stays level.
// Talks to the MPU-6050 directly over I2C, so no extra library is needed.
#include <Wire.h>
#include <ESP32Servo.h>

const int SDA_PIN = 21;
const int SCL_PIN = 22;
const int SERVO_X_PIN = 18;  // tilts the plate left and right (roll)
const int SERVO_Y_PIN = 19;  // tilts the plate forward and back (pitch)

const uint8_t MPU = 0x68;      // 0x69 if the board's AD0 pin is tied high
const float ALPHA = 0.98f;     // complementary filter: 98 % gyro, 2 % accelerometer
const float GAIN_X = 1.0f;     // servo degrees per degree of tilt; depends on your linkage
const float GAIN_Y = 1.0f;
const bool INVERT_X = false;   // flip if a servo makes the tilt worse instead of better
const bool INVERT_Y = false;
const int CENTER = 90, TRAVEL = 45;  // servo centre and how far either way it may go

Servo servoX, servoY;
float gyroBiasX = 0, gyroBiasY = 0;
float pitch = 0, roll = 0;          // filtered tilt, degrees
float pitchZero = 0, rollZero = 0;  // the tilt at start-up counts as level
unsigned long lastUs = 0;

void writeReg(uint8_t reg, uint8_t value) {
  Wire.beginTransmission(MPU);
  Wire.write(reg);
  Wire.write(value);
  Wire.endTransmission();
}

// Accelerometer in g and gyro in degrees per second, from one 14-byte read.
bool readMpu(float& ax, float& ay, float& az, float& gx, float& gy) {
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);  // ACCEL_XOUT_H
  if (Wire.endTransmission(false) != 0) return false;
  if (Wire.requestFrom(MPU, (uint8_t)14) != 14) return false;
  int16_t raw[7];
  for (int i = 0; i < 7; i++) raw[i] = (int16_t)((Wire.read() << 8) | Wire.read());
  ax = raw[0] / 16384.0f;  // ±2 g range
  ay = raw[1] / 16384.0f;
  az = raw[2] / 16384.0f;
  gx = raw[4] / 131.0f;    // ±250 °/s range; raw[3] is temperature
  gy = raw[5] / 131.0f;
  return true;
}

void accelAngles(float ax, float ay, float az, float& p, float& r) {
  p = degrees(atan2f(-ax, sqrtf(ay * ay + az * az)));
  r = degrees(atan2f(ay, az));
}

void setup() {
  Serial.begin(115200);
  Wire.begin(SDA_PIN, SCL_PIN, 400000);
  Wire.beginTransmission(MPU);
  if (Wire.endTransmission() != 0) {
    Serial.println("MPU-6050 not found at 0x68: check SDA, SCL, 3V3 and GND");
    while (true) delay(1000);
  }
  writeReg(0x6B, 0x00);  // PWR_MGMT_1: wake up
  writeReg(0x1A, 0x03);  // CONFIG: 44 Hz low-pass filter to calm the servos
  writeReg(0x1B, 0x00);  // GYRO_CONFIG: ±250 °/s
  writeReg(0x1C, 0x00);  // ACCEL_CONFIG: ±2 g

  servoX.setPeriodHertz(50);
  servoY.setPeriodHertz(50);
  servoX.attach(SERVO_X_PIN, 500, 2400);
  servoY.attach(SERVO_Y_PIN, 500, 2400);
  servoX.write(CENTER);
  servoY.write(CENTER);

  // Calibrate: hold still for two seconds while the gyro's drift and the
  // starting tilt are measured.
  Serial.println("Calibrating: keep the base still and level");
  delay(500);
  float sumGx = 0, sumGy = 0, sumP = 0, sumR = 0;
  int n = 0;
  for (int i = 0; i < 400; i++) {
    float ax, ay, az, gx, gy;
    if (readMpu(ax, ay, az, gx, gy)) {
      float p, r;
      accelAngles(ax, ay, az, p, r);
      sumGx += gx;
      sumGy += gy;
      sumP += p;
      sumR += r;
      n++;
    }
    delay(5);
  }
  if (n > 0) {
    gyroBiasX = sumGx / n;
    gyroBiasY = sumGy / n;
    pitchZero = sumP / n;
    rollZero = sumR / n;
  }
  pitch = pitchZero;
  roll = rollZero;
  lastUs = micros();
  Serial.println("Levelling. Tilt the base and watch the plate.");
}

unsigned long lastPrint = 0;

void loop() {
  float ax, ay, az, gx, gy;
  if (!readMpu(ax, ay, az, gx, gy)) {
    Serial.println("MPU-6050 read failed");
    delay(100);
    return;
  }
  const unsigned long now = micros();
  const float dt = (now - lastUs) / 1e6f;
  lastUs = now;

  // Complementary filter: the gyro is smooth but drifts, the accelerometer is
  // noisy but never drifts. Mix them.
  float ap, ar;
  accelAngles(ax, ay, az, ap, ar);
  roll = ALPHA * (roll + (gx - gyroBiasX) * dt) + (1 - ALPHA) * ar;
  pitch = ALPHA * (pitch + (gy - gyroBiasY) * dt) + (1 - ALPHA) * ap;

  // Tilt the plate against the base's tilt.
  const float tiltX = roll - rollZero, tiltY = pitch - pitchZero;
  const int sx = constrain((int)roundf(CENTER + (INVERT_X ? 1 : -1) * GAIN_X * tiltX), CENTER - TRAVEL, CENTER + TRAVEL);
  const int sy = constrain((int)roundf(CENTER + (INVERT_Y ? 1 : -1) * GAIN_Y * tiltY), CENTER - TRAVEL, CENTER + TRAVEL);
  servoX.write(sx);
  servoY.write(sy);

  if (millis() - lastPrint >= 100) {
    lastPrint = millis();
    Serial.printf("pitch = %.1f, roll = %.1f, servo_x = %d, servo_y = %d\n", tiltY, tiltX, sx, sy);
  }
  delay(5);
}

Parts

QuantityPartWhere to buy
1×ESP32 dev boardBuy at Amazon
1×MPU-6050 accelerometer and gyro board (GY-521)
2×Micro servos, SG90 or similarBuy at Adafruit
1×5 V supply for the servos, such as a USB power bank or a 5 V 2 A adapter
1×Small plate and base: card, plywood or a 3D print
10×Jumper wires

Amazon buy links may include an Associates tag, and this site may earn a commission from qualifying Amazon purchases at no extra cost to you. Adafruit links are direct.

Libraries

Wire (built in), ESP32Servo

Load it

  1. In the Arduino IDE, install the esp32 board package and pick the board named above the sketch.
  2. Install the libraries listed above from the Library Manager.
  3. Paste the sketch, choose the board's port and upload. Hold BOOT if the upload stalls at Connecting.
  4. Close the IDE's serial monitor, then open the bench here to watch the output.

Field notes

  • Fix the MPU-6050 to the base, not the plate. The sketch measures how far the base tilts and turns the servos the other way.
  • Keep the base still and level for the first two seconds after power-up: the sketch measures the gyro's drift and takes the starting position as level.
  • If the plate tilts the wrong way on one axis, set INVERT_X or INVERT_Y. If it under- or over-corrects, change GAIN_X and GAIN_Y: the right value depends on how far each servo arm moves the plate.
  • The complementary filter trusts the gyro 98 % (ALPHA) for smooth, quick moves and the accelerometer 2 % to stop the gyro drifting. Lower ALPHA if it slowly wanders, raise it if it jitters.
  • Many cheap MPU-6050 boards are clones. If the board is not found, run the I2C scanner project: it may answer at 0x69 if its AD0 pin is high.
  • Servos that jitter or reset the board are short of current. Power them from their own 5 V supply and join the grounds.

Not tested yet

  • Not tested on hardware yet. Compiles for every board listed with Arduino-ESP32 core 3.3.12.
Open the bench at 115200 baud

Community builds

Loading build reports from people who made this.

Built it? Send a build report with your board and what the serial port printed. Working reports become the "Tested on this board" entries above.