Problems controlling servos using proximity sensor

Question

I've just started using Arduino and I need to make a code to simulate the behavior of a lizard, meaning that if you come close to it, it moves away from you.

I'm using a "Arduino duemilanove" that my university lend me, I was told that it was an alternative version of Arduino Uno.

Both of the servos says "Tower Pro, mg996r, digihi-speed"

Looking different codes I made one which I think is correct. Also, I checked for each servo motor (I'm using two) and the proximity sensor running codes for everyone of them alone, if they worked well and they did but with this code, they supposed to move when you are closer than a meter but they don't stop spinning.

If anyone could help me with this, it would be great.

Here is the code:

#define Servo1 9
#define Servo2 11
#define trigPin 10
#define echoPin A0

#include <Servo.h>

Servo servo1;
Servo servo2;

int pos = 0;    // variable to store the servo position 

void setup() {
  Serial.begin (9600);
  pinMode(echoPin, INPUT);
  pinMode(trigPin, OUTPUT);
  pinMode(Servo1, OUTPUT);
  pinMode(Servo2, OUTPUT);
  servo1.attach(9);
  servo2.attach(11);

}

void loop(){
  long duracion, distancia; // variables

  /* Hacer el disparo */
  digitalWrite(10, LOW);  
  delayMicroseconds(2); 
  digitalWrite(10, HIGH);  // Flanco ascendente
  delayMicroseconds(10);        // Duracion del pulso
  digitalWrite(10, LOW);  // Flanco descendente

  /* Recepcion del eco de respuesta */
  duracion = pulseIn(A0, HIGH);

  /* Calculo de la distancia efectiva */
  distancia = (duracion/2) / 29.1;

  if (distancia<100){
    for(pos = 0; pos < 180; pos += 1)  // goes from 0 degrees to 180 degrees 
    {                                  // in steps of 1 degree 
      servo1.write(pos);    // tell servo to go to position in variable 'pos' 
      servo2.write(pos);
      delay(15);                       // waits 15ms for the servo to reach the                 position 
    } 
    for(pos = 180; pos>=1; pos-=1)     // goes from 180 degrees to 0 degrees 
    {                                
      servo1.write(pos);              // tell servo to go to position in variable                 'pos' 
      servo2.write(pos);
      delay(15);                       // waits 15ms for the servo to reach the position 
    } 

  }
  else{ 
  }
}

Answer 1

I am new to servos as well, but I do have two coding thoughts. The servos I have used in the past have started in the halfway position. That would mean the "pos" variable would start off trying to go from the starting 90 position to the 0 value in 15ms. Not enough time before the next for loop. I could see that causing some weird spinning while the servos try to get to the intended position. So..

int pos = 90; // Start servo position.

And secondly, the "distancia" could just always be less than 100. I would try finding a max distance value to compare to vs a hard coded value.

void loop() {...
long duracion, distancia; // variables
boolean isMoveTrigger = false; // Movement trigger.

/* Calculo de la distancia efectiva */
distancia = (duracion/2) / 29.1;

// Store a maximum value.
if (distancia > maxDistance) {
    maxDistance = distancia;
}

// Wait to get a starting maximum distance value.
if (countDown > 0) {
    countDown--;
} else {
    if (distancia > 0) {
      int distancePercentage = (maxDistance * 100) / distancia; // A 0-100 percentage of the maximum value.
      // Then trigger the movement when less than this percentage.
      isMoveTrigger = distancePercentage < 50;
    }
}
if (isMoveTrigger) {
    for(pos = 0; pos < 180; pos += 1) ...

Answer 2

As an extra tip, from my experience do not power the servos from the same power supply as the sensors. I built a similar project to you and it didn't work as expected. I then powered the servos directly from the battery (6v) and the sensors and arduino from a regulated 5v supply taken from the battery. The servos can cause power spikes that mess with the sensor readings.