It might well be that you are doing the ultrasonic "pings" to fast after each other.
Sound has a speed of 340 m/second. For travelling a distance of 2x50 cm the time is
1 m / 340 m = 3 milliseconds. But you have multiple echoes. Before creating the next ping you should wait at least 50 milliseconds that the former ping-echoes have faded away.
If you are pinging too fast all the still active echoes disturb the measurings.
Here is a code-version with two added things:
- macros that do serial debugging in different ways
print only once if a value has changed etc.
- has a non-blocking timing-function "TimePeriodIsOver()" which does two things:
a. keep the loop looping very fast
b. execute a ping only once every 100 milliseconds
you can adjust this time by modifying the value of constant myWaitTime
const unsigned long myWaitTime = 100; // milliseconds between ultrasonic pings
change baudrate in the serial monitor to the modern value of 115200
// MACRO-START * MACRO-START * MACRO-START * MACRO-START * MACRO-START * MACRO-START *
// a detailed explanation how these macros work is given in this tutorial
// https://forum.arduino.cc/t/comfortable-serial-debug-output-short-to-write-fixed-text-name-and-content-of-any-variable-code-example/888298
#define dbg(myFixedText, variableName) \
Serial.print( F(#myFixedText " " #variableName"=") ); \
Serial.println(variableName);
#define dbgi(myFixedText, variableName,timeInterval) \
{ \
static unsigned long intervalStartTime; \
if ( millis() - intervalStartTime >= timeInterval ){ \
intervalStartTime = millis(); \
Serial.print( F(#myFixedText " " #variableName"=") ); \
Serial.println(variableName); \
} \
}
#define dbgc(myFixedText, variableName) \
{ \
static long lastState; \
if ( lastState != variableName ){ \
Serial.print( F(#myFixedText " " #variableName" changed from ") ); \
Serial.print(lastState); \
Serial.print( F(" to ") ); \
Serial.println(variableName); \
lastState = variableName; \
} \
}
#define dbgcf(myFixedText, variableName) \
{ \
static float lastState; \
if ( lastState != variableName ){ \
Serial.print( F(#myFixedText " " #variableName" changed from ") ); \
Serial.print(lastState); \
Serial.print( F(" to ") ); \
Serial.println(variableName); \
lastState = variableName; \
} \
}
// MACRO-END * MACRO-END * MACRO-END * MACRO-END * MACRO-END * MACRO-END * MACRO-END *
unsigned long MyTestTimer = 0; // Timer-variables MUST be of type unsigned long
const byte OnBoard_LED = 13; // onboard-LED uno, mega
/*
if ( TimePeriodIsOver(MyTestTimer,1000) ) {
}
*/
unsigned long MyPingTimer = 0; // Timer-variables MUST be of type unsigned long
const unsigned long myWaitTime = 100; // milliseconds between ultrasonic pings
const int M11 = 6;
const int M12 = 5; // RIGHT WHEEL
const int M21 = 9; // LEFT WHEEL
const int M22 = 10;
int const IRB = A0; // labeling front and back sensor for IR
int const IRF = A1;
int const TRIG1 = 2;
int const ECHO1 = 3;
int const TRIG2 = 7;
int const ECHO2 = 8;
long distance;
long distance1;
void setup() {
Serial.begin(115200);
Serial.println("Setup-Start");
PrintFileNameDateTime();
pinMode(TRIG1, OUTPUT);
pinMode(ECHO1, INPUT);
pinMode(TRIG2, OUTPUT);
pinMode(ECHO2, INPUT);
pinMode(M11, OUTPUT);
pinMode(M12, OUTPUT);
pinMode(M21, OUTPUT);
pinMode(M22, OUTPUT);
pinMode(IRF, INPUT);
//Serial.begin(9600);
}
void loop() {
BlinkHeartBeatLED(OnBoard_LED, 250);
int IRFR; // front IR sensor value
int IRBR; // back IR sensor value
long ULTRAF;
long ULTRAB;
ULTRAF = ULTRASONICF(TRIG1, ECHO1);
// print variable NAME and variable-VALUE
// only in case the value has CHANGED
dbgc("01",ULTRAF);
// print variable NAME and variable-VALUE
// only in case the value has CHANGED
ULTRAB = ULTRASONICB (TRIG2 , ECHO2);
dbgc("02",ULTRAB);
// print variable NAME and variable-VALUE
// only in case the value has CHANGED
IRFR = IRCHECKF();
dbgc("03",IRFR);
while ( ULTRAF > 30 && IRFR == 1) {
// check if myWaitTime milliseconds have passed by
if ( TimePeriodIsOver(MyPingTimer, myWaitTime) ) {
// when 100 milliseconds really HAVE passed by do the next ultrasonic ping
Serial.println ("666666");
ULTRAF = ULTRASONICF(TRIG1, ECHO1);
}
IRFR = IRCHECKF();
}
while (1 <= ULTRAF && ULTRAF <= 30 && IRFR == 1) {
// check if myWaitTime milliseconds have passed by
if ( TimePeriodIsOver(MyPingTimer, myWaitTime) ) {
// when myWaitTime milliseconds really HAVE passed by do the next ultrasonic ping
Serial.print("SALMON");
ULTRAF = ULTRASONICF(TRIG1, ECHO1);
IRFR = IRCHECKF();
// don't do pings too fast. This does not work
// the pings will overlap. You have to let time pass by
// until the first pings echoes vanished
// ULTRAF = ULTRASONICF(TRIG1, ECHO1); // commented out
STOP();
}
}
while ( 1 <= ULTRAF && ULTRAF <= 30 && IRFR == 0) {
// check if myWaitTime milliseconds have passed by
if ( TimePeriodIsOver(MyPingTimer, myWaitTime) ) {
// when myWaitTime milliseconds really HAVE passed by do the next ultrasonic ping
ULTRAF = ULTRASONICF(TRIG1, ECHO1);
IRFR = IRCHECKF();
ULTRAF = ULTRASONICF(TRIG1, ECHO1);
Serial.print("cat");
STOP();
}
}
while (IRFR == 0 && ULTRAF > 30) {
// check if myWaitTime milliseconds have passed by
if ( TimePeriodIsOver(MyPingTimer, myWaitTime) ) {
// when myWaitTime milliseconds really HAVE passed by do the next ultrasonic ping
Serial.println ("WOOOOOOOOOOOO");
ULTRAF = ULTRASONICF(TRIG1, ECHO1);
IRFR = IRCHECKF();
STOP();
}
}
}
void FORWARD(int SPEED1, int SPEED2) // drivers motors forward
{
analogWrite(M11, SPEED1);
analogWrite(M12, 0);
analogWrite(M21, 0);
analogWrite(M22, SPEED2);
}
void BACKWARD(int SPEED1, int SPEED2) // drives motors backwards
{
analogWrite(M11, 0);
analogWrite(M12, SPEED1);
analogWrite(M21, SPEED2);
analogWrite(M22, 0);
}
void STOP() // stops motors
{
analogWrite(M11, 0);
analogWrite(M12, 0);
analogWrite(M21, 0);
analogWrite(M22, 0);
}
void TURNLEFT() {
analogWrite(M11, 0);
analogWrite(M12, 0);
analogWrite(M21, 0);
analogWrite(M22, 0);
}
void TURNRIGHT() {
analogWrite(M11, 0);
analogWrite(M12, 0);
analogWrite(M21, 0);
analogWrite(M22, 0);
}
void TURN50() {
analogWrite(M11, 100);
analogWrite(M12, 0);
analogWrite(M21, 100);
analogWrite(M22, 0);
}
int ULTRASONICF(int pingPin, int echoPin) {
long duration, inches, cm;
pinMode(pingPin, OUTPUT);
digitalWrite(pingPin, LOW);
delayMicroseconds(2);
digitalWrite(pingPin, HIGH);
delayMicroseconds(10);
digitalWrite(pingPin, LOW);
pinMode(echoPin, INPUT);
duration = pulseIn(echoPin, HIGH);
cm = microsecondsToCentimeters(duration);
return cm;
}
int ULTRASONICB(int pingPin, int echoPin) {
long duration, inches, cm;
pinMode(pingPin, OUTPUT);
digitalWrite(pingPin, LOW);
delayMicroseconds(2);
digitalWrite(pingPin, HIGH);
delayMicroseconds(10);
digitalWrite(pingPin, LOW);
pinMode(echoPin, INPUT);
duration = pulseIn(echoPin, HIGH);
cm = microsecondsToCentimeters(duration);
return cm;
}
int IRCHECKF() {
int IRFR;
IRFR = digitalRead(IRF);
return IRFR;
}
int IRCHECKB() {
int IRBR;
IRBR = digitalRead(IRB);
return IRBR;
}
long microsecondsToCentimeters(long microseconds) {
return microseconds / 29 / 2;
}
// helper-functions
void PrintFileNameDateTime() {
Serial.println( F("Code running comes from file ") );
Serial.println( F(__FILE__) );
Serial.print( F(" compiled ") );
Serial.print( F(__DATE__) );
Serial.print( F(" ") );
Serial.println( F(__TIME__) );
}
// easy to use helper-function for non-blocking timing
// explanation see here
// https://forum.arduino.cc/t/example-code-for-timing-based-on-millis-easier-to-understand-through-the-use-of-example-numbers-avoiding-delay/974017
boolean TimePeriodIsOver (unsigned long &startOfPeriod, unsigned long TimePeriod) {
unsigned long currentMillis = millis();
if ( currentMillis - startOfPeriod >= TimePeriod ) {
// more time than TimePeriod has elapsed since last time if-condition was true
startOfPeriod = currentMillis; // a new period starts right here so set new starttime
return true;
}
else return false; // actual TimePeriod is NOT yet over
}
void BlinkHeartBeatLED(int IO_Pin, int BlinkPeriod) {
static unsigned long MyBlinkTimer;
pinMode(IO_Pin, OUTPUT);
if ( TimePeriodIsOver(MyBlinkTimer, BlinkPeriod) ) {
digitalWrite(IO_Pin, !digitalRead(IO_Pin) );
}
}
