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

Servo servoA, servoB, servoC;

int IR = 6; 
int zumbador =11;
int camara = 8;
int girocam = 3;
int bit1 = 0;
int bit2 = 1;
int bit3 = 2;
int bit4 = 3;
int val1 = 0;
int val2 = 0;
int val3 = 0;
int val4 = 0;
byte val = 0;
int speed1 = 9;
int speed2 = 10;
int id = 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(9600); 
  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=16;
  else val1=0;
  if (analogRead(bit2)>512) val2=32;
  else val2=0;
  if (analogRead(bit3)>512) val3=64;
  else val3=0;
  if (analogRead(bit4)>512) val4=128;
  else val4=0;
  id=val1+val2+val3+val4;
  Serial.print("Hola, soy el robot numero ");
  Serial.println(id);
  
} 

void loop() 
{ 
   
   for(val = 1 ; val <= 180; val++) 
   {
     servoC.write(val); 
     Servo::refresh();  
     delay(15); 
   }
   for(val = 180 ; val > 0; val--) 
   {
     servoC.write(val);  
     Servo::refresh();  
     delay(15); 
   }
}