Brazo Robotico

#include <Servo.h>

// Pines de servos
const int PIN_BASE = 9;
const int PIN_CODO = 10;
const int PIN_PINZA = 11;

// Pines de potenciometros
const int POT_BASE = A0;
const int POT_CODO = A1;
const int POT_PINZA = A2;

// Limites de movimiento
const int BASE_MIN = 0;
const int BASE_MAX = 180;
const int CODO_MIN = 30;    // Evitar golpear la base
const int CODO_MAX = 150;
const int PINZA_ABIERTA = 90;
const int PINZA_CERRADA = 30;

// Servos
Servo servoBase;
Servo servoCodo;
Servo servoPinza;

// Posiciones actuales
int posBase = 90;
int posCodo = 90;
int posPinza = 90;

void setup() {
  Serial.begin(9600);
  Serial.println("Brazo Robotico - EigenRobotics");

  servoBase.attach(PIN_BASE);
  servoCodo.attach(PIN_CODO);
  servoPinza.attach(PIN_PINZA);

  posicionInicial();
  delay(1000);
}

void loop() {
  // Leer potenciometros
  int lecturaBase = analogRead(POT_BASE);
  int lecturaCodo = analogRead(POT_CODO);
  int lecturaPinza = analogRead(POT_PINZA);

  // Mapear a angulos
  int objetivoBase = map(lecturaBase, 0, 1023, BASE_MIN, BASE_MAX);
  int objetivoCodo = map(lecturaCodo, 0, 1023, CODO_MIN, CODO_MAX);
  int objetivoPinza = map(lecturaPinza, 0, 1023, PINZA_ABIERTA, PINZA_CERRADA);

  // Mover suavemente
  posBase = moverSuave(posBase, objetivoBase, 2);
  posCodo = moverSuave(posCodo, objetivoCodo, 2);
  posPinza = moverSuave(posPinza, objetivoPinza, 3);

  // Aplicar posiciones
  servoBase.write(posBase);
  servoCodo.write(posCodo);
  servoPinza.write(posPinza);

  delay(20);
}

int moverSuave(int actual, int objetivo, int paso) {
  if (actual < objetivo) return min(actual + paso, objetivo);
  if (actual > objetivo) return max(actual - paso, objetivo);
  return actual;
}

void posicionInicial() {
  servoBase.write(90);
  servoCodo.write(90);
  servoPinza.write(PINZA_ABIERTA);
  posBase = 90;
  posCodo = 90;
  posPinza = PINZA_ABIERTA;
}