/* 3-pin ultrasonic X2 sensors with servo. A motion-following device for a kinetic sculpture
This sketch is based on sketches by David Mellis and Calvin Kielas-Jensen but adapted for two three-pin sensors
The circuit:
+V connection of the PING))) attached to +5V
GND connection of the PING))) attached to ground
SIG connections of the PINGS))) attached to digital pin 7 and 8, servo on pin 9
This example code will be in the public domain if I can ever get in working
*/
#include <NewPing.h>
#include <Servo.h>
Servo myservo;
// this constant won't change. It's the pin number
// of the sensor's output:
const int lPingPin = 8, rPingPin = 7;
// establish variables for duration
// and the distance result in inches
long Rduration, Lduration, Rinches, Linches;
int threshold = 15; //Sensor threshold in inches
int angle = 80; //Initial angle
void setup() {
// initialize serial communication:
{
Serial.begin(9600);
}
myservo.attach(9);
}
void loop() {
// establish variables for duration of the ping,
// and the distance result in inches and centimeters:
long duration, inches, cm;
// The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
// Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
pinMode(lPingPin, OUTPUT);
digitalWrite(lPingPin, LOW);
delayMicroseconds(2); //**Here I doubled David's single sensor code and replaced "pingPin with lPingPin and rPingPin
digitalWrite(lPingPin, HIGH); // for the left and right sensors
delayMicroseconds(5);
digitalWrite(lPingPin, LOW);
pinMode(lPingPin, INPUT);
pinMode(rPingPin, OUTPUT);
digitalWrite(rPingPin, LOW);
delayMicroseconds(2);
digitalWrite(rPingPin, HIGH);
delayMicroseconds(5);
digitalWrite(rPingPin, LOW);
pinMode(rPingPin, INPUT);
// The same pin is used to read the signal from the PING))): a HIGH
// pulse whose duration is the time (in microseconds) from the sending
// of the ping to the reception of its echo off of an object.
pinMode(lPingPin, INPUT); //IS THIS CORRECT???
pinMode(rPingPin, INPUT); //IS THIS CORRECT???
duration = pulseIn(lPingPin, HIGH); //IS THIS CORRECT???
duration = pulseIn(rPingPin, HIGH); //IS THIS CORRECT???
// convert the time into a distance
inches = microsecondsToInches(duration);
cm = microsecondsToCentimeters(duration);
{
Serial.print("Left: ");
Serial.print(Linches);
Serial.println(" in");
Serial.print("Right: ");
Serial.print(Rinches);
Serial.println(" in");
}
follow();
}
long microsecondsToInches(long microseconds) {
// According to Parallax's datasheet for the PING))), there are
// 73.746 microseconds per inch (i.e. sound travels at 1130 feet per
// second). This gives the distance travelled by the ping, outbound
// and return, so we divide by 2 to get the distance of the obstacle.
// See: http://www.parallax.com/dl/docs/prod/acc/28015-PING-v1.3.pdf
return microseconds / 74 / 2;
}
long microsecondsToCentimeters(long microseconds) {
// The speed of sound is 340 m/s or 29 microseconds per centimeter.
// The ping travels out and back, so to find the distance of the
// object we take half of the distance travelled.
return microseconds / 29 / 2;
}
void follow()
{
if (Linches <= threshold || Rinches <= threshold)
{
if (Linches + 2 < Rinches)
{
angle = angle - 2;
}
if (Rinches + 2 < Linches)
{
angle = angle + 2;
}
}
if (angle > 160)
{
angle = 160;
}
if (angle < 0)
{
angle = 0;
}
myservo.write(angle);
}