I'm writing a code in which I'm supposed to take two readings (pings) for the ultrasonic sensor HC-SR04. My objective is to take two samples consecutively and measure the difference and slow down my robot vehicle if it goes beyond a certain threshold. The first reading is coming back fine, but the second reading gives me very weird values unless you're reasonably far away. I didn't put the wheels on my robot so it's stationary but the motors are still functional. This is my code:
#include "AFMotor.h"
#define trigPin 12
#define echoPin 13
AF_DCMotor motor1(1, MOTOR12_64KHZ);
AF_DCMotor motor2(2, MOTOR12_64KHZ);
void setup() {
Serial.begin(9600); // begin serial communitication
Serial.println("Motor test!");
pinMode(trigPin, OUTPUT);// set the trig pin to output (Send sound waves)
pinMode(echoPin, INPUT);// set the echo pin to input (recieve sound waves)
motor1.setSpeed(180); //set the speed of the motors, between 0-255
motor2.setSpeed(180);
}
void loop() {
long duration1, distance1,distance2,duration2; // start the scan
digitalWrite(trigPin, LOW);
delayMicroseconds(2); // delays are required for a succesful sensor operation.
digitalWrite(trigPin, HIGH);
delayMicroseconds(10); //this delay is required as well!
digitalWrite(trigPin, LOW);
duration1 = pulseIn(echoPin, HIGH);
distance1 = (duration1/2) / 29.1;// convert the distance to centimeters.
digitalWrite(trigPin, LOW);
delayMicroseconds(2); // delays are required for a succesful sensor operation.
digitalWrite(trigPin, HIGH);
delayMicroseconds(10); //this delay is
digitalWrite(trigPin, LOW);
duration2 = pulseIn(echoPin, HIGH);
distance2 = (duration2/2) / 29.1;
if (abs(distance1-distance2) > 1 && distance1 < 25){
Serial.println(distance1); Serial.println("\n"); Serial.println(distance2); Serial.println("\n");
Serial.println(abs(distance1-distance2));
Serial.print ( " CM! \n");// print out the distance in centimeters.
motor1.setSpeed(220);
motor2.setSpeed(220);
motor1.run(FORWARD);
motor2.run (FORWARD);
delay(3500);
}
else {
Serial.println ("No obstacle detected. going forward");
delayMicroseconds(15);
motor1.setSpeed(180);
motor2.setSpeed(180);
motor1.run(FORWARD); //if there's no obstacle ahead, Go Forward!
motor2.run(FORWARD);
}
}
This is my output in run time when I put my hand in front of the sensor and my hand stays in the same place:
First time when I put my hand at somewhere around 25 cm:
24
50
26
CM!
When I put my hand at 5 cm:
First sample:
5
12
7
CM!
Now it gets really messed up. Second sample:
7
3587
3580
CM!
Only sometimes I get normal values, like this:
24
29
5
CM!
This is when my hand is at 25 cm.
I suspect it's the delay in the code between each sample the sensor takes which is causing this problem. I feel as if the sensor doesn't completely finish taking the first sample and simply adds it to the second sample at the hardware level. I need to be able to get good values. But I don't know how to get around this. I need reasonably accurate values. Please help. I'm using only one sensor.
Moderator edit: CODE TAGS DAMMIT!