Sketch para el control de robot controlado por ultrasonidos

#include <Servo.h>

Servo servoMotor;
int pinE1=13;
int pinI1=12;
int pinI2=11;
int pinE2=7;
int pinI3=6;
int pinI4=5;
const int PinTrig=10;
const int PinEcho=9;
const float VelSon=34000.0;
float distancia;
int distancia_45;
int distancia_90;
int distancia_135;

void setup(){
pinMode(pinE1,OUTPUT);
pinMode(pinI1,OUTPUT);
pinMode(pinI2,OUTPUT);
pinMode(pinE2,OUTPUT);
pinMode(pinI3,OUTPUT);
pinMode(pinI4,OUTPUT);
Serial.begin(9600);
pinMode(PinEcho,INPUT);
pinMode(PinTrig,OUTPUT);
Serial.begin(9600);
servoMotor.attach(3);
}

void adelante(){
digitalWrite(pinE1,HIGH);
digitalWrite(pinI1,HIGH);
digitalWrite(pinI2,LOW);
digitalWrite(pinE2,HIGH);
digitalWrite(pinI3,HIGH);
digitalWrite(pinI4,LOW);
}

void atras(){
digitalWrite(pinE1,HIGH);
digitalWrite(pinI1,LOW);
digitalWrite(pinI2,HIGH);
digitalWrite(pinE2,HIGH);
digitalWrite(pinI3,LOW);
digitalWrite(pinI4,HIGH);
}

void girarDerecha(){
digitalWrite(pinE1,HIGH);
digitalWrite(pinI1,HIGH);
digitalWrite(pinI2,LOW);
digitalWrite(pinE2,HIGH);
digitalWrite(pinI3,LOW);
digitalWrite(pinI4,HIGH);
}

void girarIzquierda(){
digitalWrite(pinE1,HIGH);
digitalWrite(pinI1,LOW);
digitalWrite(pinI2,HIGH);
digitalWrite(pinE2,HIGH);
digitalWrite(pinI3,HIGH);
digitalWrite(pinI4,LOW);
}

void parar(){
digitalWrite(pinE1,LOW);
digitalWrite(pinI1,HIGH);
digitalWrite(pinI2,LOW);
digitalWrite(pinE2,LOW);
digitalWrite(pinI3,HIGH);
digitalWrite(pinI4,LOW);
}

long iniciarTrigger(){
digitalWrite(PinTrig,LOW);
delayMicroseconds(200);
digitalWrite(PinTrig,HIGH);
delayMicroseconds(100);
digitalWrite(PinTrig,LOW);
unsigned long tiempo= pulseIn(PinEcho,HIGH);
distancia=tiempo*0.000001*VelSon/2.0;
return(distancia);
}

void loop() {

distancia_90=iniciarTrigger();
servoMotor.write(90);
Serial.print(distancia);
Serial.print("cm");
Serial.println();

delay(100);
while(distancia_90>20)

{
adelante();
distancia_90=iniciarTrigger();
}
if(distancia_90<40)

{
delay(100);
parar();
delay(1000);
servoMotor.write(45);
distancia_45=iniciarTrigger();
delay(500);
servoMotor.write(135);
distancia_135=iniciarTrigger();
delay(500);
if(distancia_45>distancia_135)

{
girarDerecha();
delay(350);
}
else

{
girarIzquierda();
delay(350);
}
}
}