/* FUNCIONES PARA INCLUIR EN EL CONTROL DE UN ROBOT DIFERENCIAL CON DOS SERVOS DE ROTACIÓN CONTINUA.
LA VELOCIDAD DE CADA RUEDA SE DEFINE CON UN NÚMERO ENTRE 0 (PARADA) Y 100 (MÁXIMA)
VÍCTOR GALLEGO 2014
*/

#include 

Servo servo_izquierdo;
Servo servo_derecho;
int cero_derecho = 93;              // AJUSTE DE LA POSICIÓN DE QUIETO PARA CADA SERVO
int cero_izquierdo = 93;
int pin_servo_izquierdo = 9;       // PIN PWM CON EL QUE SE CONTROLA CADA SERVO
int pin_servo_derecho = 10;

void setup ()
{

  servo_izquierdo.attach(pin_servo_izquierdo);
  servo_derecho.attach(pin_servo_derecho);

}

void avanza(int tiempo_avance, int velocidad_avance)  // FUNCIÓN DE AVANCE. ARGUMENTOS, TIEMPO DEL MOVIMIENTO Y VELOCIDAD
{
  servo_izquierdo. write((180 - cero_izquierdo)*velocidad_avance / 100);
  servo_derecho. write(cero_derecho - velocidad_avance * cero_derecho / 100);
  delay(tiempo_avance);
}

void gira_derecha (int tiempo_giro_derecha, int velocidad_giro)      // FUNCIÓN DE GIRO A DERECHAS. ARGUMENTOS, TIEMPO DEL MOVIMIENTO Y VELOCIDAD
{
  servo_izquierdo. write(cero_derecho - velocidad_giro * cero_derecho / 100);
  servo_derecho. write(cero_derecho - velocidad_giro * cero_derecho / 100);
  delay(tiempo_giro_derecha);
}

void gira_izquierda (int tiempo_giro_izquierda , int velocidad_giro)  // FUNCIÓN DE GIRO A IZQUIERDAS. ARGUMENTOS, TIEMPO DEL MOVIMIENTO Y VELOCIDAD
{
  servo_izquierdo. write((180 - cero_izquierdo)*velocidad_giro / 100);
  servo_derecho. write((180 - cero_izquierdo)*velocidad_giro / 100);
  delay(tiempo_giro_izquierda);
}

void retrocede (int tiempo_retroceso, int velocidad_retroceso)      // FUNCIÓN DE RETROCESO. ARGUMENTOS, TIEMPO DEL MOVIMIENTO Y VELOCIDAD
{
  servo_izquierdo. write(cero_derecho - velocidad_retroceso * cero_derecho / 100);
  servo_derecho. write((180 - cero_izquierdo)*velocidad_retroceso / 100);
  delay(tiempo_retroceso);
}

void para (int tiempo_parada)                                    // FUNCIÓN DE PARADA. ARGUMENTO, TIEMPO DE PARADA
{
  servo_izquierdo. write(cero_izquierdo);
  servo_derecho. write(cero_derecho);
  delay(tiempo_parada);
}

void loop ()                                            // PROGRAMA PRINCIPAL
{
  para(3000);
  avanza(1000, 50);
  para(3000);
  gira_derecha(500, 50);
  avanza(1000, 50);
  gira_izquierda(500, 50);
  retrocede(1000, 50);
  para(5000);

}