#include <Servo.h>
#undef int
#undef abs
#undef double
#undef float
#undef round

Servo servoA, servoB, servoC;

int IR = 8; 
int zumbador =11;
int camara = 4;
int girocam = 2;
int bat = 4;
int bit1 = 0;
int bit2 = 1;
int bit3 = 2;
int bit4 = 3;
int led =3;
int laser =5;
int act_serv = 7;
int speed1 = 9;
int speed2 = 10;

int id = 0;
int val1 = 0;
int val2 = 0;
int val3 = 0;
int val4 = 0;
byte val = 0;
unsigned long time=0;
unsigned long time_ant=0;
int frec = 0;
int activa=0;
int Command_ok=0;
int mensaje_ok=0;
int dat = 0;
int command = 0;

void izquierda(int iz) 
{ 
  servoA.write(iz);
}

void derecha(int der) 
{ 
  servoB.write(der);
}
 
void setup() 
{ 
  Serial.begin(19200); 
  delay(100); 
  
  analogWrite(zumbador,0);
  
  servoA.attach(speed1);
  servoB.attach(speed2);
  servoC.attach(girocam);
  
  digitalWrite(act_serv,  HIGH); //Servos apagados
  
  servoC.write(93); //Parado
  
  pinMode(IR, OUTPUT);
  pinMode(zumbador, OUTPUT);
  pinMode(camara, OUTPUT);
  pinMode(14, INPUT);
  pinMode(15, INPUT);
  pinMode(16, INPUT);
  pinMode(17, INPUT);
  digitalWrite(14, HIGH);
  digitalWrite(15, HIGH);
  digitalWrite(16, HIGH);
  digitalWrite(17, HIGH);
  if (analogRead(bit1)>512) val1=1;
  else val1=0;
  if (analogRead(bit2)>512) val2=2;
  else val2=0;
  if (analogRead(bit3)>512) val3=4;
  else val3=0;
  if (analogRead(bit4)>512) val4=8;
  else val4=0;
  id=val1+val2+val3+val4;
  //Serial.print("Hola, soy el robot numero ");
  //Serial.println(id);
  digitalWrite(camara,  LOW); //Desactiva camara
  digitalWrite(act_serv,  HIGH); //Desactiva servomotores
  digitalWrite(led,  LOW); //Apaga leds
  digitalWrite(laser,  LOW); //Apaga laser
  
} 

void loop() 
{ 
   Servo::refresh();  
   if (Serial.available()) {
    val = Serial.read();
   if (val==0xF0) mensaje_ok=0;
   else mensaje_ok=1;

   if (mensaje_ok==1)
   {
    if ((val>>4)==id)
    {
      if (Command_ok==0x01)
      {
        Command_ok=0x00;
        dat = val&0x0F;
        Serial.print(id<<4|analogRead(bat)/64, BYTE);
          switch (command) {
          case 0x00: //Servo derecho
            digitalWrite(act_serv,  LOW);
            derecha(0x0C*dat);
            //Serial.print(0x0C*dat, BYTE);
          break;
          case 0x01: //Servo izquierdo
            digitalWrite(act_serv,  LOW);
            izquierda(0x0C*dat);
          break;
          case 0x02: //CAMARA ON/OFF
          {
            if (dat==0x01) digitalWrite(camara,  HIGH);
            else digitalWrite(camara,  HIGH);
          }
          break;
          case 0x03: //GIRO CAMARA
          {
            digitalWrite(act_serv,  LOW);
            servoC.write(0x0C*dat);
          }
          break;
          case 0x04://LASER ON/OFF
          {
            if (dat==0x01) digitalWrite(laser,  HIGH);
            else digitalWrite(laser,  HIGH);
          }
          break;
          case 0x05: //ZUMBADOR
          {
            if (dat==0x01)
            {
              frec= 100;
              activa=1;
              time_ant=millis();
            }
            else
            {
              activa=0;
              analogWrite(zumbador,0);
            }
          }
          break;
          case 0x06: //IR
          {
            if (dat==0x01) digitalWrite(IR,  HIGH);
            else digitalWrite(IR,  LOW);
          }
          break;
          case 0x07:  //LEDS BLANCOS
          {
            if (dat==0x01) digitalWrite(led,  HIGH); //Enciende leds
            else digitalWrite(led,  LOW);
          }
          break;
        }
      }
      else
       { 
        command=val&0xF;
        Command_ok=0x01;
        Serial.print(val, BYTE);
       }
     }
    }
   }
  time = millis();
  if (activa==1)
  {
    if (time-time_ant >= frec)
      {
        analogWrite(zumbador,255);
        if (time-time_ant >= 2*frec) time_ant=time;
       }
    else analogWrite(zumbador,0);
  }
}