Uncategorized

Build an object tracking system: integrate servo control and object detection


Integrate servo control with object detection

The project will include integrating servo control and simultaneously using two ultrasonic sensors to track moving objects. This is a continuation of previous projects, published here and here.

During the development of this phase of the project, it was discovered that the Arduino IDE had some weaknesses in its custom libraries. Specifically, the compiler will fail when writing custom libraries that reference other custom libraries. If you happen to create a reference cycle, like I did while trying to solve this problem, the compilation process can actually get stuck in an infinite loop and “hang” until the watchdog timer expires and The compiler kind of gives up.

Reference cycle: each file points to another file

….from file A

#include “File Bc”

…from file B

#include “File Ac”

If you write your library in C++, some smart people have found some way around this weakness, but I have not been successful in transferring their knowledge to my project where the library is written in C. Instead of having several source file functions, we store all functions in one source file. Again, a better approach is to group functions in some meaningful way, spread across multiple source archives. That being said, let’s get on to the fun part…

In the previous project, the last step was to connect the two ultrasonic sensors to the Uno via a pair of LEDs. From this we will add servos from previous work to complete the system so far. The servo only needs a PWM signal, power supply and ground.

To minimize the effort, I moved the location of the servo PWM to pin 6 as this is much easier than reworking the ultrasonic sensor and LED. Please refer to the previous article for the servo wiring diagram.

*Disclaimer* If you managed to do this without using a breadboard, I highly encourage you to use one from now on. I also recommend making a 5V and GND rail to connect the ultrasonic sensor to the servo.

Highlight Reels:

  • Use a breadboard if you don’t have one 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 split the sketch into three archives: the main archive, the header archive, and the original archive. I named the main sketch “ObjectTracker”, the header file “ObjectTrackerHDR.h” and the original file “ObjectTrackerSRC.c”.

The following code snippet will test the interdependence of the three file structures. If copied correctly, the program will take a distance measurement from sensor 1 and output it to the serial monitor. The value output to the serial monitor is not converted to a unit of measurement, so it does not yet have a useful meaning.


/****************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
}

Function Pulse input Now there is a third parameter: PulseInTimeout. This parameter is said to be optional, but when referenced in a custom library, the “optional” parameter becomes required. Another undocumented weakness of the Arduino IDE.

This new parameter is the length of time Pulse input The function will wait for the pulse to end. If the pulse does not end within the time indicated by PulseInTimeout, it returns a value of 0.

Convert to centimeters

Now we will add a new function to the source file, call a new function 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 the first gear has not changed. If successful, you will see the scroll 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’ll add a second distance measurement and its complementary conversion to centimeters using a similar approach to the previous step. Change your code to add the following:


/************************** 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 the output on the serial monitor: the distance measured from the second sensor. The next step is to introduce some logic from the output of the distance measurement. We do this in a new function called ObjectPresent().

ObjectPresent() function

By comparing the two measurements from each sensor, we can determine whether the object is to the left, right, or in front of both sensors. When the sensor output changes, we tell the servo to rotate in the direction of the object, thus tracking its movement.

The next piece of code is a very large piece of code. There are many new variables and parameters to adjust. The code below is complete and not meant to be modified, instead the code snippet continues to be shown. It may prove beneficial to examine the code below and integrate the algorithm manually instead 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 whether the servo is currently facing left or right.
  2. Determine which sensor is detecting the object.
  3. Increase or decrease the servo position by an amount equal to ServoPivotSpeed.
  4. Turn on the LED representing the direction of interest.
  5. Check whether the servo position is less than the minimum value, if it is less, change it to the minimum value.
  6. Check whether the servo position is greater than the maximum value, if it is greater, change it to the maximum value.

The variables that define how this system operates can be adjusted – and I recommend you do so.Specifically, the adjustment value Minimum servo position, maximum servo position, Servo pivot speedand maximum distance.

  • ServoPositionMin: The minimum allowed angle to which the servo moves. If it’s too low, the servo may malfunction.
  • ServoPositionMax: The maximum allowed angle to which the servo moves. If it’s too high, the servo may malfunction.
  • ServoPivotSpeed: An integer value added or subtracted from the current servo angle to track the object.
  • Maximum distance: The maximum distance read by the sensor (in centimeters). Any value greater than this value will be changed to this value. This prevents the sensor from detecting walls in small spaces and “tracking” stationary objects.

Build your system

To build your own system, you will need to install sensors over the area that the object will pass through. I used heavy duty double sided tape to hold everything together. Before you begin testing, make sure there is adequate clearance in the wiring.

In the picture above you can see my two sensors facing me. The small breadboard sits on top of the server, which sits on a piece of plastic I placed.

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

object tracker.zip



Source link

LEAVE A RESPONSE

Your email address will not be published. Required fields are marked *