• MCU: ESP32 DevKit V1 / ESP32-DevKitC-compatible
• Framework: Arduino via PlatformIO
• Libraries: Wire, WiFi, Adafruit PWM Servo Driver Library, Adafruit MPU6050

Ini
[env:esp32dev]
platform = espressif32
board = esp32dev
framework = arduino
monitor_speed = 115200
lib_deps =
  adafruit/Adafruit PWM Servo Driver Library
  adafruit/Adafruit MPU6050
  adafruit/Adafruit Unified Sensor

Cpp
#include <Arduino.h>
#include <Wire.h>
#include <WiFi.h>
#include <Adafruit_PWMServoDriver.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>

#define I2C_SDA_PIN 21
#define I2C_SCL_PIN 22
#define CAM_RX_PIN 16
#define CAM_TX_PIN 17
#define VERT_STEP_PIN 25
#define VERT_DIR_PIN 26
#define MOTOR_ENABLE_N_PIN 27
#define RING_STEP_PIN 32
#define RING_DIR_PIN 33
#define US_TRIG_PIN 18
#define US_ECHO_PIN 19
#define LIMIT_UPPER_N_PIN 34
#define LIMIT_LOWER_N_PIN 35
#define LED_READY_PIN 13
#define LED_FAULT_PIN 14
#define LED_HARVEST_PIN 15

#define PCA9685_ADDR 0x40
#define SERVO_ARM_CH 0
#define SERVO_SCISSOR_CH 1
#define SERVO_MIN_US 500
#define SERVO_MAX_US 2500
#define SERVO_FREQ_HZ 50

Adafruit_PWMServoDriver pwm(PCA9685_ADDR);
Adafruit_MPU6050 mpu;
HardwareSerial CamSerial(2);

enum RobotState { MOVE_HEIGHT, ROTATE_RING, SCAN_TARGET, EXTEND_ARM, OPEN_SCISSORS, CUT_PEPPER, DROP_TO_BAG, RETRACT_ARM, RETURN_HOME, FAULT };
RobotState state = RETURN_HOME;

void stepMotor(uint8_t stepPin, uint8_t dirPin, bool dir, uint16_t steps, uint16_t pulseUs) {
  digitalWrite(dirPin, dir ? HIGH : LOW);
  for (uint16_t i = 0; i < steps; i++) {
    digitalWrite(stepPin, HIGH); delayMicroseconds(pulseUs);
    digitalWrite(stepPin, LOW); delayMicroseconds(pulseUs);
  }
}

uint16_t servoPulseForAngle(uint8_t angle) {
  uint16_t us = map(angle, 0, 180, SERVO_MIN_US, SERVO_MAX_US);
  return (uint16_t)((float)us * 4096.0f * SERVO_FREQ_HZ / 1000000.0f);
}

void setServoAngle(uint8_t channel, uint8_t angle) {
  pwm.setPWM(channel, 0, servoPulseForAngle(angle));
}

float readUltrasonicCm() {
  digitalWrite(US_TRIG_PIN, LOW); delayMicroseconds(2);
  digitalWrite(US_TRIG_PIN, HIGH); delayMicroseconds(10);
  digitalWrite(US_TRIG_PIN, LOW);
  unsigned long duration = pulseIn(US_ECHO_PIN, HIGH, 30000UL);
  if (duration == 0) return -1.0f;
  return duration / 58.0f;
}

void enableMotors(bool en) { digitalWrite(MOTOR_ENABLE_N_PIN, en ? LOW : HIGH); }

void setup() {
  Serial.begin(115200);
  CamSerial.begin(115200, SERIAL_8N1, CAM_RX_PIN, CAM_TX_PIN);
  Wire.begin(I2C_SDA_PIN, I2C_SCL_PIN);

  pinMode(VERT_STEP_PIN, OUTPUT); pinMode(VERT_DIR_PIN, OUTPUT);
  pinMode(RING_STEP_PIN, OUTPUT); pinMode(RING_DIR_PIN, OUTPUT);
  pinMode(MOTOR_ENABLE_N_PIN, OUTPUT); enableMotors(false);
  pinMode(US_TRIG_PIN, OUTPUT); pinMode(US_ECHO_PIN, INPUT);
  pinMode(LIMIT_UPPER_N_PIN, INPUT); pinMode(LIMIT_LOWER_N_PIN, INPUT);
  pinMode(LED_READY_PIN, OUTPUT); pinMode(LED_FAULT_PIN, OUTPUT); pinMode(LED_HARVEST_PIN, OUTPUT);

  pwm.begin(); pwm.setPWMFreq(SERVO_FREQ_HZ);
  if (!mpu.begin(0x68, &Wire)) { Serial.println("MPU6050 not found"); state = FAULT; }
  digitalWrite(LED_READY_PIN, state != FAULT);
  enableMotors(state != FAULT);
}

void loop() {
  if (digitalRead(LIMIT_UPPER_N_PIN) == LOW && state == MOVE_HEIGHT) state = FAULT;

  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);
  float distanceCm = readUltrasonicCm();

  if (CamSerial.available()) {
    String msg = CamSerial.readStringUntil('\n');
    Serial.printf("CAM: %s\n", msg.c_str());
  }

  switch (state) {
    case RETURN_HOME:
      enableMotors(true);
      while (digitalRead(LIMIT_LOWER_N_PIN) == HIGH) stepMotor(VERT_STEP_PIN, VERT_DIR_PIN, false, 1, 800);
      state = MOVE_HEIGHT;
      break;
    case MOVE_HEIGHT: stepMotor(VERT_STEP_PIN, VERT_DIR_PIN, true, 200, 800); state = ROTATE_RING; break;
    case ROTATE_RING: stepMotor(RING_STEP_PIN, RING_DIR_PIN, true, 200, 800); state = SCAN_TARGET; break;
    case SCAN_TARGET: CamSerial.println("SCAN_TARGET"); state = EXTEND_ARM; break;
    case EXTEND_ARM: setServoAngle(SERVO_ARM_CH, 130); state = OPEN_SCISSORS; break;
    case OPEN_SCISSORS: setServoAngle(SERVO_SCISSOR_CH, 20); state = CUT_PEPPER; break;
    case CUT_PEPPER: setServoAngle(SERVO_SCISSOR_CH, 120); state = DROP_TO_BAG; break;
    case DROP_TO_BAG: delay(500); state = RETRACT_ARM; break;
    case RETRACT_ARM: setServoAngle(SERVO_ARM_CH, 30); state = RETURN_HOME; break;
    case FAULT:
    default:
      enableMotors(false); digitalWrite(LED_FAULT_PIN, HIGH); digitalWrite(LED_READY_PIN, LOW); delay(100); break;
  }
  digitalWrite(LED_HARVEST_PIN, state >= EXTEND_ARM && state <= RETRACT_ARM);
  Serial.printf("state=%d distance=%.1f ax=%.2f ay=%.2f az=%.2f\n", state, distanceCm, a.acceleration.x, a.acceleration.y, a.acceleration.z);
  delay(100);
}

• Build: pio run
• Upload: pio run -t upload
• Monitor: pio device monitor
• Hold BOOT and press EN on the ESP32 DevKit if the uploader cannot enter bootloader automatically.