ESP32 Educational Servo Arm Controller (3-DOF)

// ESP32 educational 3-DOF servo arm controller
// Uses separate 5-6 V servo supply, common ground, and constrained incremental motion.
#include <ESP32Servo.h>

const int SERVO_BASE_PIN = 13;
const int SERVO_SHOULDER_PIN = 16;
const int SERVO_ELBOW_PIN = 17;
const int JOY_BASE_PIN = 34;
const int JOY_SHOULDER_PIN = 35;
const int JOY_ELBOW_PIN = 36;

Servo baseServo;
Servo shoulderServo;
Servo elbowServo;

int baseAngle = 90;
int shoulderAngle = 90;
int elbowAngle = 90;
int baseCenter = 2048;
int shoulderCenter = 2048;
int elbowCenter = 2048;

const int BASE_MIN = 20;
const int BASE_MAX = 160;
const int SHOULDER_MIN = 35;
const int SHOULDER_MAX = 145;
const int ELBOW_MIN = 25;
const int ELBOW_MAX = 155;
const int DEAD_ZONE = 220;

int constrainBase(int a) { return constrain(a, BASE_MIN, BASE_MAX); }
int constrainShoulder(int a) { return constrain(a, SHOULDER_MIN, SHOULDER_MAX); }
int constrainElbow(int a) { return constrain(a, ELBOW_MIN, ELBOW_MAX); }

void writeServos() {
  baseAngle = constrainBase(baseAngle);
  shoulderAngle = constrainShoulder(shoulderAngle);
  elbowAngle = constrainElbow(elbowAngle);
  baseServo.write(baseAngle);
  shoulderServo.write(shoulderAngle);
  elbowServo.write(elbowAngle);
}

int axisStep(int raw, int center) {
  if (raw < 20 || raw > 4075) return 0; // hold last safe position if disconnected or stuck
  int delta = raw - center;
  if (abs(delta) < DEAD_ZONE) return 0;
  return delta > 0 ? 1 : -1;
}

int readAverage(int pin) {
  long sum = 0;
  for (int i = 0; i < 16; i++) sum += analogRead(pin);
  return sum / 16;
}

void setup() {
  Serial.begin(115200);
  analogReadResolution(12);
  baseServo.attach(SERVO_BASE_PIN, 500, 2500);
  shoulderServo.attach(SERVO_SHOULDER_PIN, 500, 2500);
  elbowServo.attach(SERVO_ELBOW_PIN, 500, 2500);

  baseCenter = readAverage(JOY_BASE_PIN);
  shoulderCenter = readAverage(JOY_SHOULDER_PIN);
  elbowCenter = readAverage(JOY_ELBOW_PIN);

  for (int a = 60; a <= 90; a++) {
    baseAngle = constrainBase(a);
    shoulderAngle = constrainShoulder(a);
    elbowAngle = constrainElbow(a);
    writeServos();
    delay(20);
  }
  Serial.println("Servo arm ready. Keep fingers clear of pinch points.");
}

void loop() {
  baseAngle = constrainBase(baseAngle + axisStep(analogRead(JOY_BASE_PIN), baseCenter));
  shoulderAngle = constrainShoulder(shoulderAngle + axisStep(analogRead(JOY_SHOULDER_PIN), shoulderCenter));
  elbowAngle = constrainElbow(elbowAngle + axisStep(analogRead(JOY_ELBOW_PIN), elbowCenter));
  writeServos();
  Serial.printf("Base %d Shoulder %d Elbow %d\n", baseAngle, shoulderAngle, elbowAngle);
  delay(20);
}