BugBot WiredLCD

#include <Servo.h>
#include <LiquidCrystal.h>
#include "Ultrasonic.h"
#define echoPin 13
#define trigPin 7
Servo frontServo;
Servo rearServo;
int centerPos = 90;
int frontRightUp = 72;
int frontLeftUp = 108;
int backRightForward = 75;
int backLeftForward = 105;
int walkspeed = 500;
int centerTurnPos = 81;
int frontTurnRightUp = 63;
int frontTurnLeftUp = 117;
int backTurnRightForward = 66;
int backTurnLeftForward = 96;
int botao1 = 0;
int botao2 = 1;
int botao3 = 8;
int botao4 = 9;
int estado1 = 0;
int estado2 = 0;
int estado3 = 0;
int estado4 = 0;
LiquidCrystal lcd(12, 11, 5, 4, 3, 2);
Ultrasonic ultrasonic(7,13);

void center()
{
  frontServo.write(centerPos);
  rearServo.write(centerPos);
}
void moveForward()
{
  frontServo.write(frontRightUp);
  rearServo.write(backLeftForward);
  delay(125);
  frontServo.write(centerPos);
  rearServo.write(centerPos);
  delay(65);
  frontServo.write(frontLeftUp);
  rearServo.write(backRightForward);
  delay(125);
  frontServo.write(centerPos);
  rearServo.write(centerPos);
  delay(65);
}
void moveBackRight()
{
  frontServo.write(frontRightUp);
  rearServo.write(backRightForward-6);
  delay(125);
  frontServo.write(centerPos);
  rearServo.write(centerPos-6);
  delay(65);
  frontServo.write(frontLeftUp+9);
  rearServo.write(backLeftForward-6);
  delay(125);
  frontServo.write(centerPos);
  rearServo.write(centerPos);
  delay(65);
}
void moveTurnLeft()
{
  frontServo.write(frontTurnRightUp);
  rearServo.write(backTurnLeftForward);
  delay(125);
  frontServo.write(centerTurnPos);
  rearServo.write(centerTurnPos);
  delay(65);
  frontServo.write(frontTurnLeftUp);
  rearServo.write(backTurnRightForward);
  delay(125);
  frontServo.write(centerTurnPos);
  rearServo.write(centerTurnPos);
  delay(65);
}
void setup()
{
  pinMode(botao1, INPUT);
  pinMode(botao2, INPUT);
  pinMode(botao3, INPUT);
  pinMode(botao4, INPUT);
  Serial.begin(9600);
  pinMode(echoPin, INPUT);
  pinMode(trigPin, OUTPUT);
  lcd.begin(16, 2);
  lcd.print("Distancia:");
  frontServo.attach(10);
  rearServo.attach(6);
}
void loop()
{
  delayMicroseconds(2);
  //seta o pino 12 com pulso alto "HIGH" ou ligado ou ainda 1
  digitalWrite(trigPin, HIGH);
  //delay de 10 microssegundos
  delayMicroseconds(10);
  //seta o pino 12 com pulso baixo novamente
  digitalWrite(trigPin, LOW);
  // função Ranging, faz a conversão do tempo de
  //resposta do echo em centimetros, e armazena
  //na variavel distancia
  int distancia = (ultrasonic.Ranging(CM));
  Serial.print("Distancia em CM: ");
  Serial.println(distancia); //vai exibir a distância no monitor serial também, além de no LCD
  delay(1000);
  lcd.setCursor(0, 1);
  lcd.print(distancia);
  estado1 = digitalRead(botao1);
  estado2 = digitalRead(botao2);
  estado3 = digitalRead(botao3);
  estado4 = digitalRead(botao4);
  if (estado1 == HIGH) {
    moveForward();
    delay(walkspeed);
  }
  if (estado2 == HIGH) {
    moveBackRight();
    delay(walkspeed);
  }
  if (estado3 == HIGH) {
    moveTurnLeft();
    delay(walkspeed);
  }
  if (estado4 == HIGH) {
    center();
    delay(walkspeed);
  }
  else {
    moveForward();
  }
}