Hey All,
I am having some trouble with my code and I could use some advice as to how I need to proceed. The overview of the project is that I have two boxes, one box with four servos and an ultrasonic sensor in it and another box with two servos and the board inside of it. The board is a Mega 2560.
The four servo box is supposed to open four doors, wait 2500 milliseconds, and then close and wait for 2900 milliseconds. It should do this continually until the ultrasonic sensor detects something close at which point the four servo box should close, and the two servo box will open and close fast (around 1000 milliseconds). Then I want it to cycle back to opening and closing the four servo box until the sensor is tripped again.
The problem that I am having is that after a couple of cycles of tripping the sensor and the two servo box rapidly opening and closing, the four servo box starts "freaking out" and will open and close rapidly a few times and then the servos stay open for a few seconds (10-15) and then it shuts. It will not do anything more until I unplug it from all power and then plug it back in or hit the reset button.
This is my first post on the forum, so I hope that I have inserted the code correctly and I appreciate any help that you can give me.
// Include the Servo library
#include <Servo.h>
//declare pins
int servoPin1 = 52;
int servoPin2 = 50;
int servoPin3 = 48;
int servoPin4 = 46;
int servoPin5 = 44;
int servoPin6 = 42;
int trigPin=24;
int echoPin=22;
int pingTravelTime;
//rename servos
Servo Servo1;
Servo Servo2;
Servo Servo3;
Servo Servo4;
Servo Servo5;
Servo Servo6;
//initialize and attach components
void setup() {
Serial.begin(9600);
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
Servo1.attach(servoPin1);
Servo2.attach(servoPin2);
Servo3.attach(servoPin3);
Servo4.attach(servoPin4);
Servo5.attach(servoPin5);
Servo6.attach(servoPin6);
}
// running continous code
void loop()
{
long duration, distance;
digitalWrite(trigPin,HIGH);
delay(15);
digitalWrite(trigPin,LOW);
duration=pulseIn(echoPin,HIGH);
distance =(duration/2)/29.1;
delay(15);
pinMode(echoPin,INPUT);
duration = pulseIn(echoPin,HIGH);
Serial.print(distance);
Serial.println();
if (distance > 30)
{
Servo1.write(0);
Servo3.write(0);
Servo2.write(0);
Servo4.write(0);
delay(2500);
Servo1.write(50);
Servo3.write(50);
Servo2.write(50);
Servo4.write(50);
delay(2900);
}
else
{
Servo1.write(0);
Servo3.write(0);
Servo2.write(0);
Servo4.write(0);
delay(1000);
Servo5.write(0);
Servo6.write(0);
delay(1000);
Servo5.write(50);
Servo6.write(50);
delay(1000);
Servo5.write(0);
Servo6.write(0);
delay(1000);
Servo5.write(50);
Servo6.write(50);
delay(1000);
Servo5.write(0);
Servo6.write(0);
delay(1000);
Servo5.write(50);
Servo6.write(50);
delay(1000);
Servo5.write(0);
Servo6.write(0);
delay(1000);
Servo5.write(50);
Servo6.write(50);
delay(1000);
Servo5.write(0);
Servo6.write(0);
delay(1000);
Servo5.write(50);
Servo6.write(50);
delay(1000);
Servo5.write(0);
Servo6.write(0);
delay(1000);
Servo5.write(50);
Servo6.write(50);
delay(1000);
Servo5.write(0);
Servo6.write(0);
delay(1000);
Servo5.write(50);
Servo6.write(50);
delay(1000);
Servo5.write(0);
Servo6.write(0);
delay(1000);
Servo5.write(50);
Servo6.write(50);
delay(10000);
}
}
See