Brazo Robótico

// Brazo robotico — Nivel Inicial (Arduino UNO).
//
// CALIBRACION: los 6 servomotores se llevan a 90 grados (posicion central).
// Es el punto de partida obligatorio del armado: con todos los servos a 90 el
// brazo puede girar en ambos sentidos por igual, tal como pide el Paso 1 de la
// guia ("asegurarnos de que la posicion de los servomotores sea de 90").
//
// Plataforma: Arduino UNO con IO Shield de DFRobot (el shield expone los pines
// de servo con alimentacion; no cambia el numero de pin). Proyecto de BAJA
// TENSION (servos 5 V): no hay 220 V ni agua.
//
// ATENCION — ALIMENTACION: seis servos consumen demasiada corriente para el
// puerto USB. Es OBLIGATORIO usar la FUENTE EXTERNA 5V 2A con GND comun a la
// placa. Alimentar por USB puede resetear el Arduino o danar el regulador.
//
// ATENCION — MECANICA: el brazo tiene 6 grados de libertad y al energizarse
// salta a 90 grados de golpe. Despejar la zona de barrido antes de conectar.
//
// Nivel inicial: se permite delay() (fiel a la progresion original).
//
// Basado en: Fragmento de codigo del Nivel Inicial, Paso 2 — Brazo Robotico,
// Saberes Digitales (INET/EDUCAR 2019).
// Desvios respecto del original (ver README):
//  (1) mBlock3 -> C++ limpio: se elimina el cruft del generador de bloques
//      (angle_rad/angle_deg, _delay/_loop) y los nombres servo_2..servo_7.
//  (2) Los seis servos se manejan con un arreglo indexado por constante, sin
//      numeros magicos en el cuerpo del programa.

#if defined(ESP32)
  #include <ESP32Servo.h>
#else
  #include <Servo.h>
#endif

// --- Servomotores (contrato UNO: pines digitales 2..7) ---
// Indice 0 = servo 1 (base) ... indice 5 = servo 6 (pinza).
const int CANT_SERVOS = 6;
const int PINES_SERVO[CANT_SERVOS] = {2, 3, 4, 5, 6, 7};

// Angulo de calibracion: centro del recorrido 0..180.
const int ANGULO_CENTRO = 90;

Servo servos[CANT_SERVOS];

void setup() {
  // Enganchar cada servo a su pin y llevarlo al centro. Al terminar, el brazo
  // queda calibrado y listo para el armado mecanico.
  for (int i = 0; i < CANT_SERVOS; i++) {
    servos[i].attach(PINES_SERVO[i]);
    servos[i].write(ANGULO_CENTRO);
  }
}

void loop() {
  // Calibracion estatica: no hay movimiento. Los seis servos sostienen los 90
  // grados; el loop queda vacio a proposito.
}

// Brazo robotico — Nivel Inicial (ESP32 DevKit v1).
//
// CALIBRACION: los 6 servomotores se llevan a 90 grados (posicion central),
// punto de partida del armado (Paso 1: "asegurarnos de que la posicion de los
// servomotores sea de 90"), para que el brazo pueda girar por igual en ambos
// sentidos.
//
// Plataforma: ESP32 (DevKit v1). Se usa la libreria ESP32Servo (la Servo.h
// clasica NO funciona en ESP32). El IO Shield de DFRobot es para UNO: en ESP32
// NO se usa shield, los servos se cablean directo a los GPIO (senal al GPIO,
// alimentacion a la fuente externa, GND comun).
//
// ATENCION — ALIMENTACION: seis servos NO se alimentan por USB. Es OBLIGATORIA
// la FUENTE EXTERNA 5V 2A con GND comun. El ESP32 trabaja a 3,3 V pero el pin
// de senal del servo tolera ese nivel; la potencia sale de la fuente de 5 V.
//
// ATENCION — MECANICA: 6 grados de libertad; al energizar, los servos saltan a
// 90 grados. Despejar la zona de barrido.
//
// Nivel inicial: se permite delay() (fiel a la progresion original).
//
// Basado en: Fragmento de codigo del Nivel Inicial, Paso 2 — Brazo Robotico,
// Saberes Digitales (INET/EDUCAR 2019).
// Desvios (ver README): mBlock3 -> C++ limpio (sin angle_rad/deg, sin
// _delay/_loop, sin servo_2..servo_7); GPIO seguros del contrato; sin shield.

#if defined(ESP32)
  #include <ESP32Servo.h>
#else
  #include <Servo.h>
#endif

// --- Servomotores (contrato ESP32: GPIO 13,14,25,26,27,32 con ESP32Servo) ---
// Indice 0 = servo 1 (base) ... indice 5 = servo 6 (pinza).
const int CANT_SERVOS = 6;
const int PINES_SERVO[CANT_SERVOS] = {13, 14, 25, 26, 27, 32};

const int ANGULO_CENTRO = 90;

Servo servos[CANT_SERVOS];

void setup() {
  for (int i = 0; i < CANT_SERVOS; i++) {
    servos[i].attach(PINES_SERVO[i]);
    servos[i].write(ANGULO_CENTRO);
  }
}

void loop() {
  // Calibracion estatica: los seis servos sostienen los 90 grados.
}

// Brazo robotico — Nivel Intermedio (Arduino UNO).
//
// MANDO A DISTANCIA: seis potenciometros comandan los seis servomotores, uno
// por articulacion. Cada potenciometro actua como perilla de posicion: al
// girarlo, el servo correspondiente sigue su angulo. Es el "control remoto"
// que pide la situacion-problema (manipular sustancias peligrosas sin contacto).
//
// Plataforma: Arduino UNO con IO Shield de DFRobot. Las entradas analogicas
// A0..A5 leen los potenciometros; los pines 2..7 mandan los servos.
//
// ATENCION — ALIMENTACION: FUENTE EXTERNA 5V 2A obligatoria (6 servos no van
// por USB), GND comun. ATENCION — MECANICA: 6 grados de libertad; al mover los
// potenciometros el brazo puede golpear. Despejar la zona.
//
// Adaptacion de escala (regla de tres del Paso 3): el ADC del UNO entrega
//   0..1023 (10 bits) y el servo acepta 0..180 grados. Angulo = lectura*180/1023.
//
// Nivel intermedio: se permite delay() para dar una cadencia de refresco suave.
//
// Basado en: Fragmentos de codigo del Nivel Intermedio, Pasos 3 a 5 — Brazo
// Robotico, Saberes Digitales (INET/EDUCAR 2019).
// Desvios (ver README):
//  (1) mBlock3 -> C++ limpio (sin angle_rad/deg, sin _delay/_loop, sin
//      servo_2..servo_7): arreglos indexados y una funcion de mapeo reutilizable.
//  (2) El original arrancaba tres potenciometros y despues sumaba tres mas; aca
//      se entregan los SEIS desde el inicio (es el estado final del Paso 5).

#if defined(ESP32)
  #include <ESP32Servo.h>
#else
  #include <Servo.h>
#endif

// --- Servomotores (contrato UNO: pines digitales 2..7) ---
const int CANT_SERVOS = 6;
const int PINES_SERVO[CANT_SERVOS] = {2, 3, 4, 5, 6, 7};

// --- Potenciometros (contrato UNO: entradas analogicas A0..A5) ---
// PIN_POT[i] controla PINES_SERVO[i] (misma articulacion).
const int PINES_POT[CANT_SERVOS] = {A0, A1, A2, A3, A4, A5};

// Escala del ADC del UNO: 10 bits -> valor maximo 1023.
const long ADC_MAX = 1023;
const int ANGULO_MAX = 180;

// Cadencia de refresco (ms). Suaviza la lectura sin bloquear percepcion.
const unsigned long REFRESCO_MS = 15;

Servo servos[CANT_SERVOS];

// Convierte una lectura del ADC (0..ADC_MAX) al angulo del servo (0..180)
// con la regla de tres del original. Se usa long para no desbordar en el
// producto lectura*180 antes de dividir.
int lecturaAAngulo(int lectura) {
  return (int)(((long)lectura * ANGULO_MAX) / ADC_MAX);
}

void setup() {
  for (int i = 0; i < CANT_SERVOS; i++) {
    pinMode(PINES_POT[i], INPUT);
    servos[i].attach(PINES_SERVO[i]);
    // Arranque seguro: centrar cada servo antes de tomar el mando del pote.
    servos[i].write(90);
  }
}

void loop() {
  // Leer los seis potenciometros y reflejar cada uno en su servo.
  for (int i = 0; i < CANT_SERVOS; i++) {
    int lectura = analogRead(PINES_POT[i]);
    servos[i].write(lecturaAAngulo(lectura));
  }
  delay(REFRESCO_MS);
}

// Brazo robotico — Nivel Intermedio (ESP32 DevKit v1).
//
// MANDO A DISTANCIA: seis potenciometros comandan los seis servomotores, uno
// por articulacion (el "control remoto" de la situacion-problema).
//
// Plataforma: ESP32 (DevKit v1), servos con ESP32Servo, SIN shield (cableado
// directo). El mapa de pines del ESP32 es AJUSTADO: 6 servos + 6 entradas
// analogicas es lo maximo que entra sin colisiones (ver README, tabla ESP32).
//
// Mapa resuelto SIN COLISIONES:
//   Servos  -> GPIO 13, 14, 25, 26, 27, 32
//   Pots    -> GPIO 36, 39, 34, 35 (ADC1 solo-entrada) + GPIO 33 (ADC1) + GPIO 4
//   * GPIO 32 es SERVO, no pote: no puede ser las dos cosas. Por eso el pote que
//     el contrato ubicaba en 32 se reubica a GPIO 4.
//   * GPIO 4 es ADC2. El ADC2 solo entra en conflicto cuando el WiFi esta
//     activo; este proyecto NO tiene IoT/WiFi, asi que GPIO 4 lee sin problemas.
//
// ATENCION — ALIMENTACION: FUENTE EXTERNA 5V 2A obligatoria (6 servos), GND
// comun. Los potenciometros se alimentan a 3,3 V (no a 5 V): el ADC del ESP32
// mide 0..3,3 V; con 5 V se satura la lectura y se dana el pin.
// ATENCION — MECANICA: 6 grados de libertad; al mover los potes el brazo puede
// golpear. Despejar la zona.
//
// Adaptacion de escala: el ADC del ESP32 entrega 0..4095 (12 bits, contra los
// 10 bits / 0..1023 del UNO). Angulo = lectura*180/4095.
//
// Nivel intermedio: se permite delay() para una cadencia de refresco suave.
//
// Basado en: Fragmentos de codigo del Nivel Intermedio, Pasos 3 a 5 — Brazo
// Robotico, Saberes Digitales (INET/EDUCAR 2019).
// Desvios (ver README): mBlock3 -> C++ limpio; GPIO seguros; ADC de 12 bits
// (÷4095 en vez de ÷1023); reubicacion del pote 6 a GPIO 4 para evitar colision
// con el servo 6 (GPIO 32); los seis potes desde el inicio (estado final Paso 5).

#if defined(ESP32)
  #include <ESP32Servo.h>
#else
  #include <Servo.h>
#endif

// --- Servomotores (contrato ESP32) ---
const int CANT_SERVOS = 6;
const int PINES_SERVO[CANT_SERVOS] = {13, 14, 25, 26, 27, 32};

// --- Potenciometros (mapa ESP32 resuelto sin colisiones) ---
// PINES_POT[i] controla PINES_SERVO[i].
//   36,39,34,35 = ADC1 solo-entrada | 33 = ADC1 | 4 = ADC2 (libre sin WiFi)
const int PINES_POT[CANT_SERVOS] = {36, 39, 34, 35, 33, 4};

// Escala del ADC del ESP32: 12 bits -> valor maximo 4095.
const long ADC_MAX = 4095;
const int ANGULO_MAX = 180;

const unsigned long REFRESCO_MS = 15;

Servo servos[CANT_SERVOS];

// Regla de tres: lectura del ADC (0..ADC_MAX) -> angulo del servo (0..180).
// long evita el desborde del producto lectura*180 antes de dividir.
int lecturaAAngulo(int lectura) {
  return (int)(((long)lectura * ANGULO_MAX) / ADC_MAX);
}

void setup() {
  for (int i = 0; i < CANT_SERVOS; i++) {
    pinMode(PINES_POT[i], INPUT);
    servos[i].attach(PINES_SERVO[i]);
    servos[i].write(90);  // arranque seguro: centrar antes de dar el mando
  }
}

void loop() {
  for (int i = 0; i < CANT_SERVOS; i++) {
    int lectura = analogRead(PINES_POT[i]);
    servos[i].write(lecturaAAngulo(lectura));
  }
  delay(REFRESCO_MS);
}

// Brazo robotico — Nivel Avanzado (Arduino UNO).
//
// SECUENCIA PICK-AND-PLACE: el brazo repite un ciclo automatico que simula
// tomar un objeto de una cinta transportadora y depositarlo en otra ubicada a
// 90 grados y a menor altura (control de calidad de la situacion-problema).
// Sin potenciometros: el movimiento es programado.
//
// Articulaciones (indice del arreglo):
//   0 = servo 1 (base, pin 2)     3 = servo 4 (pin 5)
//   1 = servo 2 (pin 3)           4 = servo 5 (pin 6)
//   2 = servo 3 (pin 4)           5 = servo 6 (PINZA, pin 7)
//
// Plataforma: Arduino UNO con IO Shield DFRobot.
// ATENCION — ALIMENTACION: FUENTE EXTERNA 5V 2A obligatoria, GND comun.
// ATENCION — MECANICA: 6 grados de libertad en movimiento AUTONOMO y continuo;
// el brazo barre un area amplia y puede golpear. Despejar la zona y tener a
// mano el corte de alimentacion antes de energizar.
//
// Nivel avanzado: NO-BLOQUEANTE. En vez de encadenar delay() (como el original
// mBlock3, que usaba _delay), la secuencia avanza con una maquina de estados
// basada en millis(). Asi el loop() nunca se congela y el patron queda listo
// para sumarle, en el futuro, sensores o paradas de emergencia sin reescribir.
//
// Basado en: Fragmento de codigo del Nivel Avanzado, Paso 2 — Brazo Robotico,
// Saberes Digitales (INET/EDUCAR 2019). Se conservan los angulos y el orden de
// movimientos del original.
// Desvios (ver README):
//  (1) mBlock3 -> C++ limpio (sin angle_rad/deg, sin _delay/_loop, sin
//      servo_2..servo_7).
//  (2) delay()/_delay() bloqueante -> maquina de estados con millis() (estandar
//      del nivel avanzado de la coleccion).
//  (3) La secuencia se declara como tabla de POSES (angulo de las 6
//      articulaciones + tiempo de espera), en vez de escrituras sueltas: mismo
//      resultado, mucho mas legible y facil de reordenar (extension sugerida en
//      el cierre del original).

#if defined(ESP32)
  #include <ESP32Servo.h>
#else
  #include <Servo.h>
#endif

// --- Servomotores (contrato UNO: pines digitales 2..7) ---
const int CANT_SERVOS = 6;
const int PINES_SERVO[CANT_SERVOS] = {2, 3, 4, 5, 6, 7};

Servo servos[CANT_SERVOS];

// Una POSE es el estado completo del brazo: el angulo de cada una de las 6
// articulaciones mas cuanto tiempo se sostiene antes de pasar a la siguiente.
struct Pose {
  int angulos[CANT_SERVOS];
  unsigned long espera_ms;
};

// Secuencia pick-and-place. La pose 0 es el HOME de arranque (todos a 90) y se
// ejecuta una sola vez; el ciclo productivo son las poses 1..9, que se repiten
// para siempre. Los angulos y tiempos son los del listado original del Paso 2.
const Pose SECUENCIA[] = {
  // {base, s2,  s3,  s4,  s5,  pinza}          espera
  {{ 90,  90,  90,  90,  90,  90}, 2000},  // 0 HOME: todos a 90, arranque estable
  {{  0,  90,  90, 130,  90,  90}, 1000},  // 1 girar la base hacia la cinta de origen
  {{  0,  90,  90,  90,  90,   0}, 1000},  // 2 bajar la muneca y abrir la pinza
  {{  0,  90,  90,  90,  90, 180}, 1000},  // 3 cerrar la pinza: toma el objeto
  {{  0,  90,  90, 130,  90, 180}, 1000},  // 4 levantar la muneca con el objeto
  {{180,  90,  90, 130, 180, 180}, 1000},  // 5 girar 180 hacia la cinta de destino
  {{180,  50,  40, 130, 180, 180}, 1000},  // 6 acomodar codo/hombro sobre el destino
  {{180,  50,  40,  90, 180, 180}, 1000},  // 7 descender a la cinta de destino
  {{180,  50,  40,  90, 180,   0}, 1000},  // 8 abrir la pinza: suelta el objeto
  {{ 90,  90,  90,  90,  90,   0}, 1000},  // 9 volver el cuerpo a 90 y reiniciar ciclo
};
const int CANT_POSES = sizeof(SECUENCIA) / sizeof(SECUENCIA[0]);

int poseActual = 0;
unsigned long marcaTiempo = 0;

// Escribe en los seis servos los angulos de la pose indicada.
void aplicarPose(int idx) {
  for (int i = 0; i < CANT_SERVOS; i++) {
    servos[i].write(SECUENCIA[idx].angulos[i]);
  }
}

void setup() {
  for (int i = 0; i < CANT_SERVOS; i++) {
    servos[i].attach(PINES_SERVO[i]);
  }
  // Arrancar en HOME y anotar el instante de inicio.
  poseActual = 0;
  aplicarPose(poseActual);
  marcaTiempo = millis();
}

void loop() {
  unsigned long ahora = millis();
  // Cuando se cumplio la espera de la pose actual, avanzar a la siguiente.
  if (ahora - marcaTiempo >= SECUENCIA[poseActual].espera_ms) {
    marcaTiempo = ahora;
    poseActual++;
    // Al terminar el ciclo, volver a la pose 1 (no se repite el HOME de arranque).
    if (poseActual >= CANT_POSES) {
      poseActual = 1;
    }
    aplicarPose(poseActual);
  }
}

// Brazo robotico — Nivel Avanzado (ESP32 DevKit v1).
//
// SECUENCIA PICK-AND-PLACE: el brazo repite un ciclo automatico que simula
// tomar un objeto de una cinta y depositarlo en otra a 90 grados y a menor
// altura (control de calidad de la situacion-problema). Sin potenciometros.
//
// Articulaciones (indice del arreglo):
//   0 = servo 1 (base, GPIO 13)   3 = servo 4 (GPIO 26)
//   1 = servo 2 (GPIO 14)         4 = servo 5 (GPIO 27)
//   2 = servo 3 (GPIO 25)         5 = servo 6 (PINZA, GPIO 32)
//
// Plataforma: ESP32 (DevKit v1), servos con ESP32Servo, SIN shield (cableado
// directo).
// ATENCION — ALIMENTACION: FUENTE EXTERNA 5V 2A obligatoria, GND comun.
// ATENCION — MECANICA: 6 grados de libertad en movimiento AUTONOMO y continuo;
// el brazo barre un area amplia. Despejar la zona y tener a mano el corte de
// alimentacion antes de energizar.
//
// Nivel avanzado: NO-BLOQUEANTE. La secuencia avanza con una maquina de estados
// basada en millis() (no con delay()/_delay encadenados como el original
// mBlock3), asi el loop() nunca se congela.
//
// Basado en: Fragmento de codigo del Nivel Avanzado, Paso 2 — Brazo Robotico,
// Saberes Digitales (INET/EDUCAR 2019). Se conservan los angulos y el orden de
// movimientos del original.
// Desvios (ver README): mBlock3 -> C++ limpio; GPIO seguros; delay bloqueante
// -> maquina de estados con millis(); secuencia declarada como tabla de POSES.

#if defined(ESP32)
  #include <ESP32Servo.h>
#else
  #include <Servo.h>
#endif

// --- Servomotores (contrato ESP32) ---
const int CANT_SERVOS = 6;
const int PINES_SERVO[CANT_SERVOS] = {13, 14, 25, 26, 27, 32};

Servo servos[CANT_SERVOS];

// Una POSE es el estado completo del brazo: angulo de las 6 articulaciones mas
// el tiempo que se sostiene antes de pasar a la siguiente.
struct Pose {
  int angulos[CANT_SERVOS];
  unsigned long espera_ms;
};

// Secuencia pick-and-place. Pose 0 = HOME de arranque (una sola vez); las poses
// 1..9 son el ciclo productivo que se repite. Angulos y tiempos del original.
const Pose SECUENCIA[] = {
  // {base, s2,  s3,  s4,  s5,  pinza}          espera
  {{ 90,  90,  90,  90,  90,  90}, 2000},  // 0 HOME: todos a 90, arranque estable
  {{  0,  90,  90, 130,  90,  90}, 1000},  // 1 girar la base hacia la cinta de origen
  {{  0,  90,  90,  90,  90,   0}, 1000},  // 2 bajar la muneca y abrir la pinza
  {{  0,  90,  90,  90,  90, 180}, 1000},  // 3 cerrar la pinza: toma el objeto
  {{  0,  90,  90, 130,  90, 180}, 1000},  // 4 levantar la muneca con el objeto
  {{180,  90,  90, 130, 180, 180}, 1000},  // 5 girar 180 hacia la cinta de destino
  {{180,  50,  40, 130, 180, 180}, 1000},  // 6 acomodar codo/hombro sobre el destino
  {{180,  50,  40,  90, 180, 180}, 1000},  // 7 descender a la cinta de destino
  {{180,  50,  40,  90, 180,   0}, 1000},  // 8 abrir la pinza: suelta el objeto
  {{ 90,  90,  90,  90,  90,   0}, 1000},  // 9 volver el cuerpo a 90 y reiniciar ciclo
};
const int CANT_POSES = sizeof(SECUENCIA) / sizeof(SECUENCIA[0]);

int poseActual = 0;
unsigned long marcaTiempo = 0;

void aplicarPose(int idx) {
  for (int i = 0; i < CANT_SERVOS; i++) {
    servos[i].write(SECUENCIA[idx].angulos[i]);
  }
}

void setup() {
  for (int i = 0; i < CANT_SERVOS; i++) {
    servos[i].attach(PINES_SERVO[i]);
  }
  poseActual = 0;
  aplicarPose(poseActual);
  marcaTiempo = millis();
}

void loop() {
  unsigned long ahora = millis();
  if (ahora - marcaTiempo >= SECUENCIA[poseActual].espera_ms) {
    marcaTiempo = ahora;
    poseActual++;
    if (poseActual >= CANT_POSES) {
      poseActual = 1;  // no se repite el HOME de arranque
    }
    aplicarPose(poseActual);
  }
}