       break;
     case 4:
       robot.rightBack();
-      robot.leftForward();
-      delay_millis = 400;
+      robot.leftStop();
+      delay_millis = 80;
       start_millis = millis();
       status = 5;
       break;
     case 5:
-      dist = robot.distanceToEnemy();
-      if (((millis() - start_millis) >= delay_millis) || (dist > 5)) {
+      if (start_millis + enemy_millis <= millis()){
+        dist = robot.distanceToEnemy();
+        start_millis = millis();
+        if(dist + 2 >= dist_good && dist - 2 <= dist_good){
+          good_count--;
+        } else {
+          good_count = 3;
+          dist_good = dist;
+        }
+        if(dist != 0 && dist_good == 0){
+          dist_good = dist;
+        }
+      }
+      if (((millis() - start_millis) >= delay_millis) || (good_count <= 0 && dist_good > 0 && dist_good < 40)) {
         status = 3;
+        good_count = 3;
       }
       break;
   }

  PCIFR  |= bit(1);
  PCICR  |= bit(1);
  status = 2;
}

ISR (PCINT1_vect)
{
  if (robot.leftEdge() || robot.rightEdge()) {

}

void loop() {
  switch (status) {
    case 1:
      if ((millis() - start_millis) >= delay_millis) {
        status = 3;
      }
