Controlador de servos

Neste exemplo imos controlar dous servos SG90. O primeiro vai tomar un ángulo 90º e o segundo 0º e un tempo despois ao revés, o primeiro un ángulo de 0º e o segundo de 90º.

Necesitamos dúas librerías: Wire para a comunicación I2C, e Adafruit PWMServoDriver para o controlador de servos.

// Controlador de servos 

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

Adafruit_PWMServoDriver controladorServo = Adafruit_PWMServoDriver(0x40); // enderezo I2C do controlador

// Valores para controlar un microservo SG90 (comentados os valores para un MG996R ou KS-3518)

int MIN_PULSE_WIDTH = 650 ;     // 500
int MAX_PULSE_WIDTH = 2600 ;    // 2400
int FREQUENCY =       60 ;      // 50

// Variables

uint8_t n_servos = 4; //Indicamos cantos servos están conectados
int tempo= 300;   

// configuración

void setup() {
  
  controladorServo.begin();               
  controladorServo.setPWMFreq(FREQUENCY);  

  for (uint8_t n = 0; n<n_servos; n++){       // poñemos todos os servos ao ángulo 0
    controladorServo.setPWM(n, 0, angulo(0));
  }
  
}

// para pasar a ángulo

int angulo(int angle){
  int pulse_wide, analog_value;
  pulse_wide   = map(angle, 0, 180, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
  analog_value = int(float(pulse_wide) / 1000000 * FREQUENCY * 4096);
  return analog_value;
}

// bucle

void loop() {      
  
  controladorServo.setPWM(0, 0, angulo(90)); 
  controladorServo.setPWM(1, 0, angulo(0));
  delay(tempo);
  controladorServo.setPWM(0, 0, angulo(0)); 
  controladorServo.setPWM(1, 0, angulo(90));
  delay(tempo);
 
}

Se queremos controlar varios servos de diferentes tipos imos ter valores máximos e mínimos diferentes.
Por exemplo, neste caso, os 5 primeiros servos son SG90 e os 2 último son servos MG996R

Os valores dos pulsos definímolos mediante arrays:

int servoPin[7]={0,1,2,3,4,9,8};
int minPulse[]={650,650,650,650,650,500,500};
int maxPulse[]={2600,2600,2600,2600,2600,2400,2400};
int FREQUENCY=60;

A función para calcular o ángulo establecémola cos datos anteriores:

int angulo(int angle, int servo){
  
  int pulse_wide, analog_value;
  pulse_wide   = map(angle, 0, 180, minPulse[servo], maxPulse[servo]);
  analog_value = int(float(pulse_wide) / 1000000 * FREQUENCY * 4096);
  return analog_value;
  
}

Para mover un dos servos a un ángulo determinado poderiamos usar esta función:

 controladorServo.setPWM(servoPin[servo], 0, angulo(sinal[servo], servo));

Adapta así o teu programa se vas usar diferentes tipos de servos.