Hello, I'm trying to make an obstacle-avoiding robot using an ultrasonic sensor and 2 servos. and when my sensor kicks in I just want one leg to move but the right still moves but in a delay. What can I do to fix this?
Code
#include <Servo.h>
const int TrigPin = A0;
const int EchoPin = A1;
Servo myservo1;
Servo myservo2;
const int DISTANCE_THRESHOLD = 50;
int pos = 0;
int pos1 = 90;
int pos2 = 80;
int pos3 = 100;
float duration_us, distance_cm;
void setup() {
Serial.begin (9600);
pinMode(TrigPin, OUTPUT);
pinMode(EchoPin, INPUT);
myservo1.attach(9);
myservo2.attach(10);
}
void loop() {
digitalWrite(TrigPin, HIGH);
delayMicroseconds(10);
digitalWrite(TrigPin,LOW);
duration_us = pulseIn(EchoPin,HIGH);
distance_cm = 0.017 * duration_us;
myservo1.write(pos1);
myservo2.write(pos1);
delay(400);
myservo1.write(pos1 - pos2);
delay(40);
myservo2.write(pos1 + pos2);
delay(400);
myservo1.write(pos1 - pos2);
delay(400);
myservo2.write(pos1 + pos2);
delay(400);
if(distance_cm < DISTANCE_THRESHOLD) {
for (pos = 0; pos <= pos3; pos += 1) {
myservo1.write(pos);
delay(15);
}
for (pos = pos3; pos >= 0; pos -= 1) {
myservo1.write(pos);
delay(15);
}
}
}
