#include "WProgram.h"
void stop();
void izquierda(int iz);
void derecha(int der);
void retrocede(int av);
void avanza(int re);
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;
int val = 0;
int speed1 = 9;
int speed2 = 10;
int g1a = 2;
int g1b = 3;
int g2a = 5;
int g2b = 4;
int i=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=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;
  val=val1+val2+val3+val4;
  Serial.print("Hola, soy el robot numero ");
  Serial.println(val);
  
} 

void loop() 
{ 
   if (Serial.available()) {
    val = Serial.read();
    if (val == 'W') {
      avanza(255);
       Serial.println("Avanzo");
    } 
    if (val == 'Z') {
      retrocede(255);
      Serial.println("Retrocedo");
    }
    if (val == 'A') {
      izquierda(255);
      Serial.println("Giro Izquierda");
    }
    if (val == 'S') {
      derecha(255);
      Serial.println("Giro Derecha");
    }
    if (val == 'P') {
      stop();
    }
  }
} 


int main(void)
{
	init();

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