#include <Servo.h>

enum {
       WAIT,
       GO00,
       GO90,
       GO120,
       SWEEP4
} servo_states;

Servo aServo;  
byte state = WAIT;
byte SweepCount = 0;
int ServoPos = 0;
unsigned long servoMoveTime = 0;
const int servoOnTime = 10;
boolean servoOn = false;

struct Button_type {
      byte Pin;
      unsigned long lastButtonUsedTime = 0;
      byte lastButtonState = HIGH;
      boolean wasLOW = false;      
};

const byte NoOfButtons = 2;
Button_type Button[NoOfButtons];

int pos = 0;    

void setup() {
  Button[0].Pin = 4;
  Button[1].Pin = 5;
  for (int i = 0; i<NoOfButtons;i++) pinMode(Button[i].Pin, INPUT_PULLUP);

  aServo.attach(6);
  aServo.write(ServoPos);
  delay(1000);
}  

void loop() {
   HandleButtons();
   HandleServo();
}

void HandleButtons(){
   if (ButtonReleased(0)) {        
     servoOn = true;
     SweepCount = 0;
     if (ServoPos > 0) state = GO00;
                  else state = GO120;
  }
   if (ButtonReleased(1)) {          
     servoOn = true;
     SweepCount = 4;
     if (ServoPos > 0) {SweepCount++; state = GO00;}  // if you are not at Pos 0, go there first
                  else state = GO90;
  }

}

void HandleServo(){
 if (servoOn && millis() - servoMoveTime >= servoOnTime) {
    servoMoveTime = millis();
    // Time to move the servo
    switch (state) {
      case GO00:
          if (ServoPos <= 0) {
            if (SweepCount > 0) SweepCount--;
            if (SweepCount > 0) state = GO90;
                           else state = WAIT;
          }
          ServoPos--;
          aServo.write(ServoPos);
          break;
      case GO90:
          if (ServoPos >= 90) {
            if (SweepCount > 0) state = GO00;
                           else state = WAIT;
          }
          ServoPos++;
          aServo.write(ServoPos);
          break;
      case GO120:
          if (ServoPos >= 120) state = WAIT;
          ServoPos++;
          aServo.write(ServoPos);
          break;
      case SWEEP4:  
           break;
      case WAIT:  
      default:
        servoOn = false;
        state = WAIT;
        break;
    }
  }
}

// Check if Button is released and debounced
boolean ButtonReleased(byte No){
  int ButtonState = digitalRead(Button[No].Pin);          
  if (ButtonState != Button[No].lastButtonState) {
    Button[No].lastButtonUsedTime = millis();
    Button[No].lastButtonState = ButtonState;
    Button[No].wasLOW = (ButtonState == LOW);
  }
  if (millis()-Button[No].lastButtonUsedTime > 50 && Button[No].wasLOW) {  
     Button[No].wasLOW = false;                                 
     return true;                                    
  } else {                                           
     return false;                                   
  };   
}