🧭 IMU e encoders: saber onde está

A IMU (unidade de medida inercial) é um chip que sente movimento sem olhar para fora. A MPU-6050 tem dois sensores de três eixos cada. O acelerômetro mede aceleração, incluindo a gravidade: parado e reto, ele mostra 1 g apontando para baixo, e por isso serve para saber a inclinação. O giroscópio mede a velocidade de giro em cada eixo, em graus por segundo; somando essa velocidade ao longo do tempo, você sabe quanto o robô virou.

Como ler: as setas azuis são os três eixos, sempre presos ao chip. A seta âmbar é a gravidade que o acelerômetro sente; a curva ciano é o giro em torno de Z, que o giroscópio mede quando o robô vira. Os dois quadros resumem o ponto forte e o fraco de cada sensor: um completa o outro, e é isso que o tópico 3 usa.

Por que aprender

Girar exatamente 90°, perceber que subiu numa rampa ou detectar que tombou são tarefas da IMU. Sem ela, o robô só sabe quanto as rodas giraram, e roda escorrega.

Conceitos-chave

A MPU-6050 fala I2C, o barramento de dois fios que você conheceu no 1.4. São quatro ligações: VCC no 3V3, GND no GND, SDA no GPIO 21 e SCL no GPIO 22. Em vez de uma biblioteca pronta, você vai conversar direto com o chip: escrever num registrador para acordá-lo e ler 14 bytes de uma vez, com aceleração, temperatura e giro.

Como ler: à esquerda, os quatro fios; vermelho é alimentação, cinza é terra, azul e ciano são os dados do I2C. À direita, a ordem em que os 14 bytes chegam quando você pede a leitura a partir do registrador 0x3B. O código lê nessa mesma ordem.

Objetivo: acordar a MPU-6050 e mostrar aceleração (g) e giro (°/s) dez vezes por segundo. Funciona no Wokwi, que tem a MPU-6050 na lista de peças.

// MPU-6050 por I2C, sem biblioteca extra (pacote esp32 3.x)
#include <Wire.h>
const int MPU = 0x68;                     // endereço com AD0 no GND

int16_t ler16() {                          // junta dois bytes: alto primeiro
  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);         // registrador de energia = 0: sai do modo sono
  if (Wire.endTransmission() != 0) {
    Serial.println("MPU-6050 nao respondeu: confira fios e endereco");
    while (true) delay(1000);
  }
  Serial.println("MPU-6050 pronta");
}

void loop() {
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);                        // começa na aceleração X
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 14);
  float ax = ler16() / 16384.0;
  float ay = ler16() / 16384.0;
  float az = ler16() / 16384.0;
  ler16();                                 // temperatura: ignorada aqui
  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);
}

Por que aprender

Ler registradores direto mostra o que as bibliotecas escondem e funciona igual para quase todo sensor I2C: endereço, registrador, quantidade de bytes, conversão de unidade. Quando trocar de IMU, você saberá onde procurar na folha de dados.

Conceitos-chave

Dá para calcular a inclinação de dois jeitos. Pelo acelerômetro, com trigonometria sobre a gravidade: certo na média, mas pula a cada tranco. Pelo giroscópio, somando o giro: liso e rápido, mas a deriva empurra o resultado para longe. O filtro complementar mistura os dois: confia 98% no giroscópio a cada passo e usa 2% do acelerômetro para puxar o ângulo de volta à verdade.

Como ler: o caminho de cima (ciano) segue o movimento rápido; o de baixo (azul) corrige devagar a deriva. Os pesos somam 1. A seta tracejada mostra que o filtro tem memória: cada novo ângulo parte do anterior. Mais peso no giroscópio deixa o ângulo mais liso; mais peso no acelerômetro corrige mais rápido, com mais ruído.

Objetivo: calcular a inclinação para a frente (arfagem) de três jeitos e ver os três no Serial Plotter: só acelerômetro, só giroscópio e filtrado. O laço roda em tempo fixo de 10 ms.

// Inclinação com filtro complementar (MPU-6050, pacote esp32 3.x)
#include <Wire.h>
const int MPU = 0x68;
const float ALFA = 0.98;                  // confiança no giroscópio
const float DT = 0.01;                    // 10 ms por passo
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();                        // temperatura e giro X: ignorados
  gy = ler16() / 131.0; ler16();           // giro Y (usado) e giro Z
}

void setup() {
  Serial.begin(115200);
  Wire.begin(21, 22);
  Wire.beginTransmission(MPU); Wire.write(0x6B); Wire.write(0); Wire.endTransmission();
  Serial.println("Calibrando: nao mexa no robo...");
  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;                   // o giro "parado" vira o 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;   // espera completar 10 ms
  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;                                          // só giroscópio: deriva
  angFiltro = ALFA * (angFiltro + gy * DT) + (1 - ALFA) * angAcel;
  Serial.printf("acel:%.1f,giro:%.1f,filtro:%.1f\n", angAcel, angGiro, angFiltro);
}

Por que aprender

Cinco linhas de conta resolvem o problema que derruba robôs equilibristas e drones caseiros. O filtro de Kalman, que você vai encontrar em projetos maiores, segue a mesma ideia de juntar um sensor rápido com um sensor que não deriva, com pesos que se ajustam sozinhos.

Conceitos-chave

Um encoder transforma giro em pulsos. No motor TT, um disco com 20 furos passa entre um LED e um sensor de luz: cada furo é um pulso. No motor N20 com encoder, um ímã no eixo passa por dois sensores Hall. Os dois sensores, chamados canais A e B, ficam defasados: quem pulsa primeiro diz o sentido do giro. Isso se chama quadratura.

Como ler: as duas ondas têm o mesmo ritmo, mas B está atrasada um quarto de ciclo. Nas linhas âmbar, A subiu primeiro: a roda gira para a frente. Se a roda inverter, B passa a mudar primeiro. Contar cada borda de A (subida e descida) dobra a resolução sem custo.

Objetivo: contar os pulsos das duas rodas com interrupção e mostrar a rotação em RPM a cada 200 ms. Encoders: esquerdo A 18 / B 19, direito A 34 / B 35 (34 e 35 não têm pull-up interno; os módulos Hall do N20 costumam ter).

// Dois encoders em quadratura com interrupção (pacote esp32 3.x)
const int ENC_E_A = 18, ENC_E_B = 19;      // roda esquerda
const int ENC_D_A = 34, ENC_D_B = 35;      // roda direita
const float PULSOS_VOLTA = 1400;          // N20 1:100 contando as 2 bordas de A. Meça o seu (tópico 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++; }  // espelhado

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 prontos: gire as rodas com a mao");
}

void loop() {
  static long antE = 0, antD = 0;
  delay(200);
  noInterrupts(); long e = pulsosE, d = pulsosD; interrupts();   // cópia sem ser interrompido
  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);
}

Por que aprender

Sem encoder, "andar 1 metro" vira "ligar o motor por 2 segundos e torcer". Com ele, você mede velocidade real (a entrada do PID do 2.3) e distância percorrida (a base da odometria do próximo tópico).

Conceitos-chave

Odometria é estimar a posição somando pequenos deslocamentos. A cada 50 ms você pega quanto cada roda andou (pulsos ÷ pulsos por volta × perímetro da roda). A média das duas é quanto o centro do robô avançou; a diferença entre elas, dividida pela distância entre as rodas, é quanto ele girou. Com isso você atualiza x, y e θ (a direção, em radianos).

Como ler: a seta azul é a frente do robô, que aponta na direção θ medida a partir do eixo x. A régua âmbar L é a distância entre as rodas. As cinco linhas do quadro são o algoritmo inteiro: se a direita andou mais que a esquerda, Δθ é positivo e o robô virou para a esquerda.

Objetivo: estimar x, y (em centímetros) e θ (em graus) empurrando o robô com a mão. Mesmos pinos do tópico 4. Ajuste o raio da roda e a distância entre rodas do seu chassi.

// Odometria de robô diferencial com dois encoders (pacote esp32 3.x)
const int ENC_E_A = 18, ENC_E_B = 19, ENC_D_A = 34, ENC_D_B = 35;
const float PULSOS_VOLTA = 1400;           // meça o seu (tópico 6)
const float RAIO = 0.0215;                 // raio da roda em metros (roda de 43 mm)
const float BASE = 0.130;                  // distância entre as rodas em metros
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);                                  // passo de 50 ms
  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));   // mantém entre -180° e 180°
  if (millis() - ultimoPrint >= 250) {
    ultimoPrint = millis();
    Serial.printf("x=%.1f cm  y=%.1f cm  theta=%.1f graus\n", x * 100, y * 100, theta * 180 / PI);
  }
}

Por que aprender

Voltar à base, andar até um ponto, desenhar um quadrado no chão: tudo isso precisa de x, y e θ. É exatamente o que o micro-ROS do 4.3 publica como odometria para o ROS 2 montar mapas.

Conceitos-chave

A odometria nunca está certa, só "quase certa". Cada passo carrega um erro pequeno: a roda escorregou, o diâmetro real é 1 mm maior, um pulso se perdeu. Como a posição é a soma de todos os passos, esses erros se acumulam, e o erro de direção é o pior: 2° errados no começo viram 3,5 cm de desvio a cada metro andado. O jeito de conviver é calibrar bem e corrigir de vez em quando com algo que não deriva.

Como ler: o quadrado azul é o que o robô fez de verdade; a linha ciano é o que a odometria somou. No primeiro lado quase não há diferença. A cada curva, um erro pequeno de ângulo entorta todos os lados seguintes. Por isso o teste de calibração usa voltas completas: o erro aparece grande e fácil de medir.

Calibração em 4 passos

Por que aprender

Saber que a posição "escorrega" muda o jeito de projetar: tarefas curtas confiam na odometria; tarefas longas precisam de referência externa, como a linha no chão do 2.6, a câmera da Trilha 3 ou o LiDAR com SLAM da Trilha 4, que existe justamente para corrigir esse erro.

Conceitos-chave