ESP32 Specifications

                        ┌─────────┐
                        │  ESP32  │
                        │ DevKit  │
                        │   V1    │
                        └─────────┘

      Left Side                        Right Side
┌───────────────────┐          ┌───────────────────┐
│ 3.3V           ●  │          │  ● GND            │
│ EN             ●  │          │  ● GPIO 23 (MOSI) │
│ GPIO 36 (A0)   ●  │          │  ● GPIO 22 (SCL)  │
│ GPIO 39 (A3)   ●  │          │  ● GPIO 1  (TX0)  │
│ GPIO 34 (A6)   ●  │          │  ● GPIO 3  (RX0)  │
│ GPIO 35 (A7)   ●  │          │  ● GPIO 21 (SDA)  │
│ GPIO 32 (A4)   ●  │          │  ● GND            │
│ GPIO 33 (A5)   ●  │          │  ● GPIO 19 (MISO) │
│ GPIO 25 (DAC1) ●  │          │  ● GPIO 18 (SCK)  │
│ GPIO 26 (DAC2) ●  │          │  ● GPIO 5  (SS)   │
│ GPIO 27        ●  │          │  ● GPIO 17 (TX2)  │
│ GPIO 14        ●  │          │  ● GPIO 16 (RX2)  │
│ GPIO 12        ●  │          │  ● GPIO 4         │
│ GND            ●  │          │  ● GPIO 2  (LED)  │
│ GPIO 13        ●  │          │  ● GPIO 15        │
│ GPIO 9  (*)    ●  │          │  ● GND            │
│ GPIO 10 (*)    ●  │          │  ● VIN (5V)       │
│ GPIO 11 (*)    ●  │          │  ● 3.3V OUT       │
└───────────────────┘          └───────────────────┘

(*) GPIO 6-11 connected to internal flash - DO NOT USE

#define MOTOR_FL_PWM  4    // PWM speed control
#define MOTOR_FL_DIR1 23   // Direction pin 1
#define MOTOR_FL_DIR2 19   // Direction pin 2

#define MOTOR_FR_PWM  16   // PWM speed control
#define MOTOR_FR_DIR1 18   // Direction pin 1
#define MOTOR_FR_DIR2 5    // Direction pin 2

#define MOTOR_RL_PWM  17   // PWM speed control
#define MOTOR_RL_DIR1 22   // Direction pin 1 (Note: I2C conflict if used)
#define MOTOR_RL_DIR2 21   // Direction pin 2 (Note: I2C conflict if used)

#define MOTOR_RL_DIR1 26   // Use GPIO 26, 27 instead
#define MOTOR_RL_DIR2 27

#define MOTOR_RR_PWM  25   // PWM speed control
#define MOTOR_RR_DIR1 33   // Direction pin 1
#define MOTOR_RR_DIR2 32   // Direction pin 2

// Front-Left Encoder
#define ENCODER_FL_A 34    // Channel A (input-only)
#define ENCODER_FL_B 35    // Channel B (input-only)

// Front-Right Encoder
#define ENCODER_FR_A 36    // Channel A (input-only)
#define ENCODER_FR_B 39    // Channel B (input-only)

// Rear-Left Encoder
#define ENCODER_RL_A 13    // Channel A
#define ENCODER_RL_B 14    // Channel B

// Rear-Right Encoder
#define ENCODER_RR_A 15    // Channel A
#define ENCODER_RR_B 12    // Channel B (strapping pin - be careful)

#define IMU_SDA 21   // I2C Data
#define IMU_SCL 22   // I2C Clock

#define IMU_SDA 26   // Alternative SDA
#define IMU_SCL 27   // Alternative SCL

// In setup():
Wire.begin(IMU_SDA, IMU_SCL);

// Automatically configured by Serial
Serial.begin(115200);  // TX = GPIO 1, RX = GPIO 3

#define LED_BUILTIN 2   // Built-in LED on GPIO 2

// Blink pattern indicates status:
// - Fast blink: Waiting for serial connection
// - Slow blink: Normal operation
// - Solid on: Error state

// PWM Configuration
#define PWM_FREQ 20000      // 20 kHz (ultrasonic, silent)
#define PWM_RESOLUTION 10   // 10-bit (0-1023)

// Setup PWM for motor
void setupPWM(uint8_t pin, uint8_t channel) {
  ledcSetup(channel, PWM_FREQ, PWM_RESOLUTION);
  ledcAttachPin(pin, channel);
}

// Set motor speed
void setMotorSpeed(uint8_t channel, int speed) {
  // speed: -1023 to +1023
  int pwm_value = abs(speed);
  ledcWrite(channel, pwm_value);
}

// In setup():
setupPWM(MOTOR_FL_PWM, 0);  // Channel 0
setupPWM(MOTOR_FR_PWM, 1);  // Channel 1
setupPWM(MOTOR_RL_PWM, 2);  // Channel 2
setupPWM(MOTOR_RR_PWM, 3);  // Channel 3

volatile long encoder_count_FL = 0;

void IRAM_ATTR encoder_FL_ISR() {
  // IRAM_ATTR: Store in fast RAM for low latency
  int A = digitalRead(ENCODER_FL_A);
  int B = digitalRead(ENCODER_FL_B);

  if (A == B) {
    encoder_count_FL++;  // Forward
  } else {
    encoder_count_FL--;  // Backward
  }
}

void setup() {
  pinMode(ENCODER_FL_A, INPUT_PULLUP);
  pinMode(ENCODER_FL_B, INPUT_PULLUP);

  // Attach interrupt on channel A rising edge
  attachInterrupt(digitalPinToInterrupt(ENCODER_FL_A), encoder_FL_ISR, RISING);
}

#include <Wire.h>

// I2C Configuration
#define IMU_I2C_ADDRESS 0x68  // ICM-20948 default address

void setup() {
  // Initialize I2C with custom pins (if needed)
  Wire.begin(IMU_SDA, IMU_SCL);
  Wire.setClock(400000);  // 400 kHz fast mode

  // Check IMU connection
  Wire.beginTransmission(IMU_I2C_ADDRESS);
  if (Wire.endTransmission() == 0) {
    Serial.println("IMU found!");
  } else {
    Serial.println("IMU not found!");
  }
}

void loop() {
  // Read IMU data
  // (use library like SparkFun_ICM20948)
}

void setup() {
  Serial.begin(115200);  // Match baud rate on Pi side
  while (!Serial) {
    delay(10);  // Wait for serial connection (optional)
  }
}

void loop() {
  // Send feedback to Pi (50 Hz)
  if (millis() - last_feedback_time >= 20) {
    sendFeedback();
    last_feedback_time = millis();
  }

  // Receive commands from Pi
  if (Serial.available()) {
    String command = Serial.readStringUntil('\n');
    parseCommand(command);
  }
}

void sendFeedback() {
  // CSV format: pos1,pos2,pos3,pos4,vel1,vel2,vel3,vel4
  Serial.print(encoder_pos_FL); Serial.print(",");
  Serial.print(encoder_pos_FR); Serial.print(",");
  Serial.print(encoder_pos_RL); Serial.print(",");
  Serial.print(encoder_pos_RR); Serial.print(",");
  Serial.print(encoder_vel_FL); Serial.print(",");
  Serial.print(encoder_vel_FR); Serial.print(",");
  Serial.print(encoder_vel_RL); Serial.print(",");
  Serial.println(encoder_vel_RR);
}

; platformio.ini
[env:esp32dev]
platform = espressif32
board = esp32dev
framework = arduino
monitor_speed = 115200
upload_speed = 921600

lib_deps =
    Wire
    SPI
    # Add IMU library if needed