Good evening,
I installed the Ping))) US sensor (Parallax Inc | Equip Your Genius®) on the Arduino Robot (http://arduino.cc/en/Guide/Robot); to be more exact, port TKD1. I have tried to adapt a code I found for a general Arduino platform to go with the robot and got a piece-wise satisfying solution. The idea is to detect an obstacle, turn 90 degrees, check again and decide to go forward or turn again. However, at the moment it goes straight till it encounters an obstacle and then just turns permanently. Why? What do I have to change?
/* Ping))) Sensor
This sketch reads a PING))) ultrasonic rangefinder and returns the
distance to the closest object in range. To do this, it sends a pulse
to the sensor to initiate a reading, then listens for a pulse
to return. The length of the returning pulse is proportional to
the distance of the object from the sensor.
The circuit:
* +V connection of the PING))) attached to +5V
* GND connection of the PING))) attached to ground
* SIG connection of the PING))) attached to digital pin 7
http://www.arduino.cc/en/Tutorial/Ping
created 3 Nov 2008
by David A. Mellis
modified 30 Aug 2011
by Tom Igoe
This example code is in the public domain.
*/
#include <ArduinoRobot.h>
// this constant won't change. It's the pin number
// of the sensor's output:
int pingPin = TKD1;
void setup() {
// initialize the Robot, SD card, and display
Serial.begin(9600);
Robot.begin();
Robot.beginTFT();
Robot.beginSD();
Robot.displayLogos();
// draw a face on the LCD screen
setFace(true);
}
void loop() {
// establish variables for duration of the ping,
// and the distance result in inches and centimeters:
long duration, inches, cm;
// The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
// Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
pinMode(pingPin, OUTPUT);
Robot.digitalWrite(pingPin, LOW);
delayMicroseconds(2);
Robot.digitalWrite(pingPin, HIGH);
delayMicroseconds(5);
Robot.digitalWrite(pingPin, LOW);
// The same pin is used to read the signal from the PING))): a HIGH
// pulse whose duration is the time (in microseconds) from the sending
// of the ping to the reception of its echo off of an object.
pinMode(pingPin, INPUT);
duration = pulseIn(pingPin, HIGH);
// convert the time into a distance
inches = microsecondsToInches(duration);
cm = microsecondsToCentimeters(duration);
// If the robot is blocked, turn until free
while(cm < 40) { // If an obstacle is less than 20cm away
setFace(false); //shows an unhappy face
Robot.motorsStop(); // stop the motors
delay(1000); // wait for a moment
Robot.turn(90); // turn to the right and try again
delay(1000); // wait for a moment
pinMode(pingPin, OUTPUT);
Robot.digitalWrite(pingPin, LOW);
delayMicroseconds(2);
Robot.digitalWrite(pingPin, HIGH);
delayMicroseconds(5);
Robot.digitalWrite(pingPin, LOW);
// The same pin is used to read the signal from the PING))): a HIGH
// pulse whose duration is the time (in microseconds) from the sending
// of the ping to the reception of its echo off of an object.
pinMode(pingPin, INPUT);
duration = pulseIn(pingPin, HIGH);
cm = microsecondsToCentimeters(duration);
delay(1000); // wait for a moment
setFace(true); // happy face
}
// if there are no objects in the way, keep moving
Robot.motorsWrite(255, 255);
delay(100);
}
long microsecondsToInches(long microseconds)
{
// According to Parallax's datasheet for the PING))), there are
// 73.746 microseconds per inch (i.e. sound travels at 1130 feet per
// second). This gives the distance travelled by the ping, outbound
// and return, so we divide by 2 to get the distance of the obstacle.
// See: http://www.parallax.com/dl/docs/prod/acc/28015-PING-v1.3.pdf
return microseconds / 74 / 2;
}
long microsecondsToCentimeters(long microseconds)
{
// The speed of sound is 340 m/s or 29 microseconds per centimeter.
// The ping travels out and back, so to find the distance of the
// object we take half of the distance travelled.
return microseconds / 29 / 2;
}
// make a happy or sad face
void setFace(boolean onOff) {
if(onOff) {
// if true show a happy face
Robot.background(0, 0, 255);
Robot.setCursor(44, 60);
Robot.stroke(0, 255, 0);
Robot.setTextSize(4);
Robot.print(":)");
}else{
// if false show an upset face
Robot.background(255, 0, 0);
Robot.setCursor(44, 60);
Robot.stroke(0, 255, 0);
Robot.setTextSize(4);
Robot.print("X(");
}
}