POST_49

 /*  TITULO: Robot Miniskybot 2.0 funcionando como siguelíneas.

    AUTOR:

    MARIANO DEL CAMPO GARCÍA (@2016) --> INGENIERO TÉCNICO INDUSTRIAL ESPECIALIDAD ELECTRÓNICA
    - FACEBOOK: https://www.facebook.com/mariano.delcampogarcia
    - TWITTER: https://twitter.com/MarianoCampoGa
    - CORREO: marianodc83@gmail.com


    DESCRIPCIÓN DEL PROGRAMA

    Con este programa simplemente hacemos que el Robot Miniskybot 2.0 siga la línea negra,
    sin salirse de la trayectoria dibujada en la pista de pruebas con un adhesivo de color negro
    y cuando lo saquemos sobre la superficie blanca, el robot permanecerá parado.


    ESQUEMA DE CONEXION

                                      +-----+
         +----[PWR]-------------------| USB |--+
         |                            +-----+  |
         |         GND/RST2  [ ][ ]            |
         |       MOSI2/SCK2  [ ][ ]  A5/SCL[ ] |
         |          5V/MISO2 [ ][ ]  A4/SDA[ ] |
         |                             AREF[ ] |
         |                              GND[ ] |
         | [ ]N/C                    SCK/13[ ] |   Pin OUT del sensorIzquierdo
         | [ ]IOREF                 MISO/12[ ] |   Pin OUT del sensorDerecho
         | [ ]RST                   MOSI/11[ ]~|
         | [ ]3V3    +---+               10[ ]~|
         | [ ]5v    -| A |-               9[ ]~|
         | [ ]GND   -| R |-               8[ ] |
         | [ ]GND   -| D |-                    |
         | [ ]Vin   -| U |-               7[ ] |
         |          -| I |-               6[ ]~|
         | [ ]A0    -| N |-               5[ ]~|   Cable blanco (Señal) del SERVO_IZQUIERDO
         | [ ]A1    -| O |-               4[ ] |
         | [ ]A2     +---+           INT1/3[ ]~|   Cable blanco (Señal) del SERVO_DERECHO
         | [ ]A3                     INT0/2[ ] |
         | [ ]A4/SDA  RST SCK MISO     TX>1[ ] |
         | [ ]A5/SCL  [ ] [ ] [ ]      RX<0[ ] |
         |            [ ] [ ] [ ]              |
         |  UNO_R3    GND MOSI 5V  ____________/
          \_______________________/

  NOTAS:

    - Todas las alimentaciones (cables rojos) y las masas (cables negros) de los servos van
      conectadas a +5V y GND respectivamente.
    - También las alimentaciones y las masas de los sensores siguelíneas van directamente
      conectadas a +5V y GND respectivamente.
    - Las conexiones están marcadas soble la placa de Arduino UNO, aunque luego conectemos
      directamente sobre los terminales de 3 pines (GVS) del sensor shield V4.0 que correspondan
      a cada pin digital de Arduino que estemos utilizando.

*/

  // Incluimos la librería para el funcionamiento de los servos
  #include <Servo.h>

  // Constantes para el motor derecho
  const int SERVO_DERECHO = 3;  // PIN PWM donde conecto el servomotor derecho
  const int SERVO_DERECHO_ADELANTE = 0;    // Mueve el motor derecho hacia adelante
  const int SERVO_DERECHO_ATRAS = 180;  // Mueve el motor derecho hacia atrás
  const int SERVO_DERECHO_PARA = 90; // Para el motor derecho

  // Constantes para el motor izquierdo (invertidas porque se monta el servo al revés)
  const int SERVO_IZQUIERDO = 5;  // PIN PWM donde conecto el servomotor izquierdo
  const int SERVO_IZQUIERDO_ADELANTE = 180;  // Mueve el motor izquierdo hacia adelante
  const int SERVO_IZQUIERDO_ATRAS = 0;    // Mueve el motor izquierdo hacia atrás
  const int SERVO_IZQUIERDO_PARA = 90; // Para el motor izquierdo

  // Constantes para definir el pin de los dos SENSORES
  const int sensorDerecho = 12;  // Pin digital donde conectamos el Sensor Derecho
  const int sensorIzquierdo = 13;  // Pin digital donde conectamos el Sensor Izquierdo

  // Variables para guardar el valor de los sensores, inicialmente no detectan línea negra
  int estadoDerecho = 1;  // para leer y guardar el estado del sensor derecho
  int estadoIzquierdo = 1; // para leer y guardar el estado del sensor derecho

  // Declaramos los dos objetos de tipo servo
  Servo motor_derecho;  // Motor derecho
  Servo motor_izquierdo;  // Motor izquierdo


  // Función que manda avanzar al robot
  void robot_avanza()
  {
    motor_derecho.write(SERVO_DERECHO_ATRAS);
    motor_izquierdo.write(SERVO_IZQUIERDO_ATRAS);
  }

  // Función que manda pararse al robot
  void robot_para()
  {
    motor_derecho.write(SERVO_DERECHO_PARA);
    motor_izquierdo.write(SERVO_IZQUIERDO_PARA);
  }

  // Función que mueve el robot a la izquierda
  void robot_izquierda()
  {
    motor_derecho.write(SERVO_DERECHO_ATRAS);
    motor_izquierdo.write(SERVO_IZQUIERDO_ADELANTE);
  }

  // Función que mueve el robot a la derecha
  void robot_derecha()
  {
    motor_derecho.write(SERVO_DERECHO_ADELANTE);
    motor_izquierdo.write(SERVO_IZQUIERDO_ATRAS);
  }

  void setup()
  {
    // Inicializa los sensores como entradas
    pinMode(sensorDerecho, INPUT);
    pinMode(sensorIzquierdo, INPUT);

    // Configura el pin de señal de los motores
    motor_derecho.attach(SERVO_DERECHO);
    motor_izquierdo.attach(SERVO_IZQUIERDO);
  }

  void loop()
  {
    // Lee el valor del estado del sensor derecho y lo guarda en la variable estadoDerecho
    estadoDerecho = digitalRead(sensorDerecho);
    // Lee el valor del estado del sensor izquierdo y lo guarda en la variable estadoIzquierdo
    estadoIzquierdo = digitalRead(sensorIzquierdo);

    // Si los dos sensores están dentro de la línea negra
    if ((estadoIzquierdo == LOW)&&(estadoDerecho == LOW))
    {
      // El robot avanza hacia adelante
      robot_avanza();
    }

    // Si el sensor izquierdo está fuera de la línea negra y el derecho dentro
    if ((estadoIzquierdo == HIGH)&&(estadoDerecho == LOW))
    {
      // El robot gira a la derecha
      robot_derecha();
    }

    // Si el sensor izquierdo está dentro de la línea negra y el derecho fuera
    if ((estadoIzquierdo == LOW)&&(estadoDerecho == HIGH))
    {
      // El robot gira a la izquierda
      robot_izquierda();
    }

    // Si los dos sensores están fuera de la línea negra (sobre superficie blanca)
    if((estadoIzquierdo == HIGH)&&(estadoDerecho == HIGH))
    {
      // El robot permanece parado
      robot_para();
    }
  }