Weird motor control

Hi guys, I’m stuck with a Adafruit + Arduino Mega not working properly.
I’m trying to make obstacle avoiding robot and I’m using this code:

#include <AFMotor.h>
#include <Servo.h>
#include <NewPing.h>

#define TRIG_PIN 14
#define ECHO_PIN 15
#define MAX_DISTANCE 200
#define MAX_SPEED 100
#define MAX_SPEED_OFFSET 20 
#define COLL_DIST 10
#define TURN_DIST COLL_DIST+10 
NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE); 

AF_DCMotor motor1(1, MOTOR12_1KHZ); 
AF_DCMotor motor2(4, MOTOR34_1KHZ); 
Servo myservo;  

int leftDistance, rightDistance;
int curDist = 0;
String motorSet = "";
int speedSet = 0;

//-------------------------------------------- SETUP LOOP ----------------------------------------------------------------------------
void setup() {
  myservo.attach(9); 
  myservo.write(90); 
  delay(1000);
 }
//------------------------------------------------------------------------------------------------------------------------------------

//---------------------------------------------MAIN LOOP ------------------------------------------------------------------------------
void loop() {
  myservo.write(90);  
  delay(90);
  curDist = readPing();  
  if (curDist < COLL_DIST) {changePath();} 
  moveForward(); 
  delay(500);
 }
//-------------------------------------------------------------------------------------------------------------------------------------

void changePath() {
  moveStop();   
  myservo.write(36); 
    delay(500);
    rightDistance = readPing();
    delay(500);
    myservo.write(144); 
    delay(700);
    leftDistance = readPing(); 
    delay(500);
    myservo.write(90); 
    delay(100);
    compareDistance();
  }

 
void compareDistance()   
{
  if (leftDistance>rightDistance)
  {
    turnLeft();
  }
  else if (rightDistance>leftDistance) 
  {
    turnRight();
  }
   else 
  {
    turnAround();
  }
}


//-------------------------------------------------------------------------------------------------------------------------------------

int readPing() {
  delay(70);  
  unsigned int uS = sonar.ping();
  int cm = uS/US_ROUNDTRIP_CM;
  return cm;
}
//-------------------------------------------------------------------------------------------------------------------------------------
void moveStop() {motor1.run(RELEASE); motor2.run(RELEASE);} 
//-------------------------------------------------------------------------------------------------------------------------------------
void moveForward() {
    motorSet = "FORWARD";
    motor1.run(FORWARD);    
    motor2.run(FORWARD);     
  for (speedSet = 0; speedSet < MAX_SPEED; speedSet +=2) 
  {
    motor1.setSpeed(speedSet);
    motor2.setSpeed(speedSet+MAX_SPEED_OFFSET);
    delay(5);
  }
}
//-------------------------------------------------------------------------------------------------------------------------------------
void moveBackward() {
    motorSet = "BACKWARD";
    motor1.run(BACKWARD);   
    motor2.run(BACKWARD);    
  for (speedSet = 0; speedSet < MAX_SPEED; speedSet +=2)
  {
    motor1.setSpeed(speedSet);
    motor2.setSpeed(speedSet+MAX_SPEED_OFFSET);
    delay(5);
  }
} 
//-------------------------------------------------------------------------------------------------------------------------------------
void turnRight() {
  motorSet = "RIGHT";
  motor1.run(FORWARD);
  motor2.run(BACKWARD); 
  delay(400);
  motorSet = "FORWARD";
  motor1.run(FORWARD);  
  motor2.run(FORWARD);     
} 
//-------------------------------------------------------------------------------------------------------------------------------------
void turnLeft() {
  motorSet = "LEFT";
  motor1.run(BACKWARD);  
  motor2.run(FORWARD);  
  delay(400); 
  motorSet = "FORWARD";
  motor1.run(FORWARD);   
  motor2.run(FORWARD);  
} 
//-------------------------------------------------------------------------------------------------------------------------------------
void turnAround() {
  motorSet = "RIGHT";
  motor1.run(FORWARD);
  motor2.run(BACKWARD);
  delay(800); 
  motorSet = "FORWARD";
  motor1.run(FORWARD);  
  motor2.run(FORWARD);     
}

It’s working just fine when I turn it on, It’s going forward all nicely, but when first obstacle arrives servo checks left and right distance, decides where to go, but after that all the time motor on M4 is first going backwards for 400ms and then going forward.
I’ve tried switching from M1 to M4 and it stays same, motor on M3 is first going backwards and then forward.
Like sensor is reading all the time that there is obstacle on right side, so he has to go left to avoid it, but there is no obstacle.

How about inserting some Serial.print()s so that you can see the current values of relevant variables such as curDist, rightDistance and leftDistance at pertinet points in the program.