#include "WProgram.h"
void setup();
void loop();
int IR = 6; 
int zumbador = 8;
int camara = 11;
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 g1a = 2;
int g1b = 3;
int g2a = 5;
int g2b = 4;
int id = 0;

/*void stop()
{
  digitalWrite(g1a,  LOW);
  digitalWrite(g1b,  LOW);
  analogWrite(speed1, 0);
  digitalWrite(g2a,  LOW);
  digitalWrite(g2b,  LOW);
  analogWrite(speed2, 0);
}

void izquierda(int iz) 
{ 
  stop();
  delay(100);
  digitalWrite(g1a,  HIGH);
  digitalWrite(g1b,  LOW);
  digitalWrite(g2a,  LOW);
  digitalWrite(g2b,  LOW);
  analogWrite(speed2, 0);
  for (int i=64; i <= iz; i=i+10){
      analogWrite(speed1, i);
      delay(100);
  } 
  analogWrite(speed1, iz);
}

void derecha(int der) 
{ 
  stop();
  delay(100);
  digitalWrite(g2a,  HIGH);
  digitalWrite(g2b,  LOW);
  digitalWrite(g1a,  LOW);
  digitalWrite(g1b,  LOW);
  analogWrite(speed1, 0);
  for (int i=64; i <= der; i=i+10){
      analogWrite(speed2, i);
      delay(100);
  } 
  analogWrite(speed2, der);
}
 
void retrocede(int av) 
{ 
  stop();
  delay(100);
  digitalWrite(g2a,  HIGH);
  digitalWrite(g2b,  LOW);
  digitalWrite(g1a,  HIGH);
  digitalWrite(g1b,  LOW);
  for (int i=64; i <= av; i=i+10){
      analogWrite(speed1, i);
      analogWrite(speed2, i);
      delay(100);
  } 
  analogWrite(speed2, av);
  analogWrite(speed1, av);
} 

void avanza(int re) 
{ 
  stop();
  delay(100);
  digitalWrite(g2a,  LOW);
  digitalWrite(g2b,  HIGH);
  digitalWrite(g1a,  LOW);
  digitalWrite(g1b,  HIGH);
  for (int i=64; i <= re; i=i+10){
      analogWrite(speed1, i);
      analogWrite(speed2, i);
      delay(100);
  } 
  analogWrite(speed2, re);
  analogWrite(speed1, re);
} */

void setup() 
{ 
  Serial.begin(9600); 
  delay(100); 
  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() 
{ 
  //digitalWrite(camara,  HIGH);
  if (Serial.available()) {
    val = Serial.read();
    //Serial.print(val, BYTE);
    Serial.print(val);
  }
   /*if (Serial.available()) {
    val = Serial.read();
    Serial.print(val, BYTE);
    if ((val&0xF0)==id)
    {
      Serial.print(val, BYTE);
      
      switch (val&0x0F) {
        case 0x00:
          avanza(255);
        break;
        case 0x01:
          retrocede(255);
        break;
        case 0x02:
        {
          izquierda(255);
          derecha(0);
        }
        break;
        case 0x03:
        {
          derecha(255);
          izquierda(0);
        }
        break;
        case 0x04:
        {
          derecha(180);
          izquierda(255);
        }
        break;
        case 0x05:
        {
          derecha(128);
          izquierda(255);
        }
        break;
        case 0x06:
        {
          derecha(80);
          izquierda(255);
        }
        break;
        case 0x07:
        {
          derecha(64);
          izquierda(255);
        }
        break;
        case 0x08:
        {
          izquierda(180);
          derecha(255);
        }
        break;
        case 0x09:
        {
          izquierda(128);
          derecha(255);
        }
        break;
        case 0x0A:
        {
          izquierda(80);
          derecha(255);
        }
        break;
        case 0x0B:
        {
          izquierda(64);
          derecha(255);
        }
        break;
        case 0x0C:
          stop();
        break;
        case 0x0D:
          digitalWrite(camara,  HIGH);
        break;
        case 0x0E:
          digitalWrite(camara,  LOW);
        break;
      }
   }
  }*/
} 


int main(void)
{
	init();

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