Follow Me 2d

This was a very simple experiment in tracking an object in 2D space using sonar sensors. I had watched others do it with different sensors or IR sensors and thought this was worth a try. Two SR04 units were mounted to a piece of aluminum mounted angled slightly out on a single servo. Goal was to simply track left/right once an item had been "acquired" by measuring distances and moving as needed. It worked rather well actually. You do need to have some limits on when it will move to avoid noise. You also need to determine what distance is the acquired sweet spot to lock onto.

Possible applications would be a "follow me" cart outside to haul stuff with. Have a set distance you need to be in front of the cart for it to lock onto and then walk. As long as you do not outpace it or make too abrupt of turns, it should be able to track you.

I never took this any further however but it could be used for anything wanting to roughly track and object simply.

Example Code

// FollowMe2-2D - testing Follow options with Ultra Sonic
// v1.00 - 10/20/2013 - SWN

#include 
#include 

#define TRIGGER_PIN1  6  // Arduino pin tied to trigger pin on the ultrasonic sensor.      LEFT SENSOR
#define ECHO_PIN1     5   // Arduino pin tied to echo pin on the ultrasonic sensor.

#define TRIGGER_PIN2  7  // Arduino pin tied to trigger pin on the ultrasonic sensor.      TOP SENSOR
#define ECHO_PIN2     8   // Arduino pin tied to echo pin on the ultrasonic sensor.

#define MAX_DISTANCE 200 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.

NewPing sonar1(TRIGGER_PIN1, ECHO_PIN1, MAX_DISTANCE); // NewPing setup of pins and maximum distance.
NewPing sonar2(TRIGGER_PIN2, ECHO_PIN2, MAX_DISTANCE); // NewPing setup of pins and maximum distance.

//** Setup the actual servo outputs as needed
byte ServoPanPin = 3;        // pin for the servos
Servo ServoPan;              // Servo for panning the sensors
byte ServoPanPosition = 90;     // Servo position tracking, start centered

//** Setup the US variables needed
int uS;
int uSLeft;                 // Sonar 1 distance var
int uSRight;                // Sonar 2 distance var
int usHorz   = 0;           // Difference between the two values
int usBuffer = 2;           // How much to allow for centered / buffer location
int ServoStep= 3;           // How many incs between each step for servo
                            // Lower = slower but more fine tuning, easier to lose target
int Distance;               // Average distance to object between the two sensors
                            // used in PID to keep the distance from the object


byte MyDebug = 1;          // Used for displaying debug data, etc

void setup() {
  Serial.begin(115200); // Open serial monitor at 115200 baud to see ping results.

  ServoPan.attach(ServoPanPin);
  ServoPan.write(ServoPanPosition);

  Serial.println("Starting...");
}

void loop() {

  delay(25);                      // Wait 50ms between pings (about 20 pings/sec). 29ms should be the shortest delay between pings.
  uS = sonar1.ping();           // Send ping, get ping time in microseconds (uS).
  uSLeft = uS / US_ROUNDTRIP_CM;	// convert to CM for using - I *THINK* this is off due to 20mhz clock on Orangutan?

  if (uSLeft == 0)
    {
      uSLeft = 9999;
    }
  
  delay(25);                      // Wait 50ms between pings (about 20 pings/sec). 29ms should be the shortest delay between pings.
  uS = sonar2.ping();           // Send ping, get ping time in microseconds (uS).
  uSRight = uS / US_ROUNDTRIP_CM;	// convert to CM for using - I *THINK* this is off due to 20mhz clock on Orangutan?

  if (uSRight == 0)
    {
    uSRight = 9999;
    }

  if ((uSLeft != 9999) || (uSRight != 9999))
  {

    usHorz = (uSLeft - uSRight);
    //** Only update distance if we have measurements for both
    if ((uSLeft != 9999) && (uSRight != 9999))
    {
      Distance = ((uSLeft + uSRight) / 2);
    }
    
    //** Handle horizontal movement controls
    if (abs(usHorz) > usBuffer)          //** If we are outside our buffer then address it
    {
      if (uSLeft < uSRight)
      {
        ServoPanPosition = (ServoPanPosition - ServoStep);
      }
      else
      {
        ServoPanPosition = (ServoPanPosition + ServoStep);
      }
      ServoPanPosition = constrain(ServoPanPosition, 10, 170);
      ServoPan.write(ServoPanPosition);
    }
    
    //** If we want to see data then spit it out here
    if (MyDebug == 1)
    {
      DisplayData();
    }

  }  // End if anything not 9999
  
}    // End main loop

void DisplayData()
{
        Serial.print("Ping, L, ");
      Serial.print(uSLeft); // Convert ping time to distance in cm and print result (0 = outside set distance range)
      Serial.print(", R, ");  

      Serial.print(uSRight); // Convert ping time to distance in cm and print result (0 = outside set distance range)
      Serial.print(", T, ");  

      Serial.print("HDiff, ");
      Serial.print(usHorz);
      Serial.print(", ");  

      Serial.print("Pan, ");
      Serial.print(ServoPanPosition);
      Serial.print(", ");  

      Serial.print("Dist, ");
      Serial.print(Distance);
      
      Serial.println();
}