1D Motion tracking w/ HC-SR04 code issue

So I am working on a simple program where I am trying to execute the following logic work flow in order to have the servo position correlate to the measurement of a person walking in a straight line tangent to the servo towards the ultrasonic sensor such that the servo’s arm points and follows the person as they walk through the sensing range. The hardware is an arduino pro micro and a ultrasonic HC-SR04 sensor module with a standard hobby servo.

I know I am missing something, but after lots of googling and hours of rehashing I thought I ought to post here where the more programming experienced can provide some input. Programming is a beast for me, and a beast I want to slay. I just can’t seem to wrap my brain around what’s missing from my logic flow/code.

When the code executes and an object approaches the sensor it follows the object, but than I get my custom error message, and the servo is locked in position. Although if queried the ping is still reactive and returning correct distance measurements.

Thanks for your help.

Logic flow:

  • If the current distance measurement is within the no change tolerance when compared to previous distance measurement=> maintain servo position, and update previous . If not.

  • Else, test to see if the current distance measurement is less than tolerated no change range of the previous measurement if so => Move servo 1 degree closer to come closer to previous measurement. If not.

  • Else, test to see if the current distance measurement is more than tolerated no change range of the previous measurement, if so => Move servo 1 degree further to come closer to previous measurement. if not.

  • Send Error with debug info.

Code I have:

// ---------------------------------------------------------------------------
// Past Primitive one dimensional motion tracker.
// ---------------------------------------------------------------------------
#include <Servo.h>
#include <NewPing.h>

#define TRIGGER_PIN  16  // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN     10  // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 400 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.

Servo myservo;  // create servo object to control a servo
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and maximum distance.

unsigned int currentPing;
unsigned int previousPing;
unsigned int servoPos = 90;             //Current servo position.  Used to update the servo's position in real time.
unsigned int targetServoPos = 90;
unsigned int servoMinPos = 1;           //Minimum servo position, wont allow movement past this point.  I like to keep it a degree short to stay away from the mechanical stops to prevent damage.
unsigned int servoMaxPos = 179;         //Maximum servo position, I like to keep it a degree short to stay away from the mechanical stops to prevent damage.
unsigned int sensorMinDis = 0;          //Minimum sensor distance to pay attention to.
unsigned int sensorMaxDis = 400;        //Needs to be the same as #define MAX_DISTANCE.
unsigned int stabalizationRange = 3;    //+/- range where deviating sensor values will be ignored.

void setup() {
    Serial.begin(115200); // Open serial monitor at 115200 baud to see ping results.
    myservo.attach(8);  // Attaches the servo on pin 8 to the servo object.
}

void loop() {
    if (servoPos >= targetServoPos - stabalizationRange && servoPos <= targetServoPos + stabalizationRange ) {
        ping ();
        Serial.println ("No change.");
    }
    else if (targetServoPos < servoPos - stabalizationRange && servoPos > 0 && targetServoPos > 0){
        servoPos = servoPos - 1;
        myservo.write(servoPos);
        delay (5);
        Serial.print ("Closer Servo Pos: ");
        Serial.println (servoPos);
    }
    else if (targetServoPos > servoPos + stabalizationRange && servoPos < 180 && targetServoPos < 180){
        servoPos = servoPos + 1;
        myservo.write(servoPos);
          delay (5);
        Serial.print ("further Servo Pos: ");
        Serial.println (servoPos);
    }
    else {Serial.println ("ERROR"); delay (250);Serial.println (servoPos);}
}


// Ping function returns relative servo position in degrees.
void ping() {
    delay(50);                      // Wait 50ms between pings (about 20 pings/sec). 29ms should be the shortest delay between pings.
    unsigned int uS = sonar.ping(); // Send ping, get ping time in microseconds (uS).
    Serial.print("Ping: ");
    Serial.print(uS / US_ROUNDTRIP_CM); // Convert ping time to distance in cm and print result (0 = outside set distance range)
    Serial.println("cm");
    unsigned int distanceInCM = uS / US_ROUNDTRIP_CM;
    targetServoPos = map(distanceInCM ,sensorMinDis ,sensorMaxDis, servoMinPos, servoMaxPos);
    Serial.print("deg: ");
    Serial.println(targetServoPos);
}

New_Attempt_at_Ultrasonic.ino (3.13 KB)

What do your debug outputs tell you?

Here’s the most recent.

serial debug readout.txt (2.79 KB)

Wouldn't you want to ping() on every pass through loop()?

That seems to have fixed it.