🧭 IMU and encoders: know where you are

A IMU (inertial measurement unit) is a chip that senses motion without looking outside. The MPU-6050 has two sensors, each with three axes. The accelerometer measures acceleration, including gravity: when still and level, it reads 1 g pointing downward, so it can be used to determine tilt. The gyroscope measures rotational speed on each axis, in degrees per second; by adding this speed over time, you can tell how far the robot has turned.

How to read this map: The blue arrows are the three axes, always fixed to the chip. The amber arrow is gravity sensed by the accelerometer; the cyan curve is rotation around Z, measured by the gyroscope when the robot turns. The two panels sum up each sensor’s strength and weakness: they complement each other, and that’s what topic 3 uses.

Why learn this

Turning exactly 90°, detecting that the robot has gone up a ramp, or noticing that it has tipped over are tasks for the IMU. Without it, the robot only knows how far the wheels have turned, and wheels slip.

Key concepts

The MPU-6050 uses I2C, the two-wire bus you learned about in 1.4. There are four connections: VCC to 3V3, GND to GND, SDA to GPIO 21, and SCL to GPIO 22. Instead of using a ready-made library, you’ll communicate directly with the chip: write to a register to wake it up, then read 14 bytes at once, containing acceleration, temperature, and rotation.

How to read this map: On the left, the four wires: red is power, gray is ground, blue and cyan carry the I2C data. On the right, the order in which the 14 bytes arrive when you request a read starting at register 0x3B. The code reads them in this same order.

Goal: Wake up the MPU-6050 and display acceleration (g) and rotation (°/s) ten times per second. It works in Wokwi, which includes the MPU-6050 in its parts list.

// MPU-6050 over I2C, without an extra library (esp32 3.x package)
#include <Wire.h>
const int MPU = 0x68;                     // address with AD0 connected to GND

int16_t ler16() {                          // join two bytes: high byte first
  uint8_t alto = Wire.read();
  uint8_t baixo = Wire.read();
  return (int16_t)((alto << 8) | baixo);
}

void setup() {
  Serial.begin(115200);
  Wire.begin(21, 22);                      // SDA, SCL
  Wire.beginTransmission(MPU);
  Wire.write(0x6B); Wire.write(0);         // power register = 0: exits sleep mode
  if (Wire.endTransmission() != 0) {
    Serial.println("MPU-6050 did not respond: check the wires and address");
    while (true) delay(1000);
  }
  Serial.println("MPU-6050 ready");
}

void loop() {
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);                        // starts at acceleration X
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 14);
  float ax = ler16() / 16384.0;
  float ay = ler16() / 16384.0;
  float az = ler16() / 16384.0;
  ler16();                                 // temperature: ignored here
  float gx = ler16() / 131.0;
  float gy = ler16() / 131.0;
  float gz = ler16() / 131.0;
  Serial.printf("ax:%.2f,ay:%.2f,az:%.2f,gx:%.1f,gy:%.1f,gz:%.1f\n", ax, ay, az, gx, gy, gz);
  delay(100);
}

Why learn this

Reading registers directly shows you what libraries hide and works the same way for almost every I2C sensor: address, register, number of bytes, unit conversion. When you switch to a different IMU, you’ll know what to look for in the datasheet.

Key concepts

You can calculate tilt in two ways. With the accelerometer, use trigonometry based on gravity: accurate on average, but it jumps with every jolt. With the gyroscope, add up the rotation: smooth and fast, but drift pushes the result farther off. The complementary filter combines the two: it trusts the gyroscope 98% at each step and uses 2% from the accelerometer to pull the angle back toward the true value.

How to read this map: The top path (cyan) follows rapid movement; the bottom one (blue) slowly corrects drift. The weights add up to 1. The dashed arrow shows that the filter has memory: each new angle starts from the previous one. Giving more weight to the gyroscope makes the angle smoother; giving more weight to the accelerometer corrects it faster, with more noise.

Goal: calculate the forward tilt (pitch) in three ways and view all three in the Serial Plotter: accelerometer only, gyroscope only, and filtered. The loop runs at a fixed interval of 10 ms.

// Tilt with complementary filter (MPU-6050, esp32 3.x package)
#include <Wire.h>
const int MPU = 0x68;
const float ALFA = 0.98;                  // gyroscope confidence
const float DT = 0.01;                    // 10 ms per step
float offsetGy = 0, angGiro = 0, angFiltro = 0;

int16_t ler16() { uint8_t a = Wire.read(); uint8_t b = Wire.read(); return (int16_t)((a << 8) | b); }

void lerMPU(float &ax, float &ay, float &az, float &gy) {
  Wire.beginTransmission(MPU); Wire.write(0x3B); Wire.endTransmission(false);
  Wire.requestFrom(MPU, 14);
  ax = ler16() / 16384.0; ay = ler16() / 16384.0; az = ler16() / 16384.0;
  ler16(); ler16();                        // temperature and X rotation: ignored
  gy = ler16() / 131.0; ler16();           // Y rotation (used) and Z rotation
}

void setup() {
  Serial.begin(115200);
  Wire.begin(21, 22);
  Wire.beginTransmission(MPU); Wire.write(0x6B); Wire.write(0); Wire.endTransmission();
  Serial.println("Calibrating: do not move the robot...");
  float ax, ay, az, gy, soma = 0;
  for (int i = 0; i < 500; i++) { lerMPU(ax, ay, az, gy); soma += gy; delay(2); }
  offsetGy = soma / 500;                   // the stationary rotation becomes zero
  lerMPU(ax, ay, az, gy);
  angFiltro = angGiro = atan2(-ax, sqrt(ay * ay + az * az)) * 180 / PI;
}

void loop() {
  static unsigned long ultimo = micros();
  if (micros() - ultimo < 10000) return;   // wait until 10 ms have elapsed
  ultimo += 10000;
  float ax, ay, az, gy;
  lerMPU(ax, ay, az, gy);
  gy -= offsetGy;
  float angAcel = atan2(-ax, sqrt(ay * ay + az * az)) * 180 / PI;
  angGiro += gy * DT;                                          // gyroscope only: drift
  angFiltro = ALFA * (angFiltro + gy * DT) + (1 - ALFA) * angAcel;
  Serial.printf("acel:%.1f,giro:%.1f,filtro:%.1f\n", angAcel, angGiro, angFiltro);
}

Why learn this

Five lines of calculations solve the problem that brings down balancing robots and home-built drones. The Kalman filter, which you'll encounter in larger projects, follows the same idea of combining a fast sensor with one that doesn't drift, using weights that adjust automatically.

Key concepts

A encoder converts rotation into pulses. In the TT motor, a disk with 20 holes passes between an LED and a light sensor: each hole is one pulse. In the N20 motor with encoder, a magnet on the shaft passes two Hall sensors. The two sensors, called channels A and B, are offset from each other: the one that pulses first indicates the direction of rotation. This is called quadrature.

How to read this map: the two waves have the same rhythm, but B lags by a quarter cycle. In the amber lines, A rose first: the wheel is turning forward. If the wheel reverses, B starts changing first. Counting each edge of A (rising and falling) doubles the resolution at no extra cost.

Goal: count the pulses from both wheels using interrupts and display the RPM every 200 ms. Encoders: left A 18 / B 19, right A 34 / B 35 (34 and 35 don't have internal pull-ups; N20 Hall modules usually do).

// Two quadrature encoders with interrupts (esp32 3.x package)
const int ENC_E_A = 18, ENC_E_B = 19;      // left wheel
const int ENC_D_A = 34, ENC_D_B = 35;      // right wheel
const float PULSOS_VOLTA = 1400;          // N20 1:100 counting both A edges. Measure yours (topic 6)
volatile long pulsosE = 0, pulsosD = 0;

void IRAM_ATTR isrE() { if (digitalRead(ENC_E_A) == digitalRead(ENC_E_B)) pulsosE++; else pulsosE--; }
void IRAM_ATTR isrD() { if (digitalRead(ENC_D_A) == digitalRead(ENC_D_B)) pulsosD--; else pulsosD++; }  // mirrored

void setup() {
  Serial.begin(115200);
  pinMode(ENC_E_A, INPUT_PULLUP); pinMode(ENC_E_B, INPUT_PULLUP);
  pinMode(ENC_D_A, INPUT); pinMode(ENC_D_B, INPUT);
  attachInterrupt(digitalPinToInterrupt(ENC_E_A), isrE, CHANGE);
  attachInterrupt(digitalPinToInterrupt(ENC_D_A), isrD, CHANGE);
  Serial.println("Encoders ready: turn the wheels by hand");
}

void loop() {
  static long antE = 0, antD = 0;
  delay(200);
  noInterrupts(); long e = pulsosE, d = pulsosD; interrupts();   // copy without being interrupted
  float rpmE = (e - antE) / PULSOS_VOLTA / 0.2 * 60;
  float rpmD = (d - antD) / PULSOS_VOLTA / 0.2 * 60;
  antE = e; antD = d;
  Serial.printf("pulsosE:%ld,pulsosD:%ld,rpmE:%.1f,rpmD:%.1f\n", e, d, rpmE, rpmD);
}

Why learn this

Without an encoder, "move 1 meter" becomes "run the motor for 2 seconds and hope." With one, you can measure actual speed (the input to the PID in 2.3) and distance traveled (the basis for odometry in the next topic).

Key concepts

Odometry is estimating position by adding up small movements. Every 50 ms, you read how far each wheel has traveled (pulses ÷ pulses per revolution × wheel circumference). The average of the two is how far the robot's center moved forward; their difference, divided by the distance between the wheels, is how much it turned. With that, you update x, y, and θ (the direction, in radians).

How to read this map: the blue arrow is the robot's front, pointing in the direction of θ measured from the x-axis. The amber ruler L is the distance between the wheels. The five lines in the box are the entire algorithm: if the right wheel traveled farther than the left, Δθ is positive and the robot turned left.

Goal: estimate x, y (in centimeters), and θ (in degrees) by pushing the robot by hand. Use the same pins as in topic 4. Adjust the wheel radius and distance between the wheels to match your chassis.

// Differential drive robot odometry with two encoders (esp32 3.x package)
const int ENC_E_A = 18, ENC_E_B = 19, ENC_D_A = 34, ENC_D_B = 35;
const float PULSOS_VOLTA = 1400;           // measure yours (topic 6)
const float RAIO = 0.0215;                 // wheel radius in meters (43 mm wheel)
const float BASE = 0.130;                  // distance between the wheels in meters
const float M_POR_PULSO = 2 * PI * RAIO / PULSOS_VOLTA;
volatile long pulsosE = 0, pulsosD = 0;
float x = 0, y = 0, theta = 0;

void IRAM_ATTR isrE() { if (digitalRead(ENC_E_A) == digitalRead(ENC_E_B)) pulsosE++; else pulsosE--; }
void IRAM_ATTR isrD() { if (digitalRead(ENC_D_A) == digitalRead(ENC_D_B)) pulsosD--; else pulsosD++; }

void setup() {
  Serial.begin(115200);
  pinMode(ENC_E_A, INPUT_PULLUP); pinMode(ENC_E_B, INPUT_PULLUP);
  pinMode(ENC_D_A, INPUT); pinMode(ENC_D_B, INPUT);
  attachInterrupt(digitalPinToInterrupt(ENC_E_A), isrE, CHANGE);
  attachInterrupt(digitalPinToInterrupt(ENC_D_A), isrD, CHANGE);
}

void loop() {
  static long antE = 0, antD = 0;
  static unsigned long ultimoPrint = 0;
  delay(50);                                  // 50 ms interval
  noInterrupts(); long e = pulsosE, d = pulsosD; interrupts();
  float dE = (e - antE) * M_POR_PULSO;
  float dD = (d - antD) * M_POR_PULSO;
  antE = e; antD = d;
  float dist = (dE + dD) / 2;
  float dTheta = (dD - dE) / BASE;
  x += dist * cos(theta + dTheta / 2);
  y += dist * sin(theta + dTheta / 2);
  theta = atan2(sin(theta + dTheta), cos(theta + dTheta));   // keep between -180° and 180°
  if (millis() - ultimoPrint >= 250) {
    ultimoPrint = millis();
    Serial.printf("x=%.1f cm  y=%.1f cm  theta=%.1f degrees\n", x * 100, y * 100, theta * 180 / PI);
  }
}

Why learn this

Returning to base, moving to a point, drawing a square on the floor: all of this requires x, y, and θ. That’s exactly what the micro-ROS in 4.3 publishes as odometry for ROS 2 to build maps.

Key concepts

Odometry is never exact, only “almost right.” Every step carries a small error: a wheel slipped, the actual diameter is 1 mm larger, a pulse was missed. Since position is the sum of all the steps, these errors accumulate, and direction error is the worst: a 2° error at the start turns into 3.5 cm of drift for every meter traveled. The way to work with this is to calibrate carefully and occasionally correct using something that doesn’t drift.

How to read this map: The blue square is what the robot actually did; the cyan line is what the odometry calculated. There’s almost no difference along the first side. At each turn, a small angle error bends all the following sides. That’s why the calibration test uses full rotations: the error becomes large and easy to measure.

Calibration in 4 steps

Why learn this

Knowing that the position “drifts” changes how you design: short tasks can rely on odometry; long tasks need an external reference, such as the line on the floor in 2.6, the camera in Track 3, or the LiDAR with SLAM in Track 4, which exists precisely to correct this error.

Key concepts