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

#include "WProgram.h"
void stop();
void izquierda(int iz);
void derecha(int der);
void retrocede(int re);
void avanza(int av);
void setup();
void loop();
Servo servoA, servoB, servoC;

int IR = 8; 
int zumbador =11;
int camara = 4;
int girocam = 2;
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;

void stop()
{
  servoA.write(91); //Parado
  servoB.write(90); //Parado
}

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

void derecha(int der) 
{ 
  servoB.write(der);
}
 
void retrocede(int re) 
{ 
  stop();
  servoA.write(re);
  servoB.write(180-re);
} 

void avanza(int av) 
{ 
  stop();
  servoA.write(180-av);
  servoB.write(av);

} 

void setup() 
{ 
  Serial.begin(19200); 
  delay(100); 
  
  analogWrite(zumbador,0);
  
  servoA.attach(speed1);
  //servo1.setMaximumPulse(2200);
  servoB.attach(speed2);
  servoC.attach(girocam);
  
  stop();
  
  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()) {
    //Serial.print(val, BYTE);
    val = Serial.read();
    if ((val>>4)==id)
    {
      Serial.print(val, BYTE);
      
      switch (val&0x0F) {
        case 0x00: //Servo derecho avanza
          digitalWrite(act_serv,  LOW);
          derecha(180);
        break;
        case 0x01: //Servo izquierdo avanza
          digitalWrite(act_serv,  LOW);
          izquierda(0);
        break;
        case 0x02: //Servo derecho retrocede
        {
          digitalWrite(act_serv,  LOW);
          derecha(0);
        }
        break;
        case 0x03: //Servo izquierdo retrocede
        {
          digitalWrite(act_serv,  LOW);
          izquierda(180);
        }
        break;
        case 0x04:
        {
          frec= 100;
          activa=1;
          time_ant=millis();
        }
        break;
        case 0x05:
        {
          activa=0;
          analogWrite(zumbador,0);
        }
        break;
        case 0x06: //IR ON
        {
          digitalWrite(IR,  HIGH);
        }
        break;
        case 0x07:  //IR OFF
        {
          digitalWrite(IR,  LOW);
        }
        break;
        case 0x08: //laser ON
        {
          digitalWrite(laser,  HIGH);
          digitalWrite(led,  HIGH); //Apaga leds
        }
        break;
        case 0x09: //laser OFF
        {
          digitalWrite(laser,  LOW);
            digitalWrite(led,  LOW); //Apaga leds
        }
        break;
        case 0x0A: //Camara derecha
        {
          digitalWrite(act_serv,  LOW);
          servoC.write(180);
        }
        break;
        case 0x0B: //Camara izquierda
        {
          digitalWrite(act_serv,  LOW);
          servoC.write(0);
        }
        break;
        case 0x0C: //Parada de los servomotores
          digitalWrite(act_serv,  HIGH);
          stop();
        break;
        case 0x0D: //Enciende camara
          digitalWrite(camara,  HIGH);
        break;
        case 0x0E: //Apaga camara
          digitalWrite(camara,  LOW);
        break;
        case 0x0F: //Camara centrada
          digitalWrite(act_serv,  LOW);
          servoC.write(93); //Parado
        break;
      }
   }
  }
  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);
  }
} 


int main(void)
{
	init();

	setup();
    
	for (;;)
		loop();
        
	return 0;
}