Trying to control two HS-311 standard Servos with two HC-SR04 Ultrasonic Sensors

Hey everyone!

For my project i’m trying to create a motion following animatronic head,

I was Lucky enough to find a perfect code on Instructables to work from ( here’s the link to it).

And here is a video of the original code in action: Youtube.

(Just like to restate that I did not create the original code I got it from someone who was nice enough to share)

So I’ve been working to adjust the code to control 2 servos, and have one servo follow what the sensors detect and have the other do a basic sweep when the sensors detect something at or under 5 inches away.

I’m very much a beginner when it comes to coding so this has proven to be a bit difficult, I tried creating 2 different loops but I either came up with errors or only one servo would work while the other did nothing.

Here is the code I’ve been working with:

#include <Servo.h> 

Servo yservo; Servo xservo;//servos for x and y

const int Lin = 10, Rin = 12, Lout = 11, Rout = 13, yserv = 9, xserv = 8; //setting sensor pins and servo pin

// establish variables for duration 
// and the distance result in inches
long Rduration, Lduration, Rinches, Linches;

int threshold = 10; //Sensor threshold in inches

int angle = 80; //Initial angle

int pos = 0;

boolean debug = false; //Serial communication for debuging. Set to true for serial communication.

void setup() {
  // initialize serial communication:
  if (debug)
  {
    Serial.begin(9600);
  }
  yservo.attach(9); //attach servo to pin 9
  xservo.attach(8);// attach servo to pin 8
}

void loop()
{
  
  //Most of the sensor code has been taken from David Mellis's PING sensor code
  //I modified it for a 4 pin sensor as oppsed to the 3 pin sensor
  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
  pinMode(Rout, OUTPUT);
  digitalWrite(Rout, LOW);
  delayMicroseconds(2);
  digitalWrite(Rout, HIGH);
  delayMicroseconds(5);
  digitalWrite(Rout, LOW);

  Rduration = pulseIn(Rin, HIGH);
  
  pinMode(Lout, OUTPUT);
  digitalWrite(Lout, LOW);
  delayMicroseconds(2);
  digitalWrite(Lout, HIGH);
  delayMicroseconds(5);
  digitalWrite(Lout, LOW);

  Lduration = pulseIn(Lin, HIGH);

  // convert the time into a distance
  Rinches = microsecondsToInches(Rduration);
  Linches = microsecondsToInches(Lduration);
  
  if (debug)
  {
    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;
}

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;
  }
  yservo.write(angle);

{ 
  if (Linches <= 5 || Rinches <= 5)
{ 
  for(pos = 0; pos <= 180; pos += 1) // goes from 0 degrees to 180 degrees 
  {                                  // in steps of 1 degree 
    xservo.write(pos);              // tell servo to go to position in variable 'pos' 
    delay(15);                       // waits 15ms for the servo to reach the position 
  } 
  for(pos = 180; pos>=0; pos-=1)     // goes from 180 degrees to 0 degrees 
  {                                
    xservo.write(pos);              // tell servo to go to position in variable 'pos' 
    delay(15);                       // waits 15ms for the servo to reach the position 
  } 
  }
   
 
  xservo.write(angle);
  
}
}

I would really appreciate some help or if someone could point me in the right direction. Let me know If there is any information needed that I have not included,
Thank you!

You need to move away from using for loops and delays as that will restrict to moving one servo then the other servo because one loop will need to end before the other starts. Look at the BlinkWithoutDelay example in the IDE and examine the principle that it uses.

Save the start time of an action, such as moving a servo a small distance then each time through loop() check whether it is time to move again. If so do it, else do something else such as checking whether it is time to move a second servo a little. If so do it, else go back round loop() again.

By the way, how are the servos powered ? Do they have an external power supply (correct answer) or are they powered from the Arduino (wrong answer).