#include <Servo.h>
Servo myservo;

const byte pinservo  = 3;
const byte PinTombol = 2;
const byte pinSingleStepButton = 12;

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);
  pinMode(pinSingleStepButton, INPUT_PULLUP);
  Serial.println("Tombol Servo ");
  myservo.write(sudut);
}


void loop() {
  Serial.println("1. top of 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() {

  Serial.println("2. entering myServoStateMachine()");

  switch (myState) {

    case sm_checkForButtonPress:
      if ( digitalRead(PinTombol) == LOW) { // if button is pressed
        Serial.println("state checkForButtonPress button-press detected");
        waitForSingleStepButtonPress();
        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");
      waitForSingleStepButtonPress();
      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("3. state waitBeforeServoMove waiting-time is over");
        waitForSingleStepButtonPress();
        myState = sm_ServoMove;
      }
      break; // immidiately jump down to END-OF-SWITCH

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


void waitForSingleStepButtonPress() {
  static boolean messagePrinted;
  messagePrinted = false;
  // as long as button is UNpressed keep whirling inside the while-loop
  while (digitalRead(pinSingleStepButton) == HIGH)  {
    if (messagePrinted == false) { // only in case the message has not yet been printed
      Serial.println("press SingleStep-Button to proceed one step");
      messagePrinted = true; // message has been printed set flag to false to stop printing again
    }
    // do nothing else but waiting for the button to be pressed
  }
  delay(500); // using a delay here because a blocking waiting makes sense here
}