#include "SR04.h"
#include <Servo.h>
#define TRIG_PIN 2
#define ECHO_PIN 3
SR04 sr04 = SR04(ECHO_PIN,TRIG_PIN);
long a;
long distance;
long duration;
unsigned long prevTime = millis();
Servo myservo1; //Left leg
Servo myservo2; //Right leg
int pos = 0;
void setup() {
Serial.begin(9600);
delay(1000);
myservo1.attach(7);
myservo2.attach(8);
pinMode(TRIG_PIN,OUTPUT);
pinMode(ECHO_PIN,INPUT);
}
void loop() {
a=sr04.Distance();
Serial.print(a);
Serial.println("cm");
digitalWrite(TRIG_PIN, LOW);
delayMicroseconds(2);
digitalWrite(TRIG_PIN, HIGH);
delayMicroseconds(10);
digitalWrite(TRIG_PIN, LOW);
duration = pulseIn(ECHO_PIN, HIGH);
distance = duration*0.034/2;
unsigned long currentTime = millis();
for (pos = 0; pos <= 180; pos += 1) { // goes from 0 degrees to 180 degrees
// in steps of 1 degree
myservo1.write(pos); // tell servo to go to position in variable 'pos'
}
for (pos = 0; pos <= 180; pos += 1) { // goes from 0 degrees to 180 degrees
// in steps of 1 degree
myservo2.write(pos); // tell servo to go to position in variable 'pos'
}
for (pos = 180; pos >= 0; pos -= 1) { // goes from 180 degrees to 0 degrees
myservo1.write(pos); // tell servo to go to position in variable 'pos'
}
for (pos = 180; pos >= 0; pos -= 1) { // goes from 180 degrees to 0 degrees
myservo2.write(pos); // tell servo to go to position in variable 'pos'
if(currentTime - prevTime > 1000) {
}
}
}