Create an Object Tracking System: Integrating Servo Control with Object Detect

Integrating Servo Control with Object Detection

This project will cover integrating servo control while using two ultrasonic sensors to track a moving object. This is a continuation of a previous project, published here and here.

It was discovered during the development of this phase of the project that the Arduino IDE has some weaknesses when it comes to custom libraries. Specifically, when writing a custom library that references other custom libraries, the compiler will fail. If you happen to create a reference loop, like I did when trying to troubleshoot this issue, the compile process can actually get stuck in an infinite loop and will "hang up" until a watchdog timer elapses and the compiler sort of gives up.

Reference Loop: Each file is pointing at each other file
...from file A
#include "File B.c"
...from file B
#include "File A.c"

If you write your libraries in C++, some clever people have found some workarounds for this weakness, but I was unsuccessful in transferring their learnings to this project where my libraries were written in C. This ultimately just means that instead of having several source files of functions, we will just store all the functions in a single source file. To reiterate, it's a better practice to have functions grouped in some meaningful manner, spread across multiple source files. That being said, let's move on to the fun part...

In the previous project, the last step had two ultrasonic sensors connected to the Uno with a pair of LEDs. Building off of this foundation, we will add the servo from our previous work to complete the system thus far. The servo only needs one PWM signal, power, and ground.
In order to minimize work, I moved the location of the servo PWM to pin 6 since this is significantly easier than reworking the ultrasonic sensors and LEDs. Refer to the previous article for a wiring diagram of the servo.
*DISCLAIMER* If you have somehow managed to make it to this point without using a breadboard, I STRONGLY encourage you to use one from this point onward. I also recommend making a 5V and GND rail to connect the Ultrasonic sensors and servo to.

Highlight Reel:

• Use a breadboard if you are not already.
• Start implementing voltage and ground rails.
• Move the servo control pin to pin 6 on the Uno.

Three-File Structure

At this point, we can break the sketch into three files: the main, the header, and the source file. I named the main sketch "ObjectTracker", the header file "ObjectTrackerHDR.h", and the source file "ObjectTrackerSRC.c".
The following segments of code will test the interdependencies of the three-file structure. If copied correctly, the program will take a distance measurement from sensor 1 and output it to the serial monitor. The value that is output to the serial monitor is not converted to a unit of measure, so it will not have a meaning that is useful yet.

                    /****************ObjectTracker***********************/

#include "ObjectTrackerSRC.c"    // Include functions from source file
#include "Servo.h"               // Include functions from Servo.h

Servo ServoOne;                  // Generates an instance of a servo object
       
void setup()                     // Initialize
{
  pinMode(Echo_1, INPUT);        // Set Pin for Echo_1 as INPUT
  pinMode(Trigger_1, OUTPUT);    // Set Pin for Trigger_1 as OUTPUT
  Serial.begin(9600);            // Establish baud rate for Serial monitoring 
  ServoOne.attach(6);            // Assigns pin 6 as a servo
}

void loop()                      // Main Loop
{
  PulseInTimeout=6000;           // Define PulseTimeout value
  interrupts();                  // Enable interrupts
  DistanceMeasurementSensor1();  // Function Call
  Serial.println(EchoTime_1);    // Print value of EchoTime_1 to serial monitor followed by newline
} 
                  

                    /*************************ObjectTrackerHDR.h*******************/

/*************Pin Definitions*****************************/
#define LED_2     8    
#define Echo_1    9      
#define Trigger_1 10     
#define LED_1     11     
#define Trigger_2 12     
#define Echo_2    13     

/*************Variable Declarations**************************/
long EchoTime_1;
long EchoTime_2;
int  Distance_1;
int  Distance_2;
int  PulseInTimeout;
int  ServoPosition;
                  

                    /***************************ObjectTrackerSRC.c****************************/

#include "Arduino.h"
#include "ObjectTrackerHDR.h"

void DistanceMeasurementSensor1()
{ 
  digitalWrite(Trigger_1, LOW);                       // Hold Trigger Low
  delayMicroseconds(2);                               // Settle Time
  digitalWrite(Trigger_1, HIGH);                      // Enable Trigger 
  delayMicroseconds(10);                              // Hold High for 10uS
  digitalWrite(Trigger_1, LOW);                       // Hold Trigger Low to start range detect
  EchoTime_1 = pulseIn(Echo_1, HIGH, PulseInTimeout); // Timer sequence for pulse train capture
}
                  

The function pulseIn now has a third parameter: PulseInTimeout. This parameter is allegedly optional, but when being referenced in a custom library, the "optional" parameter becomes required. Yet another undocumented weakness of the Arduino IDE.
This new parameter is the length of time the pulseIn function will wait for the pulse to end. If the pulse does not end by the time indicated by PulseInTimeout, it will return a value of 0. The next section will add the function that converts the output of the distance measurement to a meaningful value: centimeters.

Converting to Centimeters

Now we will add a new function to the source file, a new function call inside the main loop, and change the variable output to the calculated distance in centimeters.
Add the code changes below to the code above. Note that there are no changes to the header file. If successful, you will see a scrolling distance measurement in centimeters.

                    /********* ADD TO SOURCE FILE*************/

void CalculateDistanceSensor1()
{
  Distance_1 = (EchoTime_1/58);   // Calculate Distance from Echo
}
                  

                    /************ ADD TO SETUP LOOP ***********/

PulseInTimeout=6000;                 // Define PulseTimeout value

/************ NEW MAIN LOOP ***************/

void loop()                          // main loop
{
  interrupts();                      // Enable interrupts
  DistanceMeasurementSensor1();      // Function Call
  CalculateDistanceSensor1();        // Function Call   
  Serial.println(Distance_1);        // Print value of Distance_1_buffer to serial monitor followed by newline
} 
                  

For the next part, we are going to add the second distance measurement and its complementary conversion to centimeters in a similar method as the previous step. Alter your code to include the following additions:

                    /************************** ADD TO SOURCE FILE *****************/

void DistanceMeasurementSensor2()
{ 
  digitalWrite(Trigger_2, LOW);                         // Hold Trigger Low
  delayMicroseconds(2);                                 // Settle Time
  digitalWrite(Trigger_2, HIGH);                        // Enable Trigger 
  delayMicroseconds(10);                                // Hold High for 10uS
  digitalWrite(Trigger_2, LOW);                         // Hold Trigger Low to start range detect
  EchoTime_2 = pulseIn(Echo_2, HIGH, PulseInTimeout);   // Timer sequence for pulse train capture
}

void CalculateDistanceSensor2()
{
  Distance_2 = (EchoTime_2/58);   // Calculate Distance from Echo
}

                  

                    /************ NEW MAIN LOOP ***************/

void loop()                      // main loop
{
  interrupts();                      // Enable interrupts
  DistanceMeasurementSensor1();      // Function Call
  CalculateDistanceSensor1();        // Function Call
  DistanceMeasurementSensor2();      // Function Call
  CalculateDistanceSensor2();        // Function Call
  Serial.println(Distance_2);        // Print value of Distance_2
} 

                  

If done correctly, you will see an output on the serial monitor: the distance measured from the second sensor. The next step is to introduce some logic from the outputs of the distance measurements. We do this in a new function called ObjectPresent().

The ObjectPresent() Function

By comparing two measurements from each sensor, we can determine if an object is in front of the left, right, or both sensors. When the sensor output changes, we tell the servo to rotate in the direction of the object, thus tracking its motion.
The next chunk of code is a big one. There are a lot of new variables and parameters that can be adjusted. Instead of continuing to display the code piece-meal, the following code is complete and is not meant as an amendment. It may prove beneficial to review the code below and integrate the algorithm by hand in lieu of just copying and pasting. 

                    /*************** ObjectTracker ***************/

#include "ObjectTrackerSRC.c"
#include "Servo.h"

Servo ServoOne;                    // generates an instance of a servo object
       
void setup()                       // initialize
{
  pinMode(Echo_1, INPUT);
  pinMode(Trigger_1, OUTPUT);
  pinMode(Echo_2, INPUT);
  pinMode(Trigger_2, OUTPUT);
  ServoOne.attach(6);              // Assigns pin 6 as a servo
  ServoPosition=90;                // Default Servo position
  PulseInTimeout=8000;             // Define PulseTimeout value in microsec
  MaxDistance=40;                  // MaxDistance in cm
  ServoPositionMin=30;             // Min Servo Position
  ServoPositionMax=150;            // Max Servo Position
  ServoPivotSpeed=2;               // The Servo angle step size
}

void loop()                        // main loop
{
  interrupts();                    // Enable interrupts
  DistanceMeasurementSensor1();    // Function Call
  CalculateDistanceSensor1();      // Function Call
  DistanceMeasurementSensor2();    // Function Call
  CalculateDistanceSensor2();      // Function Call
  ObjectPresent();                 // Function Call
  ServoOne.write(ServoPosition);   // Rotates Servo to position stored in ServoPosition
  LEDsOff();                       // Turns LEDs off
} 

                  

                    /*************** ObjectTrackerHDR.h ***************/

#define LED_2     8
#define Echo_1    9
#define Trigger_1 10
#define LED_1     11
#define Trigger_2 12
#define Echo_2    13

unsigned long EchoTime_1;
unsigned long EchoTime_2;
int  Distance_1;
int  Distance_2;
int  PulseInTimeout;
int  ServoPosition;
int  MaxDistance;
int  ServoPositionMin;
int  ServoPositionMax;
int  ServoPivotSpeed;
                  

                    /*************** ObjectTrackerSRC.c ***************/

#include "Arduino.h"
#include "ObjectTrackerHDR.h"

void DistanceMeasurementSensor1()      // Measures Distance with Sensor 1
{ 
    digitalWrite(Trigger_1, LOW);                       // Hold Trigger Low
    delayMicroseconds(10);                              // Settle Time
    digitalWrite(Trigger_1, HIGH);                      // Enable Trigger 
    delayMicroseconds(5);                               // Hold High for 10uS
    digitalWrite(Trigger_1, LOW);                       // Hold Trigger Low to start range detect
    EchoTime_1 = pulseIn(Echo_1, HIGH, PulseInTimeout); // Timer sequence for pulse train capture
}

void CalculateDistanceSensor1()    // Converts Distance from EchoTime_1 to cm
{
  Distance_1 = (EchoTime_1/58);
    if (Distance_1 > MaxDistance)  // Constrains Distance_1 to MaxDistance
  {
    Distance_1 = MaxDistance;
  }
}

void DistanceMeasurementSensor2()       // Measures Distance with Sensor 2
{ 
  digitalWrite(Trigger_2, LOW);                         // Hold Trigger Low
  delayMicroseconds(10);                                // Settle Time
  digitalWrite(Trigger_2, HIGH);                        // Enable Trigger 
  delayMicroseconds(5);                                 // Hold High for 10uS
  digitalWrite(Trigger_2, LOW);                         // Hold Trigger Low to start range detect
  EchoTime_2 = pulseIn(Echo_2, HIGH, PulseInTimeout);   // Timer sequence for pulse train capture
}

void CalculateDistanceSensor2()    // Converts Distance from EchoTime_1 to cm
{
  Distance_2 = (EchoTime_2/58);     
  if (Distance_2 > MaxDistance)    // Constrains Distance_2 to MaxDistance
  {
    Distance_2 = MaxDistance;
  }
}

void ObjectPresent()                                  // Object Detection Algorithm
{
  if(ServoPosition<90)                                // Detection if less than 90 degrees
  {
      if((Distance_2-Distance_1)>0)                   // Direction comparison 
      {
       ServoPosition=ServoPosition+ServoPivotSpeed;   // Increment Servo position by ServoPivotSpeed
       digitalWrite(LED_1, HIGH);                     // Turn LED ON
      }
  
      if((Distance_1-Distance_2)>0)                   // Direction comparison
      {
        ServoPosition=ServoPosition-ServoPivotSpeed;  // Increment Servo position by ServoPivotSpeed
        digitalWrite(LED_2, HIGH);                    // Turn LED ON
      }
  }
   
if(ServoPosition>89)                                  // Detection if more than 89 degrees
  {
      if((Distance_2-Distance_1)>0)                   // Direction comparison
      {
       ServoPosition=ServoPosition+ServoPivotSpeed;   // Increment Servo position by ServoPivotSpeed
       digitalWrite(LED_1, HIGH);                     // Turn LED ON
      }
  
      if((Distance_1-Distance_2)>0)                   // Direction comparison
      {
        ServoPosition=ServoPosition-ServoPivotSpeed;   // Increment Servo position by ServoPivotSpeed
        digitalWrite(LED_2, HIGH);                     // Turn LED ON
      }
  }   
      
      if(ServoPositionServoPositionMax)          // Restricts Servo position to Max
      {
        ServoPosition=ServoPositionMax;
      }
}

void LEDsOff()                                    // Turns LEDs Off
{
  digitalWrite(LED_1, LOW);
  digitalWrite(LED_2, LOW);
}
                  

The ObjectPresent() function performs the following steps:

1. Determine if the servo is currently facing left or right.
2. Determine which sensor is detecting an object.
3. Increment or decrement the servo position by an amount equal to the ServoPivotSpeed.
4. Turn on the LED that represents the direction of interest.
5. Check if servo position is less than the minimum and change to minimum if it is less.
6. Check if servo position is greater than the maximum and change to maximum if it is greater.

The variables that define how this system operates can be adjusted— and I recommend that you do so. Specifically, adjust the values for ServoPositionMin, ServoPositionMax, ServoPivotSpeed, and MaxDistance.

• ServoPositionMin: The minimum allowable angle for the servo to be moved to. If too low, the servo may malfunction.
• ServoPositionMax: The maximum allowable angle for the servo to be moved to. If too high, the servo may malfunction.
• ServoPivotSpeed: An integer value that is added or subtracted from the current servo angle to track an object.
• MaxDistance: The maximum distance, in centimeters, that the sensors will read. Any value greater than this is changed to this value. This prevents the sensors from detecting walls in small spaces and "tracking" stationary objects.

Building Your System

To build your own system, you need to mount the sensors facing the field in which the object will pass. I used heavy duty double sided tape to hold all the pieces together. Make sure before you start testing that you have enough clearance in your cabling.



In the image above, you see my two sensors facing me. The tiny breadboard is resting on top of a servo, which is resting on a piece of plastic I had laying around.


I would like to point out that this system is based on two range detectors and is not a complex radar system. I encourage you to play with the detection algorithm and try to improve its performance. It is far from perfect.