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.

• Gerben: Try adding some parentheses; if( checkRight && ((counter-servoTimer)>=2000) )
• shurup: @Gerben I added parentheses. When I removed the Serial messages, it kind of worked like 50% of the time. When I add them back in, the same thing happens. There is no interval between the messages and the servo just shakes, but does not rotate.
• Gerben: Could you post the entire sketch? Make sure servoTimer isn't a local variable.
• shurup: @Gerben I posted the entire sketch.
• Nick Gammon: That isn't the whole sketch. The lines counter=millis();//my timer onwards are outside any function. Try compiling what you posted.

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.

Unsigned long calculation

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

• shurup: Your answer makes a lot of sense. What I did to address the issue is put counter=millis(); after each ping before I set the servoTimer. This seemed to fix the issue. Works really well now! Now I have to make it turn left as well and record distances. It should not be a problem with this issue taken care of. Quick question, what is "<!-- language: lang-c++ -->" for? Is it necessary? Thank you for your help!
• Nick Gammon: The language tag is just a hint to Stack Exchange to syntax-colour the code. Look at the posts. Mine is coloured, yours isn't. :) However I notice now that I put it in the wrong place. I must have hit "paste" at the wrong time. I've edited it out of the code.