#include <Servo.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C lcd(0x27,16,2); 
Servo s1;
Servo s2;
Servo s3;
int s1o=8;
int s2o=7;
int pingpin=2;
int cm=0;
int abst[6];
int winkel[]={130,110,90,70,50};
int pos=0;
int aufab=1;
int count=0;

void setup(){
  s1.attach(9);
  s2.attach(10);
  s3.attach(4);
  pinMode(s1o, OUTPUT);
  pinMode(s2o, OUTPUT);
  lcd.init();
  lcd.backlight();
  schaurum(200);
  schaurum(100);
}

void loop(){
  pos=pos+aufab;
  if (pos==5){
    pos=3;
    aufab=-aufab;
  }
  if (pos==-1){
    pos=1;
    aufab=-aufab;
  }

  abst[pos]=mess(winkel[pos]);

  if ((abst[1]<20 && abst[3]<20) || abst[0]<10 || abst[4]<10){
    fahr(-40,-40,500);
    if (abst[0]< abst[4]){
      do{
        fahr(-60,60,300);
        schaurum(40);
        count++;
      } 
      while (abst[0]<5 || abst[1]<10 || abst[2]<20 || abst[3]<10 || abst[4]<5);
    }
    else{
      do{
        fahr(60,-60,300);
        schaurum(40);
        count++;
      } 
      while (abst[0]<5 || abst[1]<10 || abst[2]<20 || abst[3]<10 || abst[4]<5);
    }
    count=0;
  }  
  else
  {
    if (abst[0]<20 && abst[1]>40 && abst[2]>40 && abst[3]>40 && abst[4]>40){
      fahr(50,0,0);
    }

    if (abst[0]>40 && abst[1]>40 && abst[2]>40 && abst[3]>40 && abst[4]<20){
      fahr(0,50,0);
    }

    if (abst[0]>40 && abst[1]>40 && abst[2]>40 && abst[3]>40 && abst[4]>40){
      fahr(50,50,0);
    }

    if (abst[0]>70 && abst[1]>70 && abst[2]>70 && abst[3]>70 && abst[4]>70){
      fahr(60,60,0);
    }    

    if (abst[1]<40 || abst[2]<40 || abst[3]<40){
      if(abst[0]+abst[1]<abst[3]+abst[4]){
        fahr(50,30,0);
      }
      else{
        fahr(30,50,0);
      }
    }
  }
}

int mess(int a){
  s3.write(a);
  delay(80);
  schreib();
  long del;
  pinMode(pingpin, OUTPUT);
  digitalWrite(pingpin, LOW);
  delayMicroseconds(2);
  digitalWrite(pingpin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pingpin, LOW);
  pinMode(pingpin, INPUT);
  del = pulseIn(pingpin, HIGH);
  cm=del/78;
  return cm;
}

void schaurum(int srzeit){
  fahr(0,0,0);
  for (int j=0;j<5;j++){
    abst[j]=mess(winkel[j]);
    delay(srzeit);
  }
}

void fahr(int li, int re, int zeit){

  sterne(li,re);
  if (abs(li)>5) digitalWrite(s1o,1);
  else digitalWrite(s1o,0);
  if (abs(re)>5) digitalWrite(s2o,1);
  else digitalWrite(s2o,0);
  if (li>100) li=100;
  if (re>100) re=100;
  li *=0.35;
  re *=0.35;
  s1.write(90+li);
  s2.write(90-re);
  delay(zeit);  
}

void schreib(){
  lcd.setCursor(0,0);  
  for(int i=0;i<6;i++){
    lcd.print(" ");
    if (abst[i]>99){ 
      lcd.print("99");
    }
    if (abst[i]<10){ 
      lcd.print(" ");
    }
    if (abst[i]<100){ 
      lcd.print(abst[i]);
    }
  } 
}

void sterne(int sli, int sre){
  lcd.setCursor(0,1);
  for (int k=0;k<7;k++){
   if (sli>=10*(k+1)) lcd.print ("*");
   else if (sli<0 && abs(sli)>=10*(k+1)) lcd.print ("-");
   else lcd.print(" ");
   }
   lcd.print (" ");
   for (int k=7;k>=0;k--){
   if (sre>=10*(k+1)) lcd.print ("*");
   else if (sre<0 && abs(sre)>=10*(k+1)) lcd.print ("-");
   else lcd.print(" ");
   }
}