#include <Servo.

h>
#include <SoftwareSerial.h>
#include <Ultrasonic.h>

Ultrasonic ultrasonic(9, 12);

SoftwareSerial TXRX(10,11);
String readvoice;
Servo servopin;

#define pin1 4
#define pin2 5
#define port1 3

#define pin3 7
#define port2 6
#define pin4 8

int ler;
int distance;
char lido;

void parar_motor(){
digitalWrite(pin1, LOW);
digitalWrite(pin4, LOW);
digitalWrite(pin2, LOW);
digitalWrite(pin3, LOW);
}

void tras(){
servopin.write(60);
 digitalWrite(pin1, HIGH);
digitalWrite(pin4, HIGH);
digitalWrite(pin2, LOW);
digitalWrite(pin3, LOW);
}

void frente(){
digitalWrite(pin1, LOW);
digitalWrite(pin4, LOW);
digitalWrite(pin2, HIGH);
digitalWrite(pin3, HIGH);
}

void esquerda(){
analogWrite(port1, 120);
analogWrite(port2, 120);
digitalWrite(pin1, LOW);
digitalWrite(pin4, HIGH);
digitalWrite(pin2, HIGH);
digitalWrite(pin3, LOW);
delay(500);
}

void direita(){
analogWrite(port1, 120);
analogWrite(port2, 120);
digitalWrite(pin1, HIGH);
digitalWrite(pin4, LOW);
digitalWrite(pin2, LOW);
digitalWrite(pin3, HIGH);
delay(500);
}

void setup() {
Serial.begin(9600);
TXRX.begin(9600);
servopin.attach(13);

pinMode(pin1, OUTPUT);
pinMode(port1, OUTPUT);
pinMode(pin2, OUTPUT);

pinMode(pin3, OUTPUT);
pinMode(port2, OUTPUT);
pinMode(pin4, OUTPUT);

pinMode(13, OUTPUT);

digitalWrite(pin1, LOW);
digitalWrite(pin2, LOW);
digitalWrite(pin3, LOW);

digitalWrite(pin4, LOW);
analogWrite(port1, 150);
analogWrite(port2, 150);

servopin.write(60);
}

void loop() {
// Pass INC as a parameter to get the distance
in inches
distance=ultrasonic.read();
lido=Serial.read();
ler=TXRX.read();

delay(20);

if(distance>=20){

switch(ler){
case '0': // parar
parar_motor();
break;

case '1': // para frente

frente();
break;

case '2': // para direita

servopin.write(0);
delay(1000);
distance=ultrasonic.read();
servopin.write(60);
delay(1000);
if (distance>=20){
direita();
parar_motor();

delay(200);
frente();
delay(500);
 parar_motor();
delay(400);
esquerda();
parar_motor();
delay(400);
frente();
}
break;

case '3': // para esquerda

servopin.write(120);
delay(1000);
distance=ultrasonic.read();
servopin.write(60);
delay(1000);
if (distance>=20){
esquerda();
parar_motor();

delay(200);
frente();
delay(500);
parar_motor();
delay(400);
direita();
parar_motor();
delay(400);
frente();
}
break;
case '4': // para tras
tras();
break;

case '5': // para direita

servopin.write(0);
delay(1000);
distance=ultrasonic.read();
servopin.write(60);
delay(1000);
if (distance>=20){
direita();
delay(300);
parar_motor();
frente();
}
break;

case '6': // para esquerda

servopin.write(120);
delay(1000);
distance=ultrasonic.read();
servopin.write(60);
delay(1000);
if (distance>=20){
esquerda();
delay(300);
parar_motor();
frente();
}
break;}
}
if (distance<=19){
tras();
delay(500);
parar_motor();
}
}