/* Sweep
by BARRAGAN <http://barraganstudio.com>
This example code is in the public domain.
modified 8 Nov 2013
by Scott Fitzgerald
http://www.arduino.cc/en/Tutorial/Sweep
*/
#include <Servo.h>
Servo myservo1;
Servo myservo2; // create servo object to control a servo
// twelve servo objects can be created on most boards
int pos = 0;
int pos2 = 0;
byte stateButtonUp = 0;
bool initStateUp = true;
byte stateButtonDown = 0;
bool initStateDown = true;
unsigned int nextTime = 0;
void setup()
{
Serial.begin(115200);
pinMode(2, INPUT);
myservo1.attach(9);
myservo2.attach(6);
}
void loop() {
if (digitalRead (2) == HIGH) {
buttonUpFunction();
} else {
buttonDownFunction();
}
}
void buttonUpFunction() {
if (millis() > nextTime) {
Serial.print("Up: ");
Serial.print(stateButtonUp);
Serial.print(" pos: ");
Serial.print(pos);
Serial.print(" pos2: ");
Serial.println(pos2);
switch (stateButtonUp) {
case 0:
//for (pos = 0; pos <= 80; pos ++) {
// myservo1.write(pos);
// delay(40);
//}
if (initStateUp == true) {
pos = 0;
initStateUp = false;
}
if (pos <= 80) {
myservo1.write(pos);
pos++;
} else {
stateButtonUp = 1;
initStateUp = true;
}
nextTime = millis() + 40;
break;
case 1:
//for (pos2 = 60; pos2 >= 0; pos2 --) {
// myservo2.write(pos2);
//delay(40);
//}
if (initStateUp == true) {
pos2 = 60;
initStateUp = false;
}
if (pos2 >= 0) {
myservo1.write(pos);
pos2 --;
} else {
stateButtonUp = 2;
initStateUp = true;
}
nextTime = millis() + 40;
break;
case 2:
//for (pos2 = 0; pos2 <= 40; pos2 += 1) {
// myservo2.write(pos2);
// delay(80);
//}
if (initStateUp == true) {
pos2 = 0;
initStateUp = false;
}
if (pos2 <= 40) {
myservo2.write(pos2);
pos2 += 1;
} else {
stateButtonUp = 3;
initStateUp = true;
}
nextTime = millis() + 80;
break;
case 3:
//for (pos = 80; pos <= 180; pos += 1) {
// myservo1.write(pos);
// delay(50);
//}
if (initStateUp == true) {
pos = 80;
initStateUp = false;
}
if (pos <= 180) {
myservo1.write(pos);
pos += 1;
} else {
stateButtonUp = 4;
initStateUp = true;
}
nextTime = millis() + 50;
break;
case 4:
//for (pos2 = 40; pos2 >= 120; pos2 -= 1) {
// myservo2.write(pos2);
// delay(50);
//}
if (initStateUp == true) {
pos2 = 40;
initStateUp = false;
}
if (pos2 <= 120) {
myservo2.write(pos2);
pos2 += 1;
} else {
stateButtonUp = 5;
initStateUp = true;
}
nextTime = millis() + 50;
break;
case 5:
//for (pos = 180; pos <= 80; pos += 1) {
// myservo1.write(pos);
// delay(40);
//}
if (initStateUp == true) {
pos = 180;
initStateUp = false;
}
if (pos >= 80) {
myservo1.write(pos);
pos -= 1;
} else {
stateButtonUp = 6;
initStateUp = true;
}
nextTime = millis() + 40;
break;
case 6:
//for (pos2 = 120; pos2 <= 40; pos2 += 1) {
// myservo2.write(pos2);
// delay(130);
//}
if (initStateUp == true) {
pos2 = 120;
initStateUp = false;
}
if (pos2 >= 40) {
myservo2.write(pos2);
pos2 -= 1;
} else {
stateButtonUp = 7;
initStateUp = true;
}
nextTime = millis() + 130;
break;
case 7:
//for (pos = 80; pos <= 180; pos += 1) {
// myservo1.write(pos);
// delay(50);
//}
if (initStateUp == true) {
pos = 80;
initStateUp = false;
}
if (pos <= 180) {
myservo1.write(pos);
pos += 1;
} else {
stateButtonUp = 8;
initStateUp = true;
}
nextTime = millis() + 50;
break;
case 8:
//for (pos2 = 40; pos2 >= 0; pos2 -= 1) {
// myservo2.write(pos2);
// delay(50);
//}
if (initStateUp == true) {
pos2 = 40;
initStateUp = false;
}
if (pos2 >= 0) {
myservo2.write(pos2);
pos2 -= 1;
} else {
stateButtonUp = 0;
initStateUp = true;
}
nextTime = millis() + 50;
break;
default:
stateButtonUp = 0;
} // end switch (stateButtonUp) {
}
}
void buttonDownFunction() {
if (millis() > nextTime) {
Serial.print("Down: ");
Serial.print(stateButtonDown);
Serial.print(" pos: ");
Serial.println(pos);
switch (stateButtonDown) {
case 0:
//for (pos = 90; pos >= 19; pos --) {
// myservo1.write(pos);
// myservo2.write(180 - pos);
//}
//delay(50);
if (initStateDown == true) {
pos = 90;
initStateDown = false;
}
if (pos >= 19) {
myservo1.write(pos);
myservo2.write(180 - pos);
pos --;
} else {
stateButtonDown = 1;
initStateDown = true;
}
nextTime = millis() + 50;
break;
case 1:
//for (pos = 19; pos <= 90; pos ++) // goes from 19 degrees to 90 degrees
//{
// myservo1.write(pos); // tell servo to go to position in variable 'pos'
// myservo2.write(180 - pos); // tell servo to go to position in variable 'pos'
// delay(15);
//}
if (initStateDown == true) {
pos = 19;
initStateDown = false;
}
if (pos <= 90) {
myservo1.write(pos);
myservo2.write(180 - pos);
pos++;
} else {
stateButtonDown = 0;
initStateDown = true;
}
nextTime = millis() + 15;
break;
default:
stateButtonDown = 0;
break;
} // end switch (stateButtonDown) {
}
}