---

void setup() {
  pinMode(4, OUTPUT);
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(7, OUTPUT);
  Serial.begin(9600);
}
void go(int L, int R) {
  if(L>0) digitalWrite(4,0); else digitalWrite(4,1);
  if(R>0) digitalWrite(7,0); else digitalWrite(7,1);
  analogWrite(5, abs(L)); analogWrite(6, abs(R));
}

float I=0;
int Mo = 100;
float Kp = 0.1;
float Ki = 0;
float Kd = 2;
int porog = 500;

void left() {
  go(118, 100);
  delay(150);
  go(-90,90);
  while(analogRead(A0) > porog) go(-90,90);
  while(analogRead(A1) > porog) go(-90,90);
  go(-90,90);
  delay(50);
  go(0,0);
  delay(50);
}

void pid() {
  int d1 = analogRead(A0);
  int d2 = analogRead(A1);
  int Ep = d1 - d2;
  I = I*0.7 + Ep*0.3;
  float D = Ep - I;
  float U = Ep*Kp + I*Ki + D*Kd;
  int M1 = (Mo + U)*1.18;
  M1=constrain(M1,-255,255);
  int M2 = Mo - U;
  M2=constrain(M2,-255,255);
  go(M1,M2);
}

void loop() {
  if(analogRead(A0) < porog and analogRead(A1) < porog) left(); else pid();
}