DC Motor Specifications

Full resolution = 4 × PPR = 4 × 209 = 836 counts per revolution
(4× from quadrature decoding: both edges of both channels)

Angular resolution = 360° / 836 = 0.43° per count
Linear resolution (60mm wheel) = π × 0.06 / 836 = 0.225 mm per count

Torque (Nm)    Speed (RPM)    Current (A)
0.0            333 (no-load)  0.15
0.2            300            0.5
0.4            270            0.8
0.6            240            1.2
0.8 (rated)    210            1.5
1.0 (stall)    0              2.5-3.0

                    ESP32 (3.3V/5V)
                          │
           ┌──────────────┼──────────────┐
           │              │              │
    Motor FL          Motor FR      Motor RL, RR...
           │              │              │
     ┌─────┴─────┐  ┌─────┴─────┐
     │   Encoder │  │   Encoder │
     │           │  │           │
     │ VCC  5V ──┼──┼── 5V Rail (Buck converter)
     │ A    →34  │  │   →36     GPIO (ESP32)
     │ B    →35  │  │   →39     GPIO
     │ GND  GND ─┼──┼── GND
     └───────────┘  └───────────┘

           │              │
     ┌─────┴─────┐  ┌─────┴─────┐
     │ H-Bridge  │  │ H-Bridge  │
     │  (IBT-2)  │  │  (IBT-2)  │
     │           │  │           │
     │ OUT1  →───┼──┼→ Motor +  (Red wire)
     │ OUT2  →───┼──┼→ Motor -  (Black wire)
     │ 12V  ─────┼──┼→ 12V (Battery)
     │ GND  ─────┼──┼→ GND
     │ PWM  ←ESP │  │   ←ESP    GPIO (e.g., 4, 16)
     │ DIR  ←ESP │  │   ←ESP    GPIO (e.g., 23, 18)
     └───────────┘  └───────────┘

// Option 1: Use both RPWM and LPWM (bidirectional control)
// Forward: RPWM = speed, LPWM = 0
// Backward: RPWM = 0, LPWM = speed

// Option 2: Use RPWM only + 2 direction pins
// Tie LPWM to GND
// Use 2 digital pins for H-bridge control (not standard IBT-2)

// Motor control pins
#define MOTOR_FL_PWM  4    // RPWM
#define MOTOR_FL_DIR1 23   // R_EN (HIGH = forward)
#define MOTOR_FL_DIR2 19   // L_EN (HIGH = reverse)

// Control logic:
void setMotorSpeed(int speed) {
  // speed: -255 to +255
  if (speed >= 0) {
    // Forward
    ledcWrite(MOTOR_FL_PWM_CHANNEL, speed);
    digitalWrite(MOTOR_FL_DIR1, HIGH);
    digitalWrite(MOTOR_FL_DIR2, LOW);
  } else {
    // Reverse
    ledcWrite(MOTOR_FL_PWM_CHANNEL, abs(speed));
    digitalWrite(MOTOR_FL_DIR1, LOW);
    digitalWrite(MOTOR_FL_DIR2, HIGH);
  }
}

Wheel circumference = π × D = 3.14159 × 0.06 = 0.188 m

Wheel RPS = robot_speed / circumference
          = 0.5 / 0.188 = 2.66 revolutions/second

Wheel RPM = 2.66 × 60 = 159 RPM

Motor RPM = 159 RPM (close to rated 250 RPM, good!)

Force per wheel = (mass × accel) / 4 wheels
                = (3 kg × 1 m/s²) / 4
                = 0.75 N

Torque = Force × radius
       = 0.75 N × 0.03 m
       = 0.0225 Nm

Add 50% safety margin = 0.034 Nm per motor

Power = Voltage × Current
      = 12V × 4A (typical) = 48W
      = 12V × 6A (peak) = 72W

Runtime = Battery_mAh / Current_mA
        = 5000 mAh / 4000 mA = 1.25 hours (theoretical)
        = ~45-60 minutes (realistic with Nav2 overhead)

Channel A: ___┌─┐___┌─┐___┌─┐___┌─┐
Channel B: _____┌─┐___┌─┐___┌─┐___┌─┐

Direction:
  Forward: A leads B (A rises before B)
  Reverse: B leads A (B rises before A)

// Encoder variables
volatile long encoder_count = 0;
volatile int last_encoded = 0;

// Encoder pins
#define ENCODER_A 34
#define ENCODER_B 35

// ISR (Interrupt Service Routine)
void IRAM_ATTR encoder_ISR() {
  int MSB = digitalRead(ENCODER_A);  // Most significant bit
  int LSB = digitalRead(ENCODER_B);  // Least significant bit

  int encoded = (MSB << 1) | LSB;  // Combine into 2-bit value
  int sum = (last_encoded << 2) | encoded;  // Combine with previous state

  // Determine direction based on state transition
  if (sum == 0b1101 || sum == 0b0100 || sum == 0b0010 || sum == 0b1011) {
    encoder_count++;  // Forward
  } else if (sum == 0b1110 || sum == 0b0111 || sum == 0b0001 || sum == 0b1000) {
    encoder_count--;  // Reverse
  }

  last_encoded = encoded;
}

void setup() {
  pinMode(ENCODER_A, INPUT_PULLUP);
  pinMode(ENCODER_B, INPUT_PULLUP);

  attachInterrupt(digitalPinToInterrupt(ENCODER_A), encoder_ISR, CHANGE);
  attachInterrupt(digitalPinToInterrupt(ENCODER_B), encoder_ISR, CHANGE);
}

void loop() {
  // Read encoder count
  long count = encoder_count;

  // Convert to angle (radians)
  float angle = (count / 836.0) * 2.0 * PI;  // 836 counts/rev for 4× resolution

  // Convert to distance (meters, 60mm wheel)
  float distance = (count / 836.0) * (PI * 0.06);

  Serial.printf("Count: %ld, Angle: %.2f rad, Distance: %.3f m\n", count, angle, distance);
  delay(100);
}

void testMotorDirection() {
  // Test each motor individually
  setMotorSpeed(MOTOR_FL, 50);  // 50% forward
  delay(2000);
  setMotorSpeed(MOTOR_FL, 0);   // Stop
  delay(1000);

  // If motor rotates backward when commanded forward:
  // Option 1: Swap motor wires (red ↔ black)
  // Option 2: Invert direction in software (multiply PWM by -1)
}

void testEncoderDirection() {
  encoder_count = 0;  // Reset

  setMotorSpeed(MOTOR_FL, 50);  // Forward
  delay(2000);
  setMotorSpeed(MOTOR_FL, 0);   // Stop

  Serial.print("Encoder count after forward motion: ");
  Serial.println(encoder_count);

  // Expected: Positive count (e.g., +500)
  // If negative: Swap encoder A ↔ B wires
}

float getMotorVelocity() {
  static long last_count = 0;
  static unsigned long last_time = 0;

  long current_count = encoder_count;
  unsigned long current_time = millis();

  long delta_count = current_count - last_count;
  float delta_time = (current_time - last_time) / 1000.0;  // seconds

  last_count = current_count;
  last_time = current_time;

  // Velocity in radians/second
  float velocity = (delta_count / 836.0) * (2.0 * PI) / delta_time;

  return velocity;
}