7 SERVO MOTOR CODE WEEK 3

 /*

   TITULO: cobot motores servo sg90

   GRADO: Septimo

   

   OBJETIVO:

   Aprender a controlar 3 motores servo usando un controlador externo (PCA9685).

   Vamos a decirle a cada motor a qué ángulo (grados) debe ir.

   

   CONEXIONES EN EL PCA9685:

   - Motor Base   -> Puerto 0

   - Motor Hombro -> Puerto 4

   - Motor Codo   -> Puerto 8

*/

#include <Wire.h>                  // Librería para comunicarse por los cables SDA/SCL

#include <Adafruit_PWMServoDriver.h> // Librería para manejar la tarjeta controladora

// --- 1. CONFIGURACIÓN ---

// Creamos el "objeto" que controla la tarjeta

Adafruit_PWMServoDriver tarjetaMotores = Adafruit_PWMServoDriver();

// Definimos los nombres de nuestros motores y dónde están conectados

// Es más fácil usar nombres que números sueltos

const int MOTOR_BASE = 0;   // Conectado en el huequito 0

const int MOTOR_HOMBRO = 4; // Conectado en el huequito 4

const int MOTOR_CODO = 8;   // Conectado en el huequito 8

// Valores técnicos para el Servo SG90 (No cambiar esto por ahora)

// Estos números ayudan a traducir "Grados" a "Electricidad"

const int PULSO_MIN = 150; // Equivale a 0 grados

const int PULSO_MAX = 600; // Equivale a 180 grados

void setup() {

  Serial.begin(115200); // Iniciamos el monitor serial para ver mensajes

  Serial.println(">>> INICIANDO SISTEMA COBOT <<<");

  tarjetaMotores.begin();  // Encendemos la tarjeta PCA9685

  tarjetaMotores.setPWMFreq(50); // Configuramos la velocidad de señal (50Hz es estándar para servos)

 

  delay(100);

}

void loop() {

  // --- ZONA DE PRUEBAS PARA EL ESTUDIANTE ---

  // Aquí puedes cambiar los números para mover tu robot

 

  Serial.println("--- PRUEBA 1: Todos al centro (90 grados) ---");

  moverMotor(MOTOR_BASE, 90);

  moverMotor(MOTOR_HOMBRO, 90);

  moverMotor(MOTOR_CODO, 90);

  delay(2000); // Esperamos 2 segundos para ver la posición

  Serial.println("--- PRUEBA 2: Posición de Saludo ---");

  moverMotor(MOTOR_BASE, 0);    // Base gira a la derecha

  moverMotor(MOTOR_HOMBRO, 45); // Hombro sube un poco

  moverMotor(MOTOR_CODO, 180);  // Codo totalmente estirado

  delay(2000);

  Serial.println("--- PRUEBA 3: Posición de descanso (0 grados) ---");

  moverMotor(MOTOR_BASE, 0);

  moverMotor(MOTOR_HOMBRO, 0);

  moverMotor(MOTOR_CODO, 0);

  delay(2000);

 

  // El loop se repite infinitamente, volviendo a la Prueba 1

}

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

// FUNCION AUXILIAR (La "Traductora")

// Esta parte del código convierte los grados humanos (0-180)

// a pulsos eléctricos que entiende la tarjeta.

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

void moverMotor(int numeroMotor, int grados) {

 

  // 1. Convertimos grados a pulsos

  int pulso = map(grados, 0, 180, PULSO_MIN, PULSO_MAX);

 

  // 2. Enviamos la orden a la tarjeta

  tarjetaMotores.setPWM(numeroMotor, 0, pulso);

 

  // 3. Imprimimos en pantalla para que el estudiante sepa qué pasa

  Serial.print("Moviendo Motor en puerto ");

  Serial.print(numeroMotor);

  Serial.print(" al angulo: ");

  Serial.println(grados);

}