Basic servo and rangefinder sketch

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
}

I've incorporated this code into my own robot and added a faster plastic gear servo and it works great. I changed a few values to make it work a little better with this chassis but I am quite happy with the results. Here's my full sketch currently running on my Nano-based robot. I've also continued to add notes for ease of use and understanding by beginners.

/* Turtle 2.0 by Steve Robey

   You have probably seen some of this code before. This is my
   tested, modified, improved version. Turtle 2.0 is a prototype
   of a final project and uses an HC-SR04 rangefinder mounted on an
   FMA S300 run of the mill plastic gear servo. Mounted upside down
   to keep it low to the ground.It uses an L293DNE dual H-bridge to
   drive two 6v "Smartcar" motors. These are noisy little motors so
   solder on some capacitors. Feel free to use any of this sketch
   that suits your needs.
*/

#include <Servo.h>
#define Mot1A 2
#define Mot1B 4
#define Mot2A 7
#define Mot2B 8
#define Trig 13
#define Echo 12
Servo myservo;
void setup()
{
  pinMode(Mot1A, OUTPUT);
  pinMode(Mot1B, OUTPUT);
  pinMode(Mot2A, OUTPUT);
  pinMode(Mot2B, OUTPUT);
  pinMode(Trig, OUTPUT);
  pinMode(Echo, INPUT);
  myservo.attach(9);
}

int PING()
{
  long duration, distance;
  digitalWrite(Trig, LOW); //Make sure ultrasound is off
  delayMicroseconds(2);
  digitalWrite(Trig, HIGH); //Send ping
  delayMicroseconds(10);
  digitalWrite(Trig, LOW);  //stop ping
  duration = pulseIn(Echo, HIGH); //Listen for the echo
  distance = (duration/2) / 29.1;
 
  return distance;  //Self titled album
}

void FW()
{
  digitalWrite(Mot1A, HIGH); //turn motor 1 forward
  digitalWrite(Mot1B, LOW);
  digitalWrite(Mot2A, HIGH); //turn motor 2 forward
  digitalWrite(Mot2B, LOW);
  delay(500);
}

void BW()
{
  digitalWrite(Mot1A, LOW); //turn motor 1 reverse
  digitalWrite(Mot1B, HIGH);
  digitalWrite(Mot2A, LOW); //turn motor 2 reverse
  digitalWrite(Mot2B, HIGH);
  delay(200);
}

void LF()
{
  digitalWrite(Mot1A, HIGH); //turn motor 1 forward
  digitalWrite(Mot1B, LOW);
  digitalWrite(Mot2A, LOW); //turn motor 2 reverse
  digitalWrite(Mot2B, HIGH);
  delay(200);
}

void RT()
{
  digitalWrite(Mot1A, LOW); //turn motor 1 reverse
  digitalWrite(Mot1B, HIGH);
  digitalWrite(Mot2A, HIGH); //turn motor 2 forward
  digitalWrite(Mot2B, LOW);
  delay(300);
}

void STOP()
{
  digitalWrite(Mot1A, LOW);  //Shut both motors down
  digitalWrite(Mot1B, LOW);
  digitalWrite(Mot2A, LOW);
  digitalWrite(Mot2B, LOW);
  delay(200);
}  
  
void loop() {
  myservo.write(90); //Make sure Turtle is facing forward
  int DST = PING();     //Send a ping and get forward distance
  if (DST > 50)         //If rangefinder spazzes out again
  {
    DST == 45;
  }
  if (DST <= 0)  //If rangefinder spazzes out in the other direction
  {
    DST == 45;
  }
  if (DST <= 30 ) //If an obstacle is near (Changed this a little)
  {
    STOP();              //Stop moving
    myservo.write(135);  //Turning 45 degrees left
    delay(300);        //Wait for the servo to move
    int lfRange = PING();//Copy another distance value for the left side of the robot
    myservo.write(45);  //Turn right 45 degrees off of center
    delay(300);          //Waiting for the faster servo
    int rtRange = PING(); //Grab distance to the right of the robot
    delay(300);
    myservo.write(90);             //Move back to center
    if (lfRange > rtRange) //If theres more room to the left
    {
      BW(); //Back up a little
      LF(); //Turn to the left
    }
    if (rtRange > lfRange) //If theres more room to the right
    {
      BW(); //Back up a little
      RT(); //Turn to the right
    }
    
  }
  else //If nothing is in front of Turtle
  {
    FW(); //Or go forward
  }
}

This can easily be modified to use a motor shield if you want. I have one but I prefer to work directly with my motor driver so there's slightly more code here because of that. Once again I would like tho thank those that came before me and created the code examples that I used to make this.

Thank you BARRAGAN and jvesyer.

Why are duration and distance longs, particularly as the ranges are ints ?

I thought they needed to be longs because they don't return a whole number.. Are you saying I can change them to integers and still get the same result?

Both long (32 bit) and int (16 bit) are integer types, so neither really support non whole numbers without the use of fixed point arithmetic.

long != float

Alright, I understand what you guys are saying. Thanks for reminding me of this, I haven't really used C or C subsets since college. I'm a Python programmer by nature but I'm finding the Arduino is every bit as capable as the Raspberry Pi when It comes to the applications I have for them.

I'll modify my code further this weekend so that it makes more sense to the seasoned Arduino wrangler.

Thanks guys.

As for why duration and distance are type "Long" I will direct your attention to another thread. It seems to be explained there which is also helpful to be because I honestly didn't understand why, as evidenced by my previous comments. Arduino forum thread

Particularly, the post by Jurs about 2/3 of the way down the page seems to explain why long works better. The person who started this other thread was also working with an HC-SR04 but they were getting negative numbers returned by the sensor.

The pulseIn function has a timeout of 1 second and returns the result in microseconds, so the possible result range is 0...1000000, but you have defined "duration" as 'int', so the range for duration is −32768...32767 only.

So if you should try to assign (example only) a value from pulseIn like 32770 to 'duration', then duration would contain a negative value of -3 after doing so.

So if you want to avoid negative values for duration, you would either have:

  • make the data type of 'duration' big enought to take a value of 1000000, i.e. define it "long" instead "int"
  • make pulseIn a shorter timeout