COBOT BIOFACTORY 7

 /* * LIBRERÍAS NECESARIAS:

 * 1. Wire.h (Para comunicación I2C)

 * 2. Adafruit_PWMServoDriver.h (Para controlar la tarjeta PCA9685)

 */

#include <Wire.h>

#include <Adafruit_PWMServoDriver.h>

// --- CONFIGURACIÓN DE HARDWARE ---

Adafruit_PWMServoDriver tarjetaServos = Adafruit_PWMServoDriver();

const int PIN_TRIG = 5;  // "Altavoz" del sensor ultrasónico

const int PIN_ECHO = 18; // "Oído" del sensor ultrasónico

// Ajustes para Servos SG90 (Pulsos de 50Hz)

#define SERVO_MIN  150  // Pulso para 0 grados

#define SERVO_MAX  600  // Pulso para 180 grados

// CONFIGURACIÓN DE SEGURIDAD

const int DISTANCIA_SEGURIDAD = 15; // "Escudo" de 15 cm

const int VELOCIDAD = 30;           // Suavidad de movimiento

// MEMORIA DEL COBOT (Propiocepción)

int posBase = 90;   // Servo 0

int posCodo = 90;   // Servo 1

int posHerramienta = 90; // Servo 2 (Picar/Mezclar/Recoger)

void setup() {

  Serial.begin(115200);

  pinMode(PIN_TRIG, OUTPUT);

  pinMode(PIN_ECHO, INPUT);

  

  tarjetaServos.begin();

  tarjetaServos.setPWMFreq(50); // Frecuencia para servos analógicos

  // Posición inicial de seguridad

  moverSuave(0, 90);

  moverSuave(1, 90);

  moverSuave(2, 90);

  delay(1000);

}

void loop() {

  

  // --- SELECCIONA TU MISIÓN AQUÍ (Descomenta solo una) ---

  // MISIÓN A: RECOLECTOR DE ESPORAS (Precisión)

  /*

  moverSuave(0, 45);  // Girar hacia la seta

  moverSuave(1, 140); // Bajar codo

  moverSuave(2, 40);  // Herramienta: Cerrar pinza/succión

  moverSuave(1, 90);  // Subir codo

  moverSuave(0, 90);  // Volver al centro

  */

  // MISIÓN B: MEZCLADOR DE NUTRIENTES (Ritmo)

  /*

  moverSuave(0, 80);  // Posicionar en el envase

  moverSuave(2, 40);  // Mezclar izquierda

  moverSuave(2, 140); // Mezclar derecha

  */

  // MISIÓN C: PICADOR DE ORELLANAS (Potencia)

  /*

  moverSuave(1, 40);  // Levantar cuchilla

  moverSuave(1, 120); // ¡Corte vertical!

  delay(500);

  */

  delay(1000); // Descanso entre ciclos

}

// --- FUNCIONES MAESTRAS (Herramientas del Sistema) ---

void moverSuave(int numServo, int anguloDestino) {

  int actual = obtenerPos(numServo);

  int paso = (actual < anguloDestino) ? 1 : -1;

  while (actual != anguloDestino) {

    // REVISAR SEGURIDAD ANTES DE CADA GRADO

    revisarSeguridad(); 

    actual += paso;

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

    tarjetaServos.setPWM(numServo, 0, pulso);

    

    guardarPos(numServo, actual);

    delay(VELOCIDAD);

  }

}

void revisarSeguridad() {

  float d = medirDistancia();

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

    Serial.println("¡STOP! Humano detectado en Zona Segura.");

    while (medirDistancia() < DISTANCIA_SEGURIDAD) {

      delay(100); // El robot se congela aquí

    }

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

  }

}

float medirDistancia() {

  digitalWrite(PIN_TRIG, LOW);

  delayMicroseconds(2);

  digitalWrite(PIN_TRIG, HIGH);

  delayMicroseconds(10);

  digitalWrite(PIN_TRIG, LOW);

  long tiempo = pulseIn(PIN_ECHO, HIGH);

  return tiempo * 0.034 / 2;

}

int obtenerPos(int s) {

  if (s == 0) return posBase;

  if (s == 1) return posCodo;

  return posHerramienta;

}

void guardarPos(int s, int a) {

  if (s == 0) posBase = a;

  else if (s == 1) posCodo = a;

  else posHerramienta = a;

}