#include <Servo.h>
const int servoPin = 9; // the pin number for the servo signal
const int trigPin = 11; //ultrasonic
const int echoPin = 12; //ultrasonic
const int servoMinDegrees = 0; // the limits to servo movement
const int servoMaxDegrees = 180;
Servo myservo; // create servo object to control a servo
int servoPosition = 0; // the current angle of the servo - starting at 90.
int servoSlowInterval = 20; // millisecs between servo moves
int servoFastInterval = 20;
int servoInterval = servoSlowInterval; // initial millisecs between servo moves
int servoDegrees = 2; // amount servo moves at each step
// will be changed to negative value for movement in the other direction
unsigned long currentMillis = 0; // stores the value of millis() in each iteration of loop()
unsigned long previousServoMillis = 0; // the time when the servo was last moved
void setup() {
// put your setup code here, to run once:
Serial.begin(9600);
myservo.write(servoPosition); // sets the initial position
myservo.attach(servoPin);
}
void loop() {
// put your main code here, to run repeatedly:
// Notice that none of the action happens in loop() apart from reading millis()
// it just calls the functions that have the action code
currentMillis = millis(); // capture the latest value of millis()
// this is equivalent to noting the time from a clock
// use the same time for all LED flashes to keep them synchronized
// establish variables for duration of the ping,
// and the distance result in inches and centimeters:
long duration, inches, cm;
// The sensor is triggered by a HIGH pulse of 10 or more microseconds.
// Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
pinMode(trigPin, OUTPUT);
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
// Read the signal from the sensor: 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(echoPin, INPUT);
duration = pulseIn(echoPin, HIGH);
// convert the time into a distance
inches = microsecondsToInches(duration);
cm = microsecondsToCentimeters(duration);
//Tell the Arduino to print the measurement in the serial console
Serial.print(inches);
Serial.print("in, ");
Serial.print(cm);
Serial.print("cm");
Serial.println();
if (cm > 50) {servoSweep();
}
else if (cm <= 50) {myservo.write(servoPosition);
}
}
// Converts the microseconds reading to Inches
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;
}
//Converts the Microseconds Reading to Centimeters
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 servoSweep() {
// this is similar to the servo sweep example except that it uses millis() rather than delay()
// nothing happens unless the interval has expired
// the value of currentMillis was set in loop()
if (currentMillis - previousServoMillis >= servoInterval) {
// its time for another move
previousServoMillis += servoInterval;
servoPosition = servoPosition + servoDegrees; // servoDegrees might be negative
if (servoPosition <= servoMinDegrees) {
// when the servo gets to its minimum position change the interval to change the speed
if (servoInterval == servoSlowInterval) {
servoInterval = servoFastInterval;
}
else {
servoInterval = servoSlowInterval;
}
}
if ((servoPosition >= servoMaxDegrees) || (servoPosition <= servoMinDegrees)) {
// if the servo is at either extreme change the sign of the degrees to make it move the other way
servoDegrees = - servoDegrees; // reverse direction
// and update the position to ensure it is within range
servoPosition = servoPosition + servoDegrees;
}
// make the servo move to the next position
myservo.write(servoPosition);
// and record the time when the move happened
}
}
