How come my code does not wait for a specific time before going into an if statement?

Question

I have an HC-SR04 attached to a servo. When the 90 degree reading is less than 30 cm, I want the motor to take 2 seconds to turn to 130 degrees, get a measurement on the right, and turn back to 90 degrees. My code does not seem to work. Even though I tell it to wait for 2000 ms, it skips directly to the if statement where the turning to 90 degrees is done. Here is the main code:

 #include <Servo.h>
#define trig 22
#define echo 24
long duration;
boolean turned130 = false;
int cm50, cm, cm130;
Servo servo;
boolean checkRight = false;
boolean mainPing = true;
unsigned long counter;
unsigned long servoTimer = millis();
void setup() {
  // put your setup code here, to run once:
Serial.begin(9600);
pinMode(echo, INPUT);
pinMode(trig, OUTPUT);
servo.attach(12);
servo.write(90);
delay(2000);

}

void loop(){
counter=millis();//my timer
  if(((counter-servoTimer) >= 250) && mainPing){//distance measured 4 times a second
    ping();
    cm=duration/29.1/2;
    Serial.println(cm);
    servoTimer=counter;//this if statement works fine
  }
  if((cm<30) && mainPing){

    mainPing=false;//makes the 90 degree ping and this if statement unable to run
    checkRight = true;//enables next if statement to run
    servoTimer=millis();
    servo.write(130);
    cm=30;//so that this if statement does not get called again until distance is obtained
    Serial.println("Inside less than 30 loop");//I added this for debugging, to let me know when the loop is called


  }
  if(checkRight && ((counter-servoTimer) >= 2000)){//after 2 seconds passes

    checkRight=false;
    ping();
    cm130 = duration/29.1/2;
    mainPing = true;//enable to other 2 if statements to run
    servoTimer=millis();
    Serial.println(cm130);
    servo.write(90);//turn motor back
    Serial.println("Inside other loop");//for debugging


  }



  delay(2);
  }

void ping(){

    digitalWrite(trig, LOW);
    delayMicroseconds(2);
    digitalWrite(trig, HIGH);
    delayMicroseconds(10);
    digitalWrite(trig, LOW);
    duration = pulseIn(echo, HIGH);


}

This is what I got in the Serial monitor:

25//distance triggers 2nd if statement
Inside less than 30 loop
261
Inside other loop
13
Inside less than 30 loop
14
Inside other loop
14
Inside less than 30 loop
14
Inside other loop
3242
13
Inside less than 30 loop
14
Inside other loop

This all happened within milliseconds. There was no 2 second pause and the servo never turned. What is wrong with my code that prevents the motor from turning and waiting 2 seconds? The order of the Serial messages is correct, but it is happening fast.

EDIT:

When I remove the Serial messages and disconnect from the computer, it kind of works. By that, I mean it turns and waits for 2 seconds 50% of the time. Other times, the servo just shakes a little bit and stops.

Another thing I noticed is that if I leave only Serial.println(cm) message and Serial.println("Inside less than 30 loop"), this is the result that I observe.

    12
    13
    Inside less than 30 loop

    15
    3
    Inside less than 30 loop

For some reason the loop does not get triggered the first time. Is there a possibility that I have a defective Arduino Mega?

EDIT 2:

I tested the same setup on an official Arduino Uno and it did not work either. This means that I do not have a broken Arduino. Could the servo motor be broken? That won't explain the Serial messages though.

Answer 1

I added some more debugging, and I think I have your issues detected.

Amended code:

#include <Servo.h>
#define trig 22
#define echo 24
long duration;
boolean turned130 = false;
int cm50, cm, cm130;
Servo servo;
boolean checkRight = false;
boolean mainPing = true;
unsigned long counter;
unsigned long servoTimer = millis();
void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  pinMode(echo, INPUT);
  pinMode(trig, OUTPUT);
  servo.attach(12);
  servo.write(90);
  delay(2000);
}

void loop() {
  counter = millis(); //my timer
  if (((counter - servoTimer) >= 250) && mainPing) { //distance measured 4 times a second
    Serial.print ("Doing ping at time ");
    Serial.println (counter);
    ping();
    cm = duration / 29.1 / 2;
    Serial.println(cm);
    servoTimer = counter; //this if statement works fine
  }
  if ((cm < 30) && mainPing) {

    mainPing = false; //makes the 90 degree ping and this if statement unable to run
    checkRight = true;//enables next if statement to run
    servoTimer = millis();
    servo.write(130);
    cm = 30; //so that this if statement does not get called again until distance is obtained
    Serial.println("Inside less than 30 loop");//I added this for debugging, to let me know when the loop is called
  }
  if (checkRight && ((counter - servoTimer) >= 2000)) { //after 2 seconds passes
    Serial.print ("counter = ");
    Serial.println (counter);
    Serial.print ("servoTimer = ");
    Serial.println (servoTimer);
    checkRight = false;
    ping();
    cm130 = duration / 29.1 / 2;
    mainPing = true;//enable to other 2 if statements to run
    servoTimer = millis();
    Serial.print ("cm130 = ");
    Serial.println(cm130);
    servo.write(90);//turn motor back
    Serial.println("Inside other loop");//for debugging
  }
  delay(2);
}

void ping() {
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  duration = pulseIn(echo, HIGH);
}

With no server (or anything) attached I get this output:

Doing ping at time 1999
0
Inside less than 30 loop
counter = 1999
servoTimer = 2693
cm130 = 0
Inside other loop

First note that servoTimer is somewhat higher than counter because the pulseIn function takes a while to execute (times out after 1 second allegedly, although that was less than a second).

Now we come to the problem "if" statement.

  if (checkRight && ((counter - servoTimer) >= 2000)) { //after 2 seconds passes

That can't be right, eh? counter is 1999 and servoTimer is 2693, and there is not 2000 difference. However you are doing the maths the other way:

counter - servoTimer =  1999 - 2693 = -694

Since we are using unsigned long, it cannot hold negative numbers so the result of the subtraction is 4294966602.

That is much larger than 2000, so the "if" branch is taken.