I know there are other posts about this subject but I didn't find the answer I was looking for.
So I have a robot with 4 US sensors front, back, left and right. And right know I want the robot to drive until the obstacle is 10 cm away and then the robot should stop, just for testing. So my problem right know is that the robot only stops if the obstacle is in the way of the right US sensor but I want the robot to check the front first then back then left and last right, because I think it would be hard otherwise.
But I don't find my mistake in the programming there must be a logic error I made.
Would be glad if you guys could help me out.
The code is in the attachments.
#include <ArloRobot.h>
#include <SoftwareSerial.h>
ArloRobot Arlo; // Arlo object
SoftwareSerial ArloSerial(12, 13); // Serial in I/O 12, out I/O 13
String directions[4] = {"Front: ", // Directions for display
"Back: ",
"Left: ",
"Right: "
};
int pingPin[4] = {11, 10, 9, 8}; // Ping pins
int cmDist[4]; // Cm distances
int i = 0; // Index, stat at 0
int countsLeft, countsRight;
void setup() {
Serial.begin(9600);
ArloSerial.begin(19200); // Start DHB-10 serial com
Arlo.begin(ArloSerial); // Pass to Arlo object
Arlo.clearCounts(); // Clear encoder counts
}
void loop() {
cmDist[i] = pingCm(pingPin[i]); //Get distance
Serial.print(directions[i]); // Display direction
Serial.print("cmDist["); // Display variable name
Serial.print(i); // Display variable index
Serial.print("] = "); // Display ] & =
Serial.println(cmDist[i]); // Display value
if (cmDist[i] >= 30)
{
Arlo.writeSpeeds(72, 72); // Go forward 72 counts/sec
}
else
{
Arlo.writeSpeeds(0, 0);
delay(1000);
}
delay(50);
i++;
if (i == 4) // If index is 4
{
i = 0; // Reset index to 0
delay(50); // Wait
Serial.println(); // Print a blank line
}
}
int pingCm(int pin) // Ping measurement function
{
digitalWrite(pin, LOW); // Pin to output-low
pinMode(pin, OUTPUT);
delayMicroseconds(200); // Required between successive
digitalWrite(pin, HIGH); // Send high pulse
delayMicroseconds(5); // Must be at least 2 us
digitalWrite(pin, LOW); // End pulse to start ping
pinMode(pin, INPUT); // Change to input
long microseconds = pulseIn(pin, HIGH); // Wait for echo to reflect
return microseconds / 29 / 2; // Convert us echo to cm
}
Ultrasonic_Sensor.ino (2.56 KB)