#include <Wire.h>
#include <Servo.h>
#include <LiquidCrystal_PCF8574.h>
const int trigPin = 12;
const int echoPin = 11;

LiquidCrystal_PCF8574 lcd(0x3F);

int duration, distance, dirRotate =1,i;
  Servo m1,m2;
void setup()
{

  m1.attach( 5 );
  m2.attach( 6 );

  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  Serial.begin(115200);
  while (! Serial);
  Wire.begin();
  Wire.beginTransmission(0x3F);

  lcd.begin(16, 2);
}
int getDist(){

     digitalWrite(trigPin, LOW);
     delayMicroseconds(2);

     digitalWrite(trigPin, HIGH);
     delayMicroseconds(10);

     digitalWrite(trigPin, LOW);
     return (pulseIn(echoPin, HIGH) / 58.2);

  }
void loop()
{
    lcd.setBacklight(255);
    lcd.home();
          lcd.print( "Oh crap! i need" );
    distance = getDist();
    if(distance < 20 )
    {
      lcd.print( "Oh crap! i need" );
      lcd.setCursor(0,1);
      lcd.print( "to change my way" );

        if( dirRotate == 1 ){
              m1.writeMicroseconds(1000);
              m1.write(180);
             dirRotate = 0;
        }else{
          m2.writeMicroseconds(1000);
             m2.write(180);
             dirRotate = 1;
        }


    }else{
      lcd.clear();
      lcd.print("Moving fine :D");
      m1.write(0);
      m2.write(180);
    }

    delay(300);
}