hy-srf05 and servo motor

Projects General Guidance
1

Hello i'm fairly new to Arduino and i've been having a few problems making this new project,
The idea is to make my servo go from 0 to 180 degrees (+30 degrees every 1 second) and back just like the sweep example but when the proximity sensor detects an object closer than 100 cm i want the servo to stop, make the arduino turn a led on and make another servo go from 0 to 90 until the object is further away than 100cm and just repeat the whole process.

My problem appears to be the fact that each time any servo moves the sensors data lags making it print to the serial monitor false data, making the servos lag and making the led flash.

Here is my code,
many thanks.

#include <Servo.h>
//sensor stuff
#define trig 9
#define echo 8
long duration, distance;

const int ledPin = 13;
int a = 40;
int increment = 10;
unsigned long previousMillis = 0;
unsigned long previousServoMillis = 0;
const int interval = 500;

Servo servo;
Servo servoReload;

void setup() {
Serial.begin(9600);
servo.attach(10);
servoReload.attach(11);

pinMode(trig, OUTPUT);//sensor stuff
pinMode(echo, INPUT);
pinMode(ledPin, OUTPUT);
}

void loop() {

unsigned long currentMillis = millis();

//sensor stuff
digitalWrite(trig, LOW);
delayMicroseconds(2);
digitalWrite(trig, HIGH);
delayMicroseconds(10);
digitalWrite(trig, LOW);
duration = pulseIn(echo, HIGH);
distance = duration / 58;
Serial.println(distance); //end of sensor stuff

if(distance<100){
if(currentMillis - previousMillis >= interval){
previousMillis = currentMillis;
digitalWrite(ledPin, HIGH);
servoReload.write(90);

}
}

else{
digitalWrite(ledPin, LOW);
servoReload.write(0);}

if(currentMillis - previousMillis >= interval){
previousMillis = currentMillis;
servo.write(a);
a +=increment;
if ((a >= 140) || (a <= 40)) { increment = -increment;}

}
}

2

Please use the code button </>

so your code is easy to copy to a text editor. Also, it is much easier to follow if you format it carefully. Like this.
[code]#include <Servo.h>
//sensor stuff
#define trig 9
#define echo 8
long duration, distance;

const int ledPin = 13;
int a = 40;
int increment = 10;
unsigned long previousMillis = 0;
unsigned long previousServoMillis = 0;
const int interval = 500;

Servo servo;
Servo servoReload;

void setup() {
    Serial.begin(9600);
    servo.attach(10);
    servoReload.attach(11);

    pinMode(trig, OUTPUT);//sensor stuff
    pinMode(echo, INPUT);
    pinMode(ledPin, OUTPUT);
}

void loop() {
 
    unsigned long currentMillis = millis();
    //sensor stuff
    digitalWrite(trig, LOW);
    delayMicroseconds(2);
    digitalWrite(trig, HIGH);
    delayMicroseconds(10);
    digitalWrite(trig, LOW);
    duration = pulseIn(echo, HIGH);
    distance = duration / 58;
    Serial.println(distance); //end of sensor stuff

    if(distance < 100){
        if(currentMillis - previousMillis >= interval){
            previousMillis = currentMillis;
            digitalWrite(ledPin, HIGH);
            servoReload.write(90);
        }
    }   
    else{
        digitalWrite(ledPin, LOW);
        servoReload.write(0);
    }

    if(currentMillis - previousMillis >= interval){
        previousMillis = currentMillis;
        servo.write(a);
        a += increment;
        if ((a >= 140) || (a <= 40)) {
            increment = -increment;
        }
    }
}

I think if you study your code you will see that there is conflicting stuff. For example if the short distance is triggered whatever it does will be undone by IF at line 52. And the ELSE at line 47 will screw up whatever was done after line 52.

I suspect you need to use one or two extra variables to record the state of your system - for example
objectClose = true; which then is used to prevent things from happening, or to allow things to happen. For example

if (objectClose == true) {
  // code to move the second servo
}

...R