//esclavo(Arduino NANO)
#include <Servo.h>
#include <Wire.h>
Servo myservo;
#define D2 5
#define D3 6
#define D4 7
#define D5 8
#define D6 9
#define D7 10
#define D8 11
#define D9 12
#define D10 13
#define D11 14
#define D12 15
char angulo;
char orden;
char ordenAnterior;
char ordenDeMovimiento;
int anguloAnteriorIzqDelante;
int anguloAnteriorIzqDetras;
int anguloAnteriorDerDelante;
int anguloAnteriorDerDetras;
int grados;
int V;
void setup()
{
  pinMode(A0, INPUT);
  pinMode(A1, OUTPUT);
  pinMode(A2, OUTPUT);
  pinMode(A3, OUTPUT);
  pinMode(A4, OUTPUT);
  pinMode(A5, OUTPUT);
  pinMode(D2, OUTPUT);
  pinMode(D3, OUTPUT);
  pinMode(D4, OUTPUT);
  pinMode(D5, OUTPUT);
  pinMode(D6, OUTPUT);
  pinMode(D7, OUTPUT);
  pinMode(D8, OUTPUT);
  pinMode(D9, OUTPUT);
  pinMode(D10, OUTPUT);
  pinMode(D11, OUTPUT);
  pinMode(D12, OUTPUT);
  preset();
  Serial.begin(9600);
  Wire.begin(1);    // Nos añadimos al bus como esclavo, con la dirección 1
  Wire.onRequest(requestEvent); // Registro de eventos
}
void loop()
{
  delay(10);// retraso de 10 mili-segundos
}
// Funcion que se ejecuta cuando recibe una petición desde el maestro
void requestEvent()
{
  while (Wire.available())
  {
    orden = (char)Wire.read();
    while (orden != ordenAnterior) {
      ordenAnterior = orden;
      /*
         La palabra orden va a estar deparada en dos partes, la de a
         que case tiene que entrar y la del angulo tiene que estar
         el motorangular. los cuatro primeros bits son para definir
         el movimiento de los motores no angulares y los otros cuatro
         son para definir el angulo a que se tiene que establecer
         los motores angulares.
         orden = [bits angulares|bits movimiento]
         Bits para cada angulo:
         bits     angulos correspondientes      correccion de los angulos
         0x0                0                             0
         0x1                12                            12
         0x2                24                            24
         0x3                36                            36
         0x4                48                            48
         0x5                60                            60
         0x6                72                           *77
         0x7                84                            84
         0x8                96                            96
         0x9                108                          *103
         0xA                120                           120
         0xB                132                           132
         0xC                144                           144
         0xD                156                           156
         0xE                168                           168
         0xF                180                           180
         Bits para definir el movimiento de los motores:
         0x0   medir tension
         0x1   movimiento simple delante
         0x2   movimiento simple atras
         0x3   movimiento simple derecha
         0x4   movimiento simple izquierda
         0x5   posicion de todos los motoresangulares a nivel de suelo
         0x6   motores angulares de las orugas secundarias derecha delantera
         0x7   motores angulares de las orugas secundarias izquierda delantera
         0x8   motores angulares de las orugas secundarias derecha trasera
         0x9   motores angulares de las orugas secundarias izquierda trasera
         0xA   todos los motores parados
      */
      angulo = orden >> 4;
      if (angulo == 0x0) {
        grados = 0;
      }
      else if (angulo == 0x1) {
        grados = 12;
      }
      else if (angulo == 0x2) {
        grados = 24;
      }
      else if (angulo == 0x3) {
        grados = 36;
      }
      else if (angulo == 0x4) {
        grados = 48;
      }
      else if (angulo == 0x5) {
        grados = 60;
      }
      else if (angulo == 0x6) {
        grados = 77;
      }
      else if (angulo == 0x7) {
        grados = 84;
      }
      else if (angulo == 0x8) {
        grados = 96;
      }
      else if (angulo == 0x9) {
        grados = 103;
      }
      else if (angulo == 0xA) {
        grados = 120;
      }
      else if (angulo == 0xB) {
        grados = 132;
      }
      else if (angulo == 0xC) {
        grados = 144;
      }
      else if (angulo == 0xD) {
        grados = 156;
      }
      else if (angulo == 0xE) {
        grados = 168;
      }
      else if (angulo == 0xF) {
        grados = 180;
      }
      ordenDeMovimiento = orden & 0x0f;
      switch (ordenDeMovimiento)
      {
        /*
           leer tension de la bateria como el arduino solo puede leer
           valores de 0 a 5v se añade unas resistencias en serie para
           crear un divisor de potencia y se usa una regla de tres
           para saber cuanta carga hay disponible
        */
        case 0x0:  //medir tension
          V = ((12 / 5) * analogRead(A0)) * 10; //en el maestro se dividira por 10 para tener precision de un decimal
          Wire.write(V);   // Responde a la petición del maestro
          break;
        //motores de la oruga principal
        case 0x1:  //delante
          digitalWrite(A1, HIGH);
          digitalWrite(A2, LOW);
          digitalWrite(A3, LOW);
          digitalWrite(A4, LOW);
          digitalWrite(A5, LOW);
          digitalWrite(D2, LOW);
          digitalWrite(D3, HIGH);
          digitalWrite(D4, LOW);
          digitalWrite(D5, LOW);
          digitalWrite(D6, LOW);
          digitalWrite(D7, LOW);
          digitalWrite(D8, LOW);
          break;
        case 0x2:  //atras
          digitalWrite(A1, LOW);
          digitalWrite(A2, HIGH);
          digitalWrite(A3, LOW);
          digitalWrite(A4, LOW);
          digitalWrite(A5, LOW);
          digitalWrite(D2, LOW);
          digitalWrite(D3, LOW);
          digitalWrite(D4, HIGH);
          digitalWrite(D5, LOW);
          digitalWrite(D6, LOW);
          digitalWrite(D7, LOW);
          digitalWrite(D8, LOW);
          break;
        case 0x3:  //derecha
          digitalWrite(A1, HIGH);
          digitalWrite(A2, LOW);
          digitalWrite(A3, LOW);
          digitalWrite(A4, LOW);
          digitalWrite(A5, LOW);
          digitalWrite(D2, LOW);
          digitalWrite(D3, LOW);
          digitalWrite(D4, HIGH);
          digitalWrite(D5, LOW);
          digitalWrite(D6, LOW);
          digitalWrite(D7, LOW);
          digitalWrite(D8, LOW);
          break;
        case 0x4:  //izquierda
          digitalWrite(A1, LOW);
          digitalWrite(A2, HIGH);
          digitalWrite(A3, LOW);
          digitalWrite(A4, LOW);
          digitalWrite(A5, LOW);
          digitalWrite(D2, LOW);
          digitalWrite(D3, HIGH);
          digitalWrite(D4, LOW);
          digitalWrite(D5, LOW);
          digitalWrite(D6, LOW);
          digitalWrite(D7, LOW);
          digitalWrite(D8, LOW);
          break;
        //posicion de todos los motoresangulares a nivel de suelo
        case 0x5:   myservo.attach(D9);//pin del servomotor al que atañe la siguiente linea
          myservo.write(77);//posicion a la que la oruga se queda totalmente paralela al suelo
          delay(1000);
          myservo.attach(D10);
          myservo.write(103);
          delay(1000);
          myservo.attach(D11);
          myservo.write(77);
          delay(1000);
          myservo.attach(D11);
          myservo.write(103);
          delay(1000);
          break;
        //motores angulares de las orugas secundarias derecha delantera
        case 0x6:
          if (anguloAnteriorDerDelante == grados) {
            /*ya esta en el angulo seleccionado*/
          }
          else if (anguloAnteriorDerDelante < grados)
          {
            //hace falta subir
            digitalWrite(A5, HIGH);
            digitalWrite(D2, LOW);
            myservo.attach(D9);
            myservo.write(grados);//servoMotor.write(grados entre 0 y 180)
            digitalWrite(A5, LOW);
          }
          else if (anguloAnteriorDerDelante > grados)
          {
            //hace falta bajar
            digitalWrite(A5, LOW);
            digitalWrite(D2, HIGH);
            myservo.attach(D9);
            myservo.write(grados);//servoMotor.write(grados entre 0 y 180)
            digitalWrite(D2, LOW);
          }
          anguloAnteriorDerDelante = grados;
          break;
        //motores angulares de las orugas secundarias izquierda delantera
        case 0x7:
          if (anguloAnteriorIzqDelante == grados) {
            /*ya esta en el angulo seleccionado*/
          }
          else if (anguloAnteriorIzqDelante < grados)
          {
            //hace falta subir
            digitalWrite(A3, HIGH);
            digitalWrite(A4, LOW);
            myservo.attach(D10);
            myservo.write(grados);//servoMotor.write(grados entre 0 y 180)
            digitalWrite(A3, LOW);
          }
          else if (anguloAnteriorIzqDelante > grados)
          {
            //hace falta bajar
            digitalWrite(A3, LOW);
            digitalWrite(A4, HIGH);
            myservo.attach(D10);
            myservo.write(grados);//servoMotor.write(grados entre 0 y 180)
            digitalWrite(A4, LOW);
          }
          anguloAnteriorIzqDelante = grados;
          break;
        //motores angulares de las orugas secundarias derecha trasera
        case 0x8:
          if (anguloAnteriorDerDetras == grados) {
            /*ya esta en el angulo seleccionado*/
          }
          else if (anguloAnteriorDerDetras < grados)
          {
            //hace falta subir
            digitalWrite(D5, HIGH);
            digitalWrite(D6, LOW);
            myservo.attach(D11);
            myservo.write(grados);//servoMotor.write(grados entre 0 y 180)
            digitalWrite(D5, LOW);
          }
          else if (anguloAnteriorDerDetras > grados)
          {
            //hace falta bajar
            digitalWrite(D5, LOW);
            digitalWrite(D6, HIGH);
            myservo.attach(D11);
            myservo.write(grados);//servoMotor.write(grados entre 0 y 180)
            digitalWrite(D6, LOW);
          }
          anguloAnteriorDerDetras = grados;
          break;
        //motores angulares de las orugas secundarias izquierda trasera
        case 0x9:
          if (anguloAnteriorIzqDetras == grados) {
            /*ya esta en el angulo seleccionado*/
          }
          else if (anguloAnteriorIzqDetras < grados)
          {
            //hace falta subir
            digitalWrite(D7, HIGH);
            digitalWrite(D8, LOW);
            myservo.attach(D12);
            myservo.write(grados);//servoMotor.write(grados entre 0 y 180)
            digitalWrite(D7, LOW);
          }
          else if (anguloAnteriorIzqDetras > grados)
          {
            //hace falta bajar
            digitalWrite(D7, LOW);
            digitalWrite(D8, HIGH);
            myservo.attach(D12);
            myservo.write(grados);//servoMotor.write(grados entre 0 y 180)
            digitalWrite(D8, LOW);
          }
          anguloAnteriorIzqDetras = grados;
          break;
        //todos los motores parados
        case 0xA:
          digitalWrite(A1, LOW);
          digitalWrite(A2, LOW);
          digitalWrite(A3, LOW);
          digitalWrite(A4, LOW);
          digitalWrite(A5, LOW);
          digitalWrite(D2, LOW);
          digitalWrite(D3, LOW);
          digitalWrite(D4, LOW);
          digitalWrite(D5, LOW);
          digitalWrite(D6, LOW);
          digitalWrite(D7, LOW);
          digitalWrite(D8, LOW);
          break;
      }
    }
  }
  Wire.write("OK");   // Responde a la petición del maestro
}

void preset(void) {
  //Ruedas secundarias arriba
  //servomotor delantero derecho arriba
  digitalWrite(A5, HIGH);
  digitalWrite(D2, LOW);
  myservo.attach(D9);//pin del servomotor al que atañe la siguiente linea
  myservo.write(0);//angulo al que se establecera el servomotor arriba maximo
  delay(1000);//tiempo de guarda para la maniobra
  digitalWrite(A5, LOW);
  //servomotor delantero izquierdo arriba
  digitalWrite(D3, HIGH);
  digitalWrite(D4, LOW);
  myservo.attach(D10);
  myservo.write(180);
  delay(1000);
  digitalWrite(D3, LOW);
  //servomotor trasero derecho arriba
  digitalWrite(D5, HIGH);
  digitalWrite(D6, LOW);
  myservo.attach(D11);
  myservo.write(0);
  delay(1000);
  digitalWrite(D5, LOW);
  //servomotor trasero izquierdo arriba
  digitalWrite(D7, HIGH);
  digitalWrite(D8, LOW);
  myservo.attach(D11);
  myservo.write(180);
  delay(1000);
  digitalWrite(D7, LOW);
  //---------------Todas las orugas secundarias arriba-----------------------------//
  //servomotor delantero derecho abajo
  digitalWrite(A5, LOW);
  digitalWrite(D2, HIGH);
  myservo.attach(D9);//pin del servomotor al que atañe la siguiente linea
  myservo.write(180);//angulo al que se establecera el servomotor arriba maximo
  delay(1000);
  digitalWrite(D2, LOW);
  //servomotor delantero izquierdo abajo
  digitalWrite(D3, LOW);
  digitalWrite(D4, HIGH);
  myservo.attach(D10);
  myservo.write(0);
  delay(1000);
  digitalWrite(D4, LOW);
  //servomotor trasero derecho abajo
  digitalWrite(D5, LOW);
  digitalWrite(D6, HIGH);
  myservo.attach(D11);
  myservo.write(180);
  delay(1000);
  digitalWrite(D6, LOW);
  //servomotor trasero izquierdo abajo
  digitalWrite(D7, LOW);
  digitalWrite(D8, HIGH);
  myservo.attach(D11);
  myservo.write(0);
  delay(1000);
  digitalWrite(D8, LOW);
  //---------------Todas las orugas secundarias paralelas al suelo-----------------------------//
  //Ruedas secundarias abajo
  //servomotor delantero derecho paralelo al suelo
  digitalWrite(A5, HIGH);
  digitalWrite(D2, LOW);
  myservo.attach(D9);//pin del servomotor al que atañe la siguiente linea
  myservo.write(77);//posicion a la que la oruga se queda totalmente paralela al suelo
  delay(1000);
  digitalWrite(A5, LOW);
  //servomotor delantero izquierdo paralelo al suelo
  digitalWrite(D3, HIGH);
  digitalWrite(D4, LOW);
  myservo.attach(D10);
  myservo.write(103);
  delay(1000);
  digitalWrite(D3, LOW);
  //servomotor trasero derecho paralelo al suelo
  digitalWrite(D5, HIGH);
  digitalWrite(D6, LOW);
  myservo.attach(D11);
  myservo.write(77);
  delay(1000);
  digitalWrite(D5, LOW);
  //servomotor trasero izquierdo paralelo al suelo
  digitalWrite(D7, HIGH);
  digitalWrite(D8, LOW);
  myservo.attach(D11);
  myservo.write(103);
  delay(1000);
  digitalWrite(D7, LOW);
  //---------------Todas las orugas secundarias en estado de espera-----------------------------//
  //Ruedas secundarias abajo
  //servomotor delantero derecho paralelo al suelo
  digitalWrite(A5, HIGH);
  digitalWrite(D2, LOW);
  myservo.attach(D9);//pin del servomotor al que atañe la siguiente linea
  myservo.write(57);//posicion a la que la oruga se queda totalmente paralela al suelo
  delay(1000);
  digitalWrite(A5, LOW);
  //servomotor delantero izquierdo paralelo al suelo
  digitalWrite(D3, HIGH);
  digitalWrite(D4, LOW);
  myservo.attach(D10);
  myservo.write(123);
  delay(1000);
  digitalWrite(D3, LOW);
  //servomotor trasero derecho paralelo al suelo
  digitalWrite(D5, HIGH);
  digitalWrite(D6, LOW);
  myservo.attach(D11);
  myservo.write(57);
  delay(1000);
  digitalWrite(D5, LOW);
  //servomotor trasero izquierdo paralelo al suelo
  digitalWrite(D7, HIGH);
  digitalWrite(D8, LOW);
  myservo.attach(D11);
  myservo.write(123);
  delay(1000);
  digitalWrite(D7, LOW);
}
