MY FIRST COBOT

 /*

COPY IT IN ARDUINO PROGRAM   

TITULO: mi primer cobot grado septimo

   AUTOR: Clase de Tecnología

   

   OBJETIVO:

   Crear un brazo robótico seguro. Si el sensor "ve" una mano cerca,

   el robot se congela por seguridad. Cuando la mano se quita, sigue su trabajo.

   

   HARDWARE:

   - ESP32

   - Controlador de Servos PCA9685

   - Sensor de distancia HC-SR04

   - 3 Servomotores (Base, Hombro, Codo)

*/

#include <Wire.h>

#include <Adafruit_PWMServoDriver.h>

// --- 1. CONFIGURACIÓN DEL ROBOT ---

// Creamos el objeto para controlar los servos

Adafruit_PWMServoDriver tarjetaServos = Adafruit_PWMServoDriver();

// Pines del Sensor de Ultrasonido (Los ojos del robot)

const int PIN_TRIG = 5;

const int PIN_ECHO = 18;

// Configuración de los Servos

#define SERVO_MIN  150  // Pulso para 0 grados (ajustar si es necesario)

#define SERVO_MAX  600  // Pulso para 180 grados

const int VELOCIDAD = 30; // Entre más alto el número, más lento se mueve (tiempo en milisegundos)

const int DISTANCIA_SEGURIDAD = 15; // Centímetros para activar el freno de emergencia

// Variables para recordar dónde están los servos (Memoria del robot)

int posServo0 = 90; // Servo Base

int posServo1 = 90; // Servo Hombro

int posServo2 = 90; // Servo Codo

void setup() {

  Serial.begin(115200);

  Serial.println(">>> INICIANDO MI PRIMER COBOT <<<");

  // Configurar pines del sensor

  pinMode(PIN_TRIG, OUTPUT);

  pinMode(PIN_ECHO, INPUT);

  // Iniciar la tarjeta de servos

  tarjetaServos.begin();

  tarjetaServos.setPWMFreq(50); // Los servos funcionan a 50Hz

 

  // Ponemos el robot en posición inicial (90 grados todos)

  moverServoSuave(0, 90);

  moverServoSuave(1, 90);

  moverServoSuave(2, 90);

 

  delay(1000);

}

void loop() {

  // --- RUTINA DE MOVIMIENTO DEL ROBOT ---

  // Aquí escribimos la coreografía que queremos que haga el robot.

 

  Serial.println("Moviendo a la izquierda...");

  moverServoSuave(0, 10); // Mover Servo 0 (Base) a 10 grados

 

  Serial.println("Levantando el brazo...");

  moverServoSuave(1, 45); // Mover Servo 1 (Hombro) a 45 grados

 

  Serial.println("Saludando...");

  moverServoSuave(2, 130); // Mover Servo 2 (Codo)

  moverServoSuave(2, 50);

 

  Serial.println("Volviendo al centro...");

  moverServoSuave(0, 90);

  moverServoSuave(1, 90);

 

  delay(1000); // Descansa un segundo antes de repetir todo

}

// -----------------------------------------------------------------

//      ZONA DE FUNCIONES (LAS HERRAMIENTAS DEL ROBOT)

// -----------------------------------------------------------------

/* FUNCIÓN MAESTRA: moverServoSuave

   Esta función es la magia. Mueve el servo pasito a pasito.

   Antes de dar cada paso, verifica si es seguro avanzar.

*/

void moverServoSuave(int numeroServo, int anguloDestino) {

 

  // 1. Averiguar dónde está el servo ahora mismo

  int anguloActual = obtenerPosicionActual(numeroServo);

 

  // 2. Calcular si tenemos que sumar o restar grados para llegar

  int paso = 0;

  if (anguloActual < anguloDestino) {

    paso = 1;  // Vamos subiendo (0 -> 180)

  } else {

    paso = -1; // Vamos bajando (180 -> 0)

  }

  // 3. Bucle: Caminar grado por grado hasta llegar

  while (anguloActual != anguloDestino) {

   

    // --- PROTOCOLO DE SEGURIDAD ---

    // Antes de moverme, reviso el sensor

    revisarSeguridad();

    // Si la función de arriba detecta algo, se quedará pausada ahí dentro.

    // Si sale de esa función, significa que el camino está libre.

   

    // Avanzamos un grado

    anguloActual = anguloActual + paso;

   

    // Enviamos la orden al motor

    int pulso = map(anguloActual, 0, 180, SERVO_MIN, SERVO_MAX);

    tarjetaServos.setPWM(numeroServo, 0, pulso);

   

    // Actualizamos la memoria del robot

    guardarPosicion(numeroServo, anguloActual);

   

    // Esperamos un poco para que el movimiento sea suave y visible

    delay(VELOCIDAD);

  }

}

/*

   FUNCIÓN: revisarSeguridad

   Mide la distancia. Si hay algo cerca, entra en un bucle infinito (pausa)

   hasta que el objeto se quite.

*/

void revisarSeguridad() {

  float distancia = medirDistancia();

 

  // Si la distancia es menor a la seguridad (ej: 15cm)

  if (distancia > 0 && distancia < DISTANCIA_SEGURIDAD) {

   

    Serial.println("¡ALERTA! Objeto detectado. Parada de emergencia.");

   

    // MIENTRAS (While) el objeto siga ahí, no hacemos nada.

    while (distancia > 0 && distancia < DISTANCIA_SEGURIDAD) {

      // Volvemos a medir para saber si ya se fue

      distancia = medirDistancia();

      delay(100); // Pequeña espera para no saturar

    }

   

    Serial.println("Zona despejada. Reanudando movimiento.");

  }

}

/*

   FUNCIÓN: medirDistancia

   Usa el sensor HC-SR04 para calcular cm.

*/

float medirDistancia() {

  digitalWrite(PIN_TRIG, LOW);

  delayMicroseconds(2);

  digitalWrite(PIN_TRIG, HIGH);

  delayMicroseconds(10);

  digitalWrite(PIN_TRIG, LOW);

 

  long duracion = pulseIn(PIN_ECHO, HIGH);

  float distanciaCm = duracion * 0.034 / 2;

  return distanciaCm;

}

/*

   AYUDAS DE MEMORIA

   Como el PCA9685 no sabe leer dónde están los servos,

   usamos estas funciones para recordar las posiciones en nuestras variables.

*/

int obtenerPosicionActual(int servo) {

  if (servo == 0) return posServo0;

  if (servo == 1) return posServo1;

  if (servo == 2) return posServo2;

  return 90; // Valor por defecto

}

void guardarPosicion(int servo, int angulo) {

  if (servo == 0) posServo0 = angulo;

  if (servo == 1) posServo1 = angulo;

  if (servo == 2) posServo2 = angulo;

}