#define M1r 3
#define M1b 5
#define M2r 10
#define M2b 11
#define M3r 8
#define M3b 9
#define M4r 12
#define M4b 13

#define SONAR_TRIGGER_PIN 6
#define SONAR_ECHO_PIN 7

String inputString="";

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  
  pinMode(SONAR_TRIGGER_PIN, OUTPUT);
  pinMode(SONAR_ECHO_PIN, INPUT);
 
  pinMode(M1r,OUTPUT);
  pinMode(M1b,OUTPUT);
  pinMode(M2r,OUTPUT);
  pinMode(M2b,OUTPUT);
  pinMode(M3r,OUTPUT);
  pinMode(M3b,OUTPUT);
  pinMode(M4r,OUTPUT);
  pinMode(M4b,OUTPUT);

}

void loop() {
  // put your main code here, to run repeatedly:
  unsigned int ostacolo = distanza();

  if(ostacolo < 30)
  {
    fermo();
    delay(1000);
    indietro();    
    delay(1000);
    destra();
    delay(1000);
    avanti();
    Serial.end();
  }  
else {
    Serial.begin(9600);
    char inChar = (char) Serial.read();
    inputString += inChar;
    Serial.println(inputString);
      if (inputString == "a") {
        avanti();
      }
      else if (inputString == "b") {
        indietro();
      }
      else if (inputString == "c") {
        destra();
      }
      else if (inputString == "d") {
        sinistra();
      }
      else if (inputString == "f") {
        fermo();
      }
inputString = "";
}

}
//funzioni




unsigned int distanza()
{
  digitalWrite(SONAR_TRIGGER_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(SONAR_TRIGGER_PIN, LOW);
  unsigned long pulse_lunghezza = pulseIn(SONAR_ECHO_PIN, HIGH);
  delay(50);
  return((unsigned int) (pulse_lunghezza/58));
  
}


void avanti()
{
  digitalWrite(M1r,HIGH);
  digitalWrite(M1b,LOW);
  digitalWrite(M2r,HIGH);
  digitalWrite(M2b,LOW);
  digitalWrite(M3r,HIGH);
  digitalWrite(M3b,LOW);
  digitalWrite(M4r,HIGH);
  digitalWrite(M4b,LOW);
}

void indietro()
{
  digitalWrite(M1r,LOW);
  digitalWrite(M1b,HIGH);
  digitalWrite(M2r,LOW);
  digitalWrite(M2b,HIGH);
  digitalWrite(M3r,LOW);
  digitalWrite(M3b,HIGH);
  digitalWrite(M4r,LOW);
  digitalWrite(M4b,HIGH);
}

void destra()
{
  digitalWrite(M1r,LOW);
  digitalWrite(M1b,LOW);
  digitalWrite(M2r,HIGH);
  digitalWrite(M2b,LOW);
  digitalWrite(M3r,LOW);
  digitalWrite(M3b,LOW);
  digitalWrite(M4r,HIGH);
  digitalWrite(M4b,LOW);
}

void sinistra()
{
  digitalWrite(M1r,HIGH);
  digitalWrite(M1b,LOW);
  digitalWrite(M2r,LOW);
  digitalWrite(M2b,LOW);
  digitalWrite(M3r,HIGH);
  digitalWrite(M3b,LOW);
  digitalWrite(M4r,LOW);
  digitalWrite(M4b,LOW);
}

void fermo()
{
  digitalWrite(M1r,LOW);
  digitalWrite(M1b,LOW);
  digitalWrite(M2r,LOW);
  digitalWrite(M2b,LOW);
  digitalWrite(M3r,LOW);
  digitalWrite(M3b,LOW);
  digitalWrite(M4r,LOW);
  digitalWrite(M4b,LOW);
}