#include <Servo.

h>

// Demo_03
// Author: Ivan A. Calle
// Description: Control de posicion de dos servomotores
//
// NOTAS:
// - Similar a "Demo_03" pero este programa trabaja con el programa
// "Demo_03"

//-----------------------------------------------------------------//
// 1. CONFIGURACION INICIAL
//-----------------------------------------------------------------//
Servo my_servo_01; // Objeto "myservo" para manejar el servomotor
Servo my_servo_02; // Objeto "myservo" para manejar el servomotor
Servo my_servo_03; // Objeto "myservo" para manejar el servomotor
int my_servo_PIN_01 = 9; // Pin del Arduino para controlar el servomotor
int my_servo_PIN_02 = 10;
int my_servo_PIN_03 = 11; // Pin del Arduino para controlar el servomotor

//-----------------------------------------------------------------//
// 2. RUTINA DE CONFIGURACION
//-----------------------------------------------------------------//
void setup()
{

// CONFIGURACION DE LOS SERVOS

my_servo_01.attach(my_servo_PIN_01,525,2250); // Valores maximos para el TowerPRO
MG995
my_servo_01.write(0);

my_servo_02.attach(my_servo_PIN_02,525,2250); // Valores maximos para el TowerPRO

MG995
my_servo_02.write(130);

my_servo_03.attach(my_servo_PIN_03,525,2250); // Valores maximos para el

TowerPRO MG995
my_servo_03.write(70);

delay(1000);
Serial.begin(9600);
}

//-----------------------------------------------------------------//
// 3. RUTINA PRINCIPAL
//-----------------------------------------------------------------//
void loop()
{

// REENVIAMOS LA DATA ENVIADA DESDE LA PC SOLO CUANDO SEA NECESARIO

if(Serial.available()==3)
{
// A. LEEMOS EL BYTE ENVIADO
int theta1 = Serial.read(); // Se lee el valor uchar
int theta3= Serial.read(); // Se lee el valor uchar
 int theta4= Serial.read(); // Se lee el valor uchar
// C. ENVIAMOS EL ANGULO AL SERVO MOTOR
my_servo_01.write(theta1);
my_servo_02.write(theta3);
my_servo_03.write(theta4);
delay(20);
}
}