// INERVA ROBOTICS & ROBOOPS //// FIRMWARE PARA GALATEA 2.0 //// 2011 ///
#include <Servo.h>

Servo Servos[9] ; // max servos is 32 for mega, 8 for other boards

int ojos = 0;
int PCAB = 13;
int PDDP = 12;
int PDDC = 8;
int PDIP = 3;
int PDIC = 2;
int PTDP = 6;
int PTDC = 7;
int PTIP = 5;
int PTIC = 4;

int CAB = 0;
int DDP = 1;
int DDC = 2;
int DIP = 3;
int DIC = 4;
int TDP = 5;
int TDC = 6;
int TIP = 7;
int TIC = 8;

int conta = 0;
int modo = 0;
int parar = 0;
int lenti = 14;
int orden = 0;
int sendok = 0;
int trama[8];
int sonar[5];
int laser = 0;
unsigned  int crc;

//Declaracion de funciones
void medicion();
void control();
void avanzar();
void retroceder();
void giroizq();
void giroder();
void show();
void keep();
void down();
void susto();
void psusto();
void autonomo();
void intser();
void dinmed(int x);
void checktrama();
void fstart();
void fstop();
void fledcentral();
void fdatossonar();
void fdatosacelerometro();
void fposicion();
void fciclo();
void fdatosbateria();
void fledcabeza();
void funcionok();
void farranque();

int dinizq=20;
int dinder=20;
int dincen=20;
int medida, medidacentro, medidaizq, medidader;
int inicio=1;
int parpa=0;

void setup()
{
  Serial.begin(9600);
  Servos[CAB].attach( PCAB, 600, 2400);
  Servos[DDP].attach( PDDP, 600, 2400);
  Servos[DDC].attach( PDDC, 600, 2400);
  Servos[DIP].attach( PDIP, 600, 2400);
  Servos[DIC].attach( PDIC, 600, 2400);
  Servos[TDP].attach( PTDP, 600, 2400);
  Servos[TDC].attach( PTDC, 600, 2400);
  Servos[TIP].attach( PTIP, 600, 2400);
  Servos[TIC].attach( PTIC, 600, 2400);  
  
  pinMode(10, OUTPUT);
  pinMode(9, OUTPUT);
  
  // Posicion de inicio servos
  Servos[CAB].write(1450);  
  Servos[DDP].write(1500);
  Servos[DDC].write(1500);
  Servos[DIP].write(1500);
  Servos[DIC].write(1500);
  Servos[TDP].write(1500);
  Servos[TDC].write(1500);
  Servos[TIP].write(1500);
  Servos[TIC].write(1500);
}

void loop() // Bucle principal
{
  if (inicio==1){farranque();}
  checktrama();
  if( orden == 1){ // Si recibe orden Star
    autonomo();
  }
  if( orden == 2){
    avanzar(); 
    orden = 0;
  }
  if( orden == 3){
    retroceder(); 
    orden = 0;
  }
  if( orden == 4){
    giroizq(); 
    orden = 0;
  }
  if( orden == 5){
    giroder(); 
    orden = 0;
  }
  if( orden == 6){
    show(); 
    orden = 0;
  }
  
  if( orden == 8){
    keep(); 
    orden = 0;
  }
  
  if( orden == 9){
    down(); 
    orden = 0;
  }
  
  if( orden == 7){ // Barrido sonar y envio de datos
    medicion();
    unsigned  int crc;
    Serial.write(0x80);
    Serial.write(0x33);
    Serial.write(0x01);
    Serial.write(medidader);
    Serial.write(medidacentro);
    Serial.write(medidaizq);
    Serial.write(0x81);
    crc = 0x34 + medidaizq + medidacentro + medidader;
    Serial.write(crc);
    orden = 0;
  }
  
  if (parpa==1){ // Modo parpadeo leds cabeza
    medidacentro = analogRead(1);
    digitalWrite(9, HIGH);
    delay(medidacentro);
    digitalWrite(9, LOW);
    delay(medidacentro);
  }
//  
//  if( orden == 0){ // Cuando esta en espera retrocede
//   medidacentro = analogRead(1);
//   if (medidacentro<=10){
//       orden=3;
//   }  
//}  
}

void autonomo()
{ 
  modo = 0;
  medicion();
  conta = 0;
  control();
}

void dinmed(int x) //toma medidas en mientras camina
{
  if (x == 25) //si esta en el extremo izquierdo lo guarda en dinizq
  {
    dinizq = analogRead(1)/4;
    if (laser==1){
      digitalWrite(9, HIGH);
      delay(10);
      digitalWrite(9, LOW);
    }
         
  }
  else  if ((x == 0) || (x == 50) || (x == 99)) //si esta en el centro lo guarda en dincen
  {
    dincen = analogRead(1)/4;
    if (laser==1){
      digitalWrite(9, HIGH);
      delay(10);
      digitalWrite(9, LOW);
    }
  }
  else  if (x == 75) //si esta en el extremo derecho lo guarda en dinder
  {
    dinder = analogRead(1)/4;
    if (laser==1){
      digitalWrite(9, HIGH);
      delay(10);
      digitalWrite(9, LOW);
    }
  }
  if (dinizq<=2 || dincen<=3 || dinder<=2) //Si cualquiera de las medidas es igual o menor a 10 activa retroceso
  {
    parar=1;
    modo=1;
  }
}

void medicion()
{
  Servos[CAB].write(1450);  
  Servos[DDP].write(2200);
  Servos[DDC].write(1200);
  Servos[DIP].write(950);
  Servos[DIC].write(2000);
  Servos[TDP].write(1050);
  Servos[TDC].write(1500);
  Servos[TIP].write(2000);
  Servos[TIC].write(1500);
  delay(250);
  checktrama();
  medidacentro = analogRead(1)/4;
  if (laser==1){
      digitalWrite(9, HIGH);
      delay(10);
      digitalWrite(9, LOW);
    }
  for (int i = 0; i < 40;i++)
  {
    Servos[CAB].write(1450 - (i * 14));   
    delay(5); 
  }
  Servos[CAB].write(900);  
  delay(250);
  checktrama();
  medidader = analogRead(1)/4;
  if (laser==1){
      digitalWrite(9, HIGH);
      delay(10);
      digitalWrite(9, LOW);
    }
  for (int i = 0; i < 80;i++)
  {
    Servos[CAB].write(900 + (i * 14));   
    delay(5); 
  }
  Servos[CAB].write(2000);
  delay(250);  
  checktrama();
  medidaizq = analogRead(1)/4;
  if (laser==1){
      digitalWrite(9, HIGH);
      delay(10);
      digitalWrite(9, LOW);
    }
  for (int i = 0; i < 40;i++)
  {
    Servos[CAB].write(2000 - (i * 14));   
    delay(5); 
  }
  Servos[CAB].write(1450);  
  checktrama();
  int op1;
  int op2;
  int izq = 0,cen = 0,der = 0;
  op1 = (medidaizq>medidacentro);
  op2 = (medidaizq>medidader);
  izq = (op1 & op2);
  op1 = (medidacentro>medidaizq);
  op2 = (medidacentro>medidader);
  cen = (op1 & op2);
  op1 = (medidader>medidaizq);
  op2 = (medidader>medidacentro);
  der = (op1 & op2);
  if (cen == 1 || medidacentro>=6) // Avanzar
  {
    modo = 0;
  }
  else if (izq == 1) // Girar izquierda
  {
    modo = 2;
  } 
  else  // Girar derecha
  {
    modo = 3;
  }
  checktrama();
}

void control() //Se controla el número de repiticiones de cada ciclo de pasos
{
  if (modo == 0) {
    for(int rep = 0; rep < 4; rep++)
    {
      avanzar();
      checktrama();
    }
  }
  if (modo == 1) {
    for(int rep = 0; rep < 2; rep++)
    {
      retroceder();
      checktrama();
    }
  }
  if (modo == 2) {
    for(int rep = 0; rep < 1; rep++)
    {
      giroizq();
      checktrama();
    }
  }
  if (modo == 3) {
    for(int rep = 0; rep < 1; rep++)
    {
      giroder();
      checktrama();
    }
  }
}

void avanzar()
{
  checktrama();
  Servos[DDP].write(1300);
  Servos[DIP].write(1300);    
  Servos[TDP].write(1700);
  Servos[TIP].write(1300);
  for(int i = 0; i<25; i++)
  {
    void checktrama();
    Servos[CAB].write(1450+(22*i));
    Servos[DDC].write(1733-(18.66*i));
    Servos[DIC].write(2200-(56*i));
    Servos[TDC].write(1266-(18.66*i));
    Servos[TIC].write(800+(18.66*i));
    dinmed(i);
    delay(lenti);
    if (parar==1)
    {
      parar=0;
      return;
    }
  }
  Servos[DDP].write(1300);
  Servos[DIP].write(1700);  
  Servos[TDP].write(1300);
  Servos[TIP].write(1300);  
  for(int i = 25; i<50; i++)
  {
    void checktrama();
    Servos[CAB].write(2000-(22*(i-25)));
    Servos[DDC].write(1733-(18.66*i));
    Servos[DIC].write(800+(18.66*(i-25)));
    Servos[TDC].write(800+(56*(i-25)));
    Servos[TIC].write(800+(18.66*i));
    dinmed(i);
    delay(lenti);
    if (parar==1)
    {
      parar=0;
      return;
    }
  }
  Servos[DDP].write(1700);   
  Servos[DIP].write(1700);
  Servos[TDP].write(1700);
  Servos[TIP].write(1300);
  for(int i = 50; i<75; i++)
  {
    void checktrama();
    Servos[CAB].write(2000-(22*(i-25)));
    Servos[DDC].write(800+(56*(i-50)));
    Servos[DIC].write(800+(18.66*(i-25)));
    Servos[TDC].write(2200-(18.66*(i-50)));
    Servos[TIC].write(800+(18.66*i));
    dinmed(i);
    delay(lenti);
    if (parar==1)
    {
      parar=0;
      return;
    }
  }
  Servos[DDP].write(1300);   
  Servos[DIP].write(1700);
  Servos[TDP].write(1700);
  Servos[TIP].write(1700);
  for(int i = 75; i<100; i++)
  {
    void checktrama();
    Servos[CAB].write(900+(22*(i-75)));
    Servos[DDC].write(2200-(18.66*(i-75)));
    Servos[DIC].write(800+(18.66*(i-25)));
    Servos[TDC].write(2200-(18.66*(i-50)));
    Servos[TIC].write(2200-(56*(i-75)));
    dinmed(i);
    delay(lenti);
    if (parar==1)
    {
      parar=0;
      return;
    }
  }
}

void retroceder()
{
  Servos[DDP].write(1300);
  Servos[DIP].write(1700);    
  Servos[TDP].write(1300);
  Servos[TIP].write(1300);
  for(int i = 0; i<25; i++)
  {
    void checktrama();
    Servos[CAB].write(1450+(22*i));
    Servos[DDC].write(800+(18.66*i));
    Servos[DIC].write(1266-(18.66*i));
    Servos[TDC].write(2200-(56*i));
    Servos[TIC].write(1733-(18.66*i));
    dinmed(i);
    delay(lenti);
  }
  Servos[DDP].write(1300);
  Servos[DIP].write(1300);  
  Servos[TDP].write(1700);
  Servos[TIP].write(1300);  
  for(int i = 25; i<50; i++)
  {
    void checktrama();
    Servos[CAB].write(2000-(22*(i-25)));
    Servos[DDC].write(800+(18.66*i));
    Servos[DIC].write(800+(56*(i-25)));
    Servos[TDC].write(800+(18.66*(i-25)));
    Servos[TIC].write(1733-(18.66*i));
    dinmed(i);
    delay(lenti);
  }
  Servos[DDP].write(1300);   
  Servos[DIP].write(1700);
  Servos[TDP].write(1700);
  Servos[TIP].write(1700);
  for(int i = 50; i<75; i++)
  {
    void checktrama();
    Servos[CAB].write(2000-(22*(i-25)));
    Servos[DDC].write(800+(18.66*i));
    Servos[DIC].write(2200-(18.66*(i-50)));
    Servos[TDC].write(800+(18.66*(i-25)));
    Servos[TIC].write(800+(56*(i-50)));
    dinmed(i);
    delay(lenti);
  }
  Servos[DDP].write(1700);   
  Servos[DIP].write(1700);
  Servos[TDP].write(1700);
  Servos[TIP].write(1300);
  for(int i = 75; i<100; i++)
  {
    void checktrama();
    Servos[CAB].write(900+(22*(i-75)));
    Servos[DDC].write(2200-(56*(i-75)));
    Servos[DIC].write(2200-(18.66*(i-50)));
    Servos[TDC].write(800+(18.66*(i-25)));
    Servos[TIC].write(2200-(18.66*(i-75)));
    dinmed(i);
    delay(lenti);
  }
}

void giroizq()
{  
  Servos[CAB].write(1450);      
  Servos[DDP].write(1300);
  Servos[DIP].write(1300);    
  Servos[TDP].write(1700);
  Servos[TIP].write(1300);
  for(int i = 0; i<25; i++)
  {
    void checktrama();
    Servos[DDC].write(2200-(18.66*i));
    Servos[DIC].write(800+(56*i));
    Servos[TDC].write(1733-(18.66*i));
    Servos[TIC].write(1266-(18.66*i));
    dinmed(i);
    delay(lenti);
  }
  Servos[DDP].write(1300);
  Servos[DIP].write(1700);  
  Servos[TDP].write(1700);
  Servos[TIP].write(1700);  
  for(int i = 25; i<50; i++)
  {
    void checktrama();
    Servos[DDC].write(2200-(18.66*i));
    Servos[DIC].write(2200-(18.66*(i-25)));
    Servos[TDC].write(1733-(18.66*i));
    Servos[TIC].write(800+(56*(i-25)));
    dinmed(i);
    delay(lenti);
  }
  Servos[DDP].write(1300);   
  Servos[DIP].write(1700);
  Servos[TDP].write(1300);
  Servos[TIP].write(1300);
  for(int i = 50; i<75; i++)
  {
    void checktrama();
    Servos[DDC].write(2200-(18.66*i));
    Servos[DIC].write(2200-(18.66*(i-25)));
    Servos[TDC].write(800+(56*(i-50)));
    Servos[TIC].write(2200-(18.66*(i-50)));
    dinmed(i);
    delay(lenti);
  }
  Servos[DDP].write(1700);   
  Servos[DIP].write(1700);
  Servos[TDP].write(1700);
  Servos[TIP].write(1300);
  for(int i = 75; i<100; i++)
  {
    void checktrama();
    Servos[DDC].write(800+(56*(i-75)));
    Servos[DIC].write(2200-(18.66*(i-25)));
    Servos[TDC].write(2200-(18.66*(i-75)));
    Servos[TIC].write(2200-(18.66*(i-50)));
    dinmed(i);
    delay(lenti);
  }
}

void giroder()
{
  Servos[CAB].write(1450);  
  Servos[DDP].write(1700);
  Servos[DIP].write(1700);    
  Servos[TDP].write(1700);
  Servos[TIP].write(1300);
  for(int i = 0; i<25; i++)
  {
    void checktrama();
    Servos[DDC].write(2200-(56*i));
    Servos[DIC].write(800+(18.66*i));
    Servos[TDC].write(1733+(18.66*i));
    Servos[TIC].write(1266+(18.66*i));
    dinmed(i);
    delay(lenti);
  }
  Servos[DDP].write(1300);
  Servos[DIP].write(1700);  
  Servos[TDP].write(1300);
  Servos[TIP].write(1300);  
  for(int i = 25; i<50; i++)
  {
    void checktrama();
    Servos[DDC].write(800+(18.66*(i-25)));
    Servos[DIC].write(800+(18.66*i));
    Servos[TDC].write(2200-(56*(i-25)));
    Servos[TIC].write(1266+(18.66*i));
    dinmed(i);
    delay(lenti);
  }
  Servos[DDP].write(1300);   
  Servos[DIP].write(1700);
  Servos[TDP].write(1700);
  Servos[TIP].write(1700);
  for(int i = 50; i<75; i++)
  {
    void checktrama();
    Servos[DDC].write(800+(18.66*(i-25)));
    Servos[DIC].write(800+(18.66*i));
    Servos[TDC].write(800+(18.66*(i-50)));
    Servos[TIC].write(2200-(56*(i-50)));
    dinmed(i);
    delay(lenti);
  }
  Servos[DDP].write(1300);   
  Servos[DIP].write(1300);
  Servos[TDP].write(1700);
  Servos[TIP].write(1300);
  for(int i = 75; i<100; i++)
  {
    void checktrama();
    Servos[DDC].write(800+(18.66*(i-25)));
    Servos[DIC].write(2200-(56*(i-75)));
    Servos[TDC].write(800+(18.66*(i-50)));
    Servos[TIC].write(800+(18.66*(i-75)));
    dinmed(i);
    delay(lenti);
  }
}

void intser() // Datos en buffer
{
  trama[0] = Serial.read();
  while (trama[0] != 0x80)
  {  
    while(Serial.available()==0);
    trama[0] = Serial.read();
  }
  for (int x = 1; x < 8; x++)
  {
    while(Serial.available()==0);
    trama[x] = Serial.read();
  }
  crc = trama[1] + trama[2] + trama[3] + trama[4] + trama[5];
  if (trama[7] != crc && false)
  {
    return;
  }
  else
  {
    switch(trama[1])
    {
      case 0x30: //start
      fstart();
      break;
      case 0x31: //stop
      fstop();
      break;
      case 0x32:
      fledcentral();
      break;
      case 0x33:
      fdatossonar();
      break;
      case 0x35:
      fdatosacelerometro();
      break;
      case 0x34:
      fposicion();
      break;
      case 0x36:
      fciclo();
      break;
      case 0x37:
      fdatosbateria();
      break;
      case 0x38:
      fledcabeza();
      break;
    }   
  }
  if (sendok == 1) // Si sendok activo enviar OK
     {
       Serial.write(0x80);
       Serial.write(0x32);
       Serial.write(0x01);
       Serial.write(0x01);
       Serial.write(0x01);
       Serial.write(0x01);
       Serial.write(0x81);
       Serial.write(0x36);
       sendok = 0;
     }
}

void fstart() // Si Start autonomo...
{
  orden = 1;
  sendok = 1;  
}
  
void fstop() //Si Stop autónomo...
{        
  orden = 0;
  sendok = 1;
}
  
void fledcentral() // Led central On/Off
{
  if (trama[2] == 0x00 && trama[3] == 0x01)
     {
       digitalWrite(10, HIGH);
       sendok = 1;
     }
  if (trama[2] == 0x01 && trama[3] == 0x00)
     {
       digitalWrite(10, LOW);
       sendok = 1;
     }      
}
         
  
void fdatossonar() // Solicitud datos Sonar
{
  Serial.write(0x80);
  Serial.write(0x33);
  Serial.write(0x01);
  Serial.write(dinizq);
  Serial.write(dincen);
  Serial.write(dinder);
  Serial.write(0x81);
  crc = 0x34 + dinizq + dincen + dinder;
  Serial.write(crc);  
}
  
void fdatosacelerometro() //Se solicita datos acelerómetro
{
   unsigned int ex, ey, ez;
   Serial.write(0x80);
   Serial.write(0x35);
   ex = analogRead(7)/4;
   ex = max (ex,65);
   ex = min (ex,192);
   Serial.write(ex);
   ey = analogRead(6)/4;
   ey = max (ey,65);
   ey = min (ey,192);
   Serial.write(ey);
   ez = analogRead(5)/4;
   ez = max (ez,65);
   ez = min (ez,192);
   Serial.write(ez);
   Serial.write(0x01);
   crc = 0x35 + ex + ey + ez + 0x01;
   Serial.write(0x81);
   Serial.write(crc);
}
  
void fposicion() //Solicitud Posicion predefinida
{
   if (trama[5] == 0x01) // Se solicita posicion show 
      {
        orden=6;
        sendok = 1;
      }
   else if (trama[5] == 0x02) // Se solicita posicion keep
      {
        orden=8;
        sendok = 1;
      }
   else // Se activa posicion down
      {
        orden=9;
        sendok = 1;
      } 
}
  
void fciclo() //Solicitud ciclo telecomandado;
{
   if (trama[5] == 0x01) //Avanza
      {
        orden=2;
        sendok = 1;
      }
   if (trama[5] == 0x02) //Retrocede
      {
        orden=3;
        sendok = 1;
      }
   if (trama[5] == 0x03) //Izquierda
      {
        orden=4;
        sendok = 1;
      }
   if (trama[5] == 0x04) //Derecha
      {
        orden=5;
        sendok = 1;
      }
   if (trama[5] == 0x05) // barrido sonar
      {
        orden=7;
        sendok = 1;
      }
}
  
void fdatosbateria() //Solicitud datos nivel bateria
{
   unsigned int bateria;
   bateria = analogRead(2)/4;
   Serial.write(0x80);
   Serial.write(0x37);
   Serial.write(0x01);
   Serial.write(0x01);
   Serial.write(0x01);
   Serial.write(bateria);
   Serial.write(0x81);
   crc = 0x37 + 0x01 + 0x01 + 0x01 + bateria;
   Serial.write(crc);
} 
  
void fledcabeza() // Modo leds cabeza
{
   if (trama[5] == 0x00) //Leds Off
    {
      digitalWrite(9, LOW);
      laser=0;
      parpa=0;
      sendok = 1;
    }
   else if (trama[5] == 0x01)
         {
           digitalWrite(9, HIGH); //Modo Leds On
           laser=0;
           parpa=0;
           sendok = 1;
         }
      else if (trama[5] == 0x02) // Modo leds activos cuando lectura sonar
            {
              digitalWrite(9, LOW);
              laser=1;
              parpa=0;
              sendok = 1;
            }
           else if (trama[5] == 0x03) // Modo leds rafaga
                 {
//                 for (int i = 0; i < 8;i++)
//                   {
//                     digitalWrite(9, HIGH);
//                     delay(10);
//                     digitalWrite(9, LOW);
//                     delay(100);
//                   }
                 parpa=1;
                 laser=0;
                 sendok = 1;
                 }   
}
  
void show() // Posición predefinida "SHOW"
{
  Servos[CAB].write(1450);  
  Servos[DDP].write(750);//900
  Servos[DDC].write(1500);
  Servos[DIP].write(2400); //2100
  Servos[DIC].write(1500);
  Servos[TDP].write(1050);
  Servos[TDC].write(2000);
  Servos[TIP].write(2000);
  Servos[TIC].write(1000);  
}

void keep() // Posición predefinida "KEEP"
{
  Servos[CAB].write(1450);  
  Servos[DDP].write(1500);
  Servos[DDC].write(1500);
  Servos[DIP].write(1500);
  Servos[DIC].write(1500);
  Servos[TDP].write(1500);
  Servos[TDC].write(1500);
  Servos[TIP].write(1500);
  Servos[TIC].write(1500);  
}

void down() // Posición predefinida "DOWN"
{
  Servos[CAB].write(1450);  
  Servos[DDP].write(2200);
  Servos[DDC].write(1200);
  Servos[DIP].write(950);
  Servos[DIC].write(2000);
  Servos[TDP].write(1050);
  Servos[TDC].write(1500);
  Servos[TIP].write(2000);
  Servos[TIC].write(1500);  
}

void checktrama()
{
//  fmecogen();  
  if(Serial.available()!=0){intser();}
}

//void fmecogen()
//{
//  unsigned int ex, ey, ez;
//  ex = analogRead(7)/4;
//  ey = analogRead(6)/4;
//  ez = analogRead(5)/4;
//  if (ey<=100 && ez<=100)
//  {
//    delay(5000);
//  }
//}

void farranque()
{
  inicio=0;
  for (int i=0; i<4; i++)
    {
      sonar[i] = analogRead(1)/4;
    }
    medida = (sonar[0]+sonar[1]+sonar[2]+sonar[3]+sonar[4])/5; 
  if (medida>=20)
   {
     digitalWrite(9, HIGH);
     delay (1000);       
     digitalWrite(9, LOW);
     medida=0; 
     orden=1;
     laser=1;     
   }
  else
   {
     medida=0;
     for (int i = 0; i < 8;i++) // Parpadeo inicial de leds cabeza y central
       {
         digitalWrite(9, HIGH);
         digitalWrite(10, HIGH);
         delay(10);
         digitalWrite(9, LOW);
         digitalWrite(10, LOW);
         delay(100);
       }
   }
}