Hi everybody.
I'm working on a small robot that will have one rangefinder mounted on a servo so I can use the single sensor to look around when there is an obstacle and decide which way to go. As I'm working out how to do this with the hardware I have on hand, I'm developing some of my code based on what others have done and I have already found to work. I thought this bit of code might be useful to somebody else so I decided to share it here. Feel free to make any suggestions you can think of to improve on this code. It's only enough to operate a servo and a rangefinder but will be a good stepping-stone to developing robotic navigation.
I'm using an HC-SR04 ultrasonic rangefinder which is a two-pin sensor. My main reason for using this one over a one-wire package is cost. I've mounted that on top of an FMA S355M servo. This is a slow servo with metal gears. It has lots of torque and I would not recommend this servo for the application simply because it is slow and you really don't need a whole lot of torque to move a rangefinder, speed would be better. It just happened to be the one I grabbed off of the pile.
There are a lot of notes in the code, but I would first like to explain what the overall code is doing. After setting up pins for everything which is fairly straightforward, the servo is moved so that the rangefinder is facing forward. If there are no near obstructions directly in front of the robot, it stays that way. Depending on the environment your robot is in, you may want to change that. I am assuming a fairly open area with minimal obstructions IE a living room or large kitchen etc. If the rangefinder detects an obstruction directly in front of the robot (20CM or less) then we want to look to the left and right to see which direction is more clear and go that way. My code simply looks in both directions and returns the distance, it's up to you to decide what you want to do with the numbers. The code is set to turn 45 degrees left and right of center, but you can modify it to rotate however far you want and can easily add to the code to collect several datapoints if you wish. If no obstruction is detected, the code relays the message "No forward obstructions" and the loop continues.
I hope somebody finds this code useful.
//Servo_Lookaround -Scrapped together by Steve Robey
//
/*This is a basic sketch I put together to be used in robotics projects and is only
preliminary code. the two main portions of the code are modified versions of two
other bits of code I found on the internet and modified. If you would like to see
those other two sketches, the first is the servo control which is based on the Arduino
tutorial "Sweep" by BARRAGAN located at http://www.arduino.cc/en/Tutorial/Sweep the
second portion collecting distance is from an instructable "Simple Arduino and HC-SR04
Example" by jvester and is located at..
http://www.instructables.com/id/Simple-Arduino-and-HC-SR04-Example/
I've merely put these two things together to be a starting point for my own robotics
projects and thought it might be helpful to others. Feel free to use this code in its
entirety or in part, just do the right thing and give credit where credit is due.
Thanks.
*/
#include <Servo.h>
#define trigPin 13 //Trigger pin on the rangefinder (Pin13 on the Arduino)
#define echoPin 12 //Echo pin on the rangefinder (Pin12 on the Arduino)
Servo myservo; //Creating a single servo object
void setup() {
Serial.begin (9600); //Enabling serial for debugging purposes
pinMode(trigPin, OUTPUT); //Setup Trigger pin as an output
pinMode(echoPin, INPUT); //Setup Echo pin as an input
myservo.attach(9); //Attaches the servo on pin 9 to the servo object
}
long PING() //This function will serve to collect all distance measurements
{
long duration, distance;
digitalWrite(trigPin, LOW); //Ensure we are not sending a ping
delayMicroseconds(2); //Short delay to settle the sensor
digitalWrite(trigPin, HIGH); //Sending the ping. The HC-SR04 must remain high for
delayMicroseconds(10); //at least 10 micro-seconds or you can get weird numbers
digitalWrite(trigPin, LOW); //Stop pinging once again
duration = pulseIn(echoPin, HIGH); //Listening for the echo and timing it
distance = (duration/2) / 29.1; //Calculating distance in CM based on the duration
//If you want to return distance in inches: (duration/2) / 74.1;
return distance; //Return the distance value from the PING function
}
void loop()
{
myservo.write(90); //Ensure the sensor is facing forward
int fwRange = PING(); //Get the first distance value in front of the robot
if (fwRange <= 20) //If there is an obstruction 20CM or closer directly in front
{
Serial.print("Forward value: "); //This line for debugging
Serial.println(fwRange); //Outputting distance forward
myservo.write(45); //Turning 45 degrees left (I may have these backward)
delay(500); //Wait for the servo to move (this can be shorter for fast servos)
int lfRange = PING(); //Copy another distance value for the left side of the robot
Serial.print("Left value: "); //Debugging again
Serial.println(lfRange); //Outputting that number for debugging
myservo.write(135); //Turn right 45 degrees off of center
delay(500); //Waiting for my slow metal geared servo
int rtRange = PING(); //Grab distance to the right of the robot
Serial.print("Right value: "); //Yep, more debugging
Serial.println(rtRange); //And the distance in CM
myservo.write(90); //Move back to center
delay(500); //Waiting for the servo
}
else //If there are no forward obstructions, I elected to not look around and would
{ //Just go on. There's no need to look around if we can go forward.
Serial.println("No forward obstructions");
}
delay(2000); //Just a 2 second delay before we do it all over again. Not necessary
}