/* 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) {
  }
}