After the success with the 2D tracking using the SR04 sonars, I thought I'd try another dimension. Easy enough to do, just added a tilt servo and third SR04 sensor and a little code. It actually works although the CPU did go away in the video.
Possible use would be to make a face appear it is following you if you are within distance of a talking head like BoxHead. The biggest issue is the appear with four eyes and all those SR04 units looking at you. If the EZ series of sensors were not so expensive, one could use one each for eyes and one for the nose to make it work.
I never took this any further as well however but it could be used for anything wanting to roughly track an object simply.
This is the same code as the 2d with the extra axis added in.
// FollowMe2 - testing Follow options with Ultra Sonice
// 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 TRIGGER_PIN3 10 // Arduino pin tied to trigger pin on the ultrasonic sensor. RIGHT SENSOR
#define ECHO_PIN3 11 // 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 sonar3(TRIGGER_PIN2, ECHO_PIN2, MAX_DISTANCE); // NewPing setup of pins and maximum distance.
NewPing sonar2(TRIGGER_PIN3, ECHO_PIN3, 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
byte ServoTiltPin = 4;
Servo ServoTilt;
byte ServoTiltPosition = 90;
//** Setup the US variables needed
int uS;
int uSLeft; // Sonar 1 distance var
int uSRight; // Sonar 2 distance var
int uSTop; // Sonar 3 - top unit
int usHorz = 0; // Difference between the two values
int usVert = 0;
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);
ServoTilt.attach(ServoTiltPin);
ServoTilt.write(ServoTiltPosition);
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;
}
delay(25); // Wait 50ms between pings (about 20 pings/sec). 29ms should be the shortest delay between pings.
uS = sonar3.ping(); // Send ping, get ping time in microseconds (uS).
uSTop = uS / US_ROUNDTRIP_CM; // convert to CM for using - I *THINK* this is off due to 20mhz clock on Orangutan?
if (uSTop == 0)
{
uSTop = 9999;
}
if ((uSLeft != 9999) || (uSRight != 9999) || (uSTop != 9999))
{
usHorz = (uSLeft - uSRight);
//** Only update distance if we have measurements for both
if ((uSLeft != 9999) && (uSRight != 9999))
{
Distance = ((uSLeft + uSRight) / 2);
usVert = (uSTop - Distance);
}
//** 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);
}
//** Handle vertical movement controls
if (abs(usVert) > usBuffer) //** If we are outside our buffer then address it
{
if (uSTop < Distance)
{
ServoTiltPosition = (ServoTiltPosition - ServoStep);
}
else
{
ServoTiltPosition = (ServoTiltPosition + ServoStep);
}
ServoTiltPosition = constrain(ServoTiltPosition, 10, 170);
ServoTilt.write(ServoTiltPosition);
}
//** 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(uSTop); // Convert ping time to distance in cm and print result (0 = outside set distance range)
Serial.print(", ");
Serial.print("HDiff, ");
Serial.print(usHorz);
Serial.print(", ");
Serial.print("VDiff, ");
Serial.print(usVert);
Serial.print(", ");
Serial.print("Pan, ");
Serial.print(ServoPanPosition);
Serial.print(", ");
Serial.print("Tilt, ");
Serial.print(ServoTiltPosition);
Serial.print(", ");
Serial.print("Dist, ");
Serial.print(Distance);
Serial.println();
}