Dual-Mode Robotic Arm

#include <Arduino.h>
#include <Servo.h>

// L298N: las dos salidas OUT1/OUT2 manejan una bobina y OUT3/OUT4 la otra.
const uint8_t L298_IN1 = 2;
const uint8_t L298_IN2 = 3;
const uint8_t L298_IN3 = 4;
const uint8_t L298_IN4 = 5;
const uint8_t MODE_BUTTON_PIN = 6;
const uint8_t CYCLE_BUTTON_PIN = 7;
const uint8_t HOME_SWITCH_PIN = 8;
const uint8_t ARM_SERVO_PIN = 9;
const uint8_t GRIPPER_SERVO_PIN = 10;
const uint8_t BASE_POT_PIN = A0;
const uint8_t ARM_POT_PIN = A1;
const uint8_t GRIPPER_POT_PIN = A2;

const unsigned long STEP_INTERVAL_MS = 8;
const unsigned long SERVO_SETTLE_MS = 650;
const unsigned long HOME_TIMEOUT_MS = 15000;
const int MANUAL_DEADBAND = 70;
const int AUTO_PICK_STEPS = 110;  // Ajustar a la geometria real del brazo.
const int AUTO_DROP_STEPS = 220;  // Debe ser el doble de AUTO_PICK_STEPS.

Servo servoBrazo;
Servo servoPinza;

const uint8_t coilPattern[4][4] = {
  {HIGH, LOW, HIGH, LOW},
  {LOW, HIGH, HIGH, LOW},
  {LOW, HIGH, LOW, HIGH},
  {HIGH, LOW, LOW, HIGH}
};

int phaseIndex = 0;
unsigned long lastStepMs = 0;
unsigned long lastButtonMs = 0;
bool automaticMode = false;
bool lastModeReading = HIGH;
bool lastCycleReading = HIGH;

// Estados del recorrido: busca origen, recoge, gira, deja y vuelve a origen.
enum AutoState {
  AUTO_IDLE,
  AUTO_HOME,
  AUTO_GO_PICK,
  AUTO_LOWER_PICK,
  AUTO_CLOSE_GRIPPER,
  AUTO_LIFT_PICK,
  AUTO_GO_DROP,
  AUTO_LOWER_DROP,
  AUTO_OPEN_GRIPPER,
  AUTO_LIFT_DROP,
  AUTO_RETURN_HOME
};

AutoState autoState = AUTO_IDLE;
int stepsRemaining = 0;
unsigned long stateStartedMs = 0;

void applyCoils() {
  digitalWrite(L298_IN1, coilPattern[phaseIndex][0]);
  digitalWrite(L298_IN2, coilPattern[phaseIndex][1]);
  digitalWrite(L298_IN3, coilPattern[phaseIndex][2]);
  digitalWrite(L298_IN4, coilPattern[phaseIndex][3]);
}

void stepBase(int direction) {
  phaseIndex += (direction > 0) ? 1 : -1;
  if (phaseIndex > 3) phaseIndex = 0;
  if (phaseIndex < 0) phaseIndex = 3;
  applyCoils();
}

bool timeForStep() {
  unsigned long now = millis();
  if (now - lastStepMs < STEP_INTERVAL_MS) return false;
  lastStepMs = now;
  return true;
}

bool homePressed() {
  // COM a GND y NO a D8: pulsado equivale a LOW.
  return digitalRead(HOME_SWITCH_PIN) == LOW;
}

void enterState(AutoState nextState) {
  autoState = nextState;
  stateStartedMs = millis();

  switch (autoState) {
    case AUTO_HOME:
      servoBrazo.write(70);
      servoPinza.write(25);       // pinza abierta
      break;
    case AUTO_GO_PICK:
      stepsRemaining = AUTO_PICK_STEPS;
      break;
    case AUTO_LOWER_PICK:
      servoBrazo.write(120);
      break;
    case AUTO_CLOSE_GRIPPER:
      servoPinza.write(78);       // cerrar sin forzar la pinza
      break;
    case AUTO_LIFT_PICK:
      servoBrazo.write(70);
      break;
    case AUTO_GO_DROP:
      stepsRemaining = AUTO_DROP_STEPS;
      break;
    case AUTO_LOWER_DROP:
      servoBrazo.write(118);
      break;
    case AUTO_OPEN_GRIPPER:
      servoPinza.write(25);
      break;
    case AUTO_LIFT_DROP:
      servoBrazo.write(70);
      break;
    case AUTO_RETURN_HOME:
      break;
    case AUTO_IDLE:
      break;
  }
}

void startAutomaticCycle() {
  enterState(AUTO_HOME);
}

void runManual() {
  int baseValue = analogRead(BASE_POT_PIN);
  int armAngle = map(analogRead(ARM_POT_PIN), 0, 1023, 20, 160);
  int gripperAngle = map(analogRead(GRIPPER_POT_PIN), 0, 1023, 20, 85);

  servoBrazo.write(armAngle);
  servoPinza.write(gripperAngle);

  int offset = baseValue - 512;
  if (abs(offset) > MANUAL_DEADBAND && timeForStep()) {
    stepBase(offset > 0 ? 1 : -1);
  }
}

bool runStepsForward() {
  if (stepsRemaining <= 0) return true;
  if (timeForStep()) {
    stepBase(1);
    --stepsRemaining;
  }
  return stepsRemaining <= 0;
}

void runAutomatic() {
  unsigned long elapsed = millis() - stateStartedMs;

  switch (autoState) {
    case AUTO_HOME:
      if (homePressed()) {
        phaseIndex = 0;
        applyCoils();
        enterState(AUTO_GO_PICK);
      } else if (elapsed >= HOME_TIMEOUT_MS) {
        // Si el interruptor no se alcanza, detiene el ciclo en vez de golpear el mecanismo.
        enterState(AUTO_IDLE);
      } else if (timeForStep()) {
        stepBase(-1);
      }
      break;

    case AUTO_GO_PICK:
      if (runStepsForward()) enterState(AUTO_LOWER_PICK);
      break;
    case AUTO_LOWER_PICK:
      if (elapsed >= SERVO_SETTLE_MS) enterState(AUTO_CLOSE_GRIPPER);
      break;
    case AUTO_CLOSE_GRIPPER:
      if (elapsed >= SERVO_SETTLE_MS) enterState(AUTO_LIFT_PICK);
      break;
    case AUTO_LIFT_PICK:
      if (elapsed >= SERVO_SETTLE_MS) enterState(AUTO_GO_DROP);
      break;
    case AUTO_GO_DROP:
      if (runStepsForward()) enterState(AUTO_LOWER_DROP);
      break;
    case AUTO_LOWER_DROP:
      if (elapsed >= SERVO_SETTLE_MS) enterState(AUTO_OPEN_GRIPPER);
      break;
    case AUTO_OPEN_GRIPPER:
      if (elapsed >= SERVO_SETTLE_MS) enterState(AUTO_LIFT_DROP);
      break;
    case AUTO_LIFT_DROP:
      if (elapsed >= SERVO_SETTLE_MS) enterState(AUTO_RETURN_HOME);
      break;
    case AUTO_RETURN_HOME:
      if (homePressed()) {
        enterState(AUTO_IDLE);
      } else if (elapsed >= HOME_TIMEOUT_MS) {
        enterState(AUTO_IDLE);
      } else if (timeForStep()) {
        stepBase(-1);
      }
      break;
    case AUTO_IDLE:
      break;
  }
}

void readButtons() {
  bool modeReading = digitalRead(MODE_BUTTON_PIN);
  bool cycleReading = digitalRead(CYCLE_BUTTON_PIN);
  unsigned long now = millis();

  if (now - lastButtonMs >= 40) {
    if (lastModeReading == HIGH && modeReading == LOW) {
      automaticMode = !automaticMode;
      if (!automaticMode) enterState(AUTO_IDLE);
      lastButtonMs = now;
    }

    if (lastCycleReading == HIGH && cycleReading == LOW && automaticMode && autoState == AUTO_IDLE) {
      startAutomaticCycle();
      lastButtonMs = now;
    }
  }

  lastModeReading = modeReading;
  lastCycleReading = cycleReading;
}

void setup() {
  pinMode(L298_IN1, OUTPUT);
  pinMode(L298_IN2, OUTPUT);
  pinMode(L298_IN3, OUTPUT);
  pinMode(L298_IN4, OUTPUT);
  pinMode(MODE_BUTTON_PIN, INPUT_PULLUP);
  pinMode(CYCLE_BUTTON_PIN, INPUT_PULLUP);
  pinMode(HOME_SWITCH_PIN, INPUT_PULLUP);

  servoBrazo.attach(ARM_SERVO_PIN);
  servoPinza.attach(GRIPPER_SERVO_PIN);
  servoBrazo.write(70);
  servoPinza.write(25);
  applyCoils();
}

void loop() {
  readButtons();

  if (automaticMode) {
    runAutomatic();
  } else {
    runManual();
  }

  delay(1);
}