#include <Servo.h>
Servo myservo;

#define pinservo 3
#define PinTombol 2

int sudut = 0;
int sudutStep = 10;
const int minSudut = 0;
const int maxSudut = 90;

int TekanTombol = 0;

// states of the state-machine
const byte sm_checkForButtonPress  = 0;
const byte sm_waitBeforeServoMove  = 1;
const byte sm_CalculateNewServoPos = 2;
const byte sm_ServoMove            = 3;

byte myState = sm_checkForButtonPress; // state-variable

unsigned long myWaitTimer; // variable for non-blocking timing


void setup() {
  Serial.begin(9600);
  myservo.attach(pinservo);
  pinMode(PinTombol, INPUT_PULLUP);
  Serial.println("Tombol Servo ");
  myservo.write(sudut);
}


void loop() {
  myServoStateMachine();
}

// easy to use helper-function for non-blocking timing
boolean TimePeriodIsOver (unsigned long &startOfPeriod, unsigned long TimePeriod) {
  unsigned long currentMillis  = millis();
  if ( currentMillis - startOfPeriod >= TimePeriod ) {
    // more time than TimePeriod has elapsed since last time if-condition was true
    startOfPeriod = currentMillis; // a new period starts right here so set new starttime
    return true;
  }
  else return false;            // actual TimePeriod is NOT yet over
}

void myServoStateMachine() {

  switch (myState) {

    case sm_checkForButtonPress:
      if ( digitalRead(PinTombol) == LOW) { // if button is pressed
        Serial.println("state checkForButtonPress button-press detected");
        myWaitTimer = millis(); // store actual time into variable "myWaitTimer"
        myState = sm_CalculateNewServoPos ;
      }
      break; // immidiately jump down to END-OF-SWITCH

    case sm_CalculateNewServoPos :
      Serial.println("state CalculateNewServoPos");
      sudut = sudut + sudutStep;
      if (sudut >= maxSudut) {
        sudutStep = -sudutStep;
      }

      if (sudut <= minSudut) {
        sudutStep = -sudutStep;
      }

      Serial.println("start waiting ...");
      myState = sm_waitBeforeServoMove;
      break; // immidiately jump down to END-OF-SWITCH

    case sm_waitBeforeServoMove:
      // check if 2000 milliseconds have passed by
      if ( TimePeriodIsOver(myWaitTimer, 2000) ) {
        // in case 2000 milliseconds REALLY have passed by 
        Serial.println("state waitBeforeServoMove waiting-time is over");
        myState = sm_ServoMove;
      }
      break; // immidiately jump down to END-OF-SWITCH

    case sm_ServoMove:
      Serial.println("state ServoMove");
      myservo.write(sudut);
      Serial.print("Bergerak ke: ");
      Serial.print(sudut);
      Serial.println(" Derajat");
      myState = sm_checkForButtonPress;
      break; // immidiately jump down to END-OF-SWITCH
  } // END-OF-SWITCH
}