SERVO PROGRAMMING CODE

 //Componente  Pin ESP32 Función Color Cable Servo

//Sensor HC-SR04 (Trig) GPIO 5  Disparador de señal -

//Sensor HC-SR04 (Echo) GPIO 18 Receptor de señal -

//Servo 1 (Base/Giro) GPIO 13 Movimiento Derecha/Izquierda  Amarillo/Naranja

//Servo 2 (Articulación)  GPIO 12 Movimiento Vertical (Saludo)  Amarillo/Naranja

//Servo 3 (Pinza) GPIO 14 Abrir/Cerrar Pinza  Amarillo/Naranja//

#include <ESP32Servo.h>

// Definición de pines para el sensor

const int TRIG_PIN = 5;

const int ECHO_PIN = 18;

// Definición de pines para los 3 Servos

const int SERVO1_PIN = 13; // Base

const int SERVO2_PIN = 12; // Brazo (Saludo)

const int SERVO3_PIN = 14; // Pinza (Gripper)

// Crear objetos para los servos

Servo baseServo;

Servo armServo;

Servo gripperServo;

// Variables para el sensor

long duration;

int distance;

const int safetyDistance = 30; // Distancia de seguridad en cm

void setup() {

  Serial.begin(115200);

 

  pinMode(TRIG_PIN, OUTPUT);

  pinMode(ECHO_PIN, INPUT);

 

  // Configurar temporizadores para ESP32 (soporta hasta 16 canales PWM)

  ESP32PWM::allocateTimer(0);

  ESP32PWM::allocateTimer(1);

  ESP32PWM::allocateTimer(2);

 

  baseServo.setPeriodHertz(50);

  armServo.setPeriodHertz(50);

  gripperServo.setPeriodHertz(50);

 

  // Adjuntar servos a los pines

  baseServo.attach(SERVO1_PIN, 500, 2400);

  armServo.attach(SERVO2_PIN, 500, 2400);

  gripperServo.attach(SERVO3_PIN, 500, 2400);

  Serial.println("Sistema Cobot Inicializado con Seguridad activa...");

}

// Función para medir la distancia actual

void checkDistance() {

  digitalWrite(TRIG_PIN, LOW);

  delayMicroseconds(2);

  digitalWrite(TRIG_PIN, HIGH);

  delayMicroseconds(10);

  digitalWrite(TRIG_PIN, LOW);

 

  duration = pulseIn(ECHO_PIN, HIGH);

  distance = duration * 0.034 / 2;

}

// Función de movimiento seguro: Verifica el sensor antes de mover

bool safeMove(Servo &s, int angle, int waitTime) {

  checkDistance();

  if (distance > 0 && distance < safetyDistance) {

    Serial.println("ALERTA: Obstáculo detectado. Motores bloqueados.");

    return false; // Detiene la ejecución del paso actual

  }

  s.write(angle);

  delay(waitTime);

  return true;

}

void loop() {

  checkDistance();

  // Solo si el camino está despejado

  if (distance > safetyDistance || distance == 0) {

   

    // --- 1. MOVIMIENTO DE LA BASE (SERVO 1) ---

    if (!safeMove(baseServo, 180, 1000)) return; // 90° Derecha

    if (!safeMove(baseServo, 0, 1000))   return; // 90° Izquierda

    if (!safeMove(baseServo, 90, 5000))  return; // Centro y espera 5 segundos

   

    // --- 2. SALUDO VERTICAL (SERVO 2) ---

    if (!safeMove(armServo, 180, 600))   return; // Arriba

    if (!safeMove(armServo, 90, 600))    return; // Centro

    if (!safeMove(armServo, 0, 600))     return; // Abajo

    if (!safeMove(armServo, 90, 600))    return; // Centro

   

    // --- 3. ACCIÓN DE LA PINZA (SERVO 3) ---

    if (!safeMove(gripperServo, 180, 800)) return; // Abre Pinza

    if (!safeMove(gripperServo, 90, 800))  return; // Cierra Pinza (sujeta)

    if (!safeMove(gripperServo, 180, 800)) return; // Abre Pinza nuevamente

    if (!safeMove(gripperServo, 90, 500))  return; // Posición neutra

   

  } else {

    // Si hay alguien cerca, no hace nada y espera

    delay(200);

  }

}