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{ 
 }
}

• geometrikal: print distance to the serial port monitor and check it gets above 100
• jfpoilpret: Your loops to move both servos last 179 x 2 x 15 ms = 5.4 seconds, which means every time distance is less than 1 meter, your servos will move during 5.4 seconds. Also note that pulseIn will return 0 if the sensor does not detect any obstacle within its range 5the range depends on the sensor you use, but it is typically 3 to 4 meters).
• Chris Stratton: If your servos "keep spinning" then they have been modified from normal positioning mode to instead be continuous rotation. That means that there's only a specific narrow PWM input width which will cause them to stop. Typically it is adjustable with a potentiometer through a hole in the case. Otherwise, change the value of your "stop" condition PWM output width in your code.

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.