Hello all.
I have been working on a robot car that will do the following things:
Psuedo Code:
Sweep one time with Ping sensor attached to servo looking for a target
If no target found, drive forward while sweeping with Ping sensor until a target is detected
When target is detected, move towards target automatically changing direction to keep target centered in driving direction
Stop when target is predetermined distance away
Reverse if target is too close (closer than predetermined distance)
It seems to just keep looking but does not find a target unless I move something close to it.
Here is the loop that should be doing all this:
void loop()
{
// long cm, duration;
// Move servo:
posOldDistance = (pos);
pingOldDistance = (chkDistanceFwd);
panServo.write(pos);
// Define next servo position:
if (dir == 1)
{
if(pos < maxSweep)
{
pos += 10;
}
else
{
dir = -1;
}
}
else if(dir == -1)
{
if(pos > minSweep)
{
pos -= 10;
}
else
{
dir = 1;
}
}
// Notice how there's no delays in this sketch to allow you to do other processing in-line while doing distance pings.
if (millis() >= pingTimer)
{ // pingSpeed milliseconds since last ping, do another ping.
pingTimer += pingSpeed; // Set the next ping time.
sonar.ping_timer(echoCheck); // Send out the ping, calls "echoCheck" function every 24uS where you can check the ping status.
}
// Do other stuff here, really. Think of it as multi-tasking.
// print results to the serial port in the form "# #", or "degrees distance":
// The Processing sketch is designed to read them in this form.
Serial.print(" chkDistanceFwd=");
Serial.print(chkDistanceFwd);
Serial.print(" dangerThresh="); //displays value of dangerThresh to validate operation
Serial.print(dangerThresh);
Serial.print(" rightDistance=");
Serial.print(rightDistance);
Serial.print(" leftDistance=");
Serial.print(leftDistance);
Serial.print(" ");
Serial.print(" posOldDistance=");
Serial.print(posOldDistance);
Serial.print(" ");
Serial.print(" posNewDistance=");
Serial.print(posNewDistance);
Serial.print(" ");
Serial.print(" pos=");
Serial.print(" ");
Serial.print(pos);
Serial.print(" ");
Serial.print(" pingCloseDistance=");
Serial.print(pingCloseDistance);
Serial.print(" ");
// give servo a break:
// delay(15);
}
//--------------------------------------
void echoCheck()
{ // Timer2 interrupt calls this function every 24uS where you can check the ping status.
// Don't do anything here!
if (sonar.check_timer())
{ // This is how you check to see if the ping was received.
// Here's where you can add code.
Serial.print(" Ping: ");
Serial.print(sonar.ping_result / US_ROUNDTRIP_CM); // Ping returned, uS result in ping_result, convert to cm with US_ROUNDTRIP_CM.
Serial.println("cm");
chkDistanceFwd = (sonar.ping_result / US_ROUNDTRIP_CM);
posNewDistance =(pos);
pingNewDistance = (sonar.ping_result / US_ROUNDTRIP_CM);
if (pingNewDistance > pingOldDistance)
{
(pingCloseDistance) = (pingOldDistance);
}
else
{
(pingCloseDistance) = (pingNewDistance);
}
if (posOldDistance <= 89 || posNewDistance <= 89)//0 is to the right
{
(rightDistance) = (pingCloseDistance);
}
else if (posOldDistance >= 91 || posNewDistance >= 91)//180 is to the left
{
(leftDistance) = (pingCloseDistance);
}
else
{
(pingCloseDistance) = (chkDistanceFwd);
}
/* if (chkDistanceFwd <= dangerThresh)//if target is <= minimum programmed distance. STOP
{
bothWheelStop();
} */
if (chkDistanceFwd > dangerThresh && chkDistanceFwd <= 60)//if target distance is greater than programmed default and <= 60 cm go straight medium speed
{
forwardMed();
}
else if (chkDistanceFwd > dangerThresh)
{
if (chkDistanceFwd <= rightDistance || chkDistanceFwd <= leftDistance) //if target distance is greater than programmed default, go straight
{
forward();
}
if (chkDistanceFwd > rightDistance || leftDistance > rightDistance) //if target distance is greater than programmed default AND if target is closer on the RIGHT side...turn RIGHT to face target
{
turnRight();
}
if (chkDistanceFwd > leftDistance || rightDistance > leftDistance) //if target distance is greater than programmed default AND if target is closer on the LEFT side...turn LEFT to face target
{
turnLeft();
}
if (chkDistanceFwd <= dangerThresh)//if target is <= minimum programmed distance. STOP
{
bothWheelStop();
reverse();
}
}
// Don't do anything here!
}
}
The routine is not exactly an ISR, it's a wrapped function inside an ISR. The interrupt is canceled when sonar.check_timer() is true, so you're free to do whatever you want inside the sonar.check_timer() if statement, including serial output. I wrote the library and it's working right here in front of me, serial output and all. Even one of my example sketches for the timer interrupt method works in exactly this way: