#include <Keypad.h>
#include <Servo.h>

const byte ROWS = 4;
const byte COLS = 4;

// Array to represent keys on keypad
char hexaKeys[ROWS][COLS] = {
  {'1', '2', '3', 'A'},
  {'4', '5', '6', 'B'},
  {'7', '8', '9', 'C'},
  {'*', '0', '#', 'D'}
};

byte rowPins[ROWS] = {13, 12, 11, 10};
byte colPins[COLS] = {9, 8, 7, 6};
Keypad customKeypad = Keypad(makeKeymap(hexaKeys), rowPins, colPins, ROWS, COLS);
Servo Myservo;

boolean TimePeriodIsOver (unsigned long &expireTime, unsigned long TimePeriod) {
  unsigned long currentMillis  = millis();  
  if ( currentMillis - expireTime >= TimePeriod ){
    expireTime = currentMillis; // set new expireTime
    return true;                // more time than TimePeriod) has elapsed since last time if-condition was true
  } 
  else return false;            // not expired
}

unsigned long MyServoTimer = 0;  // Timer-variables MUST be of type unsigned long

const byte waitForKeyPress        = 0;
const byte turnServo              = 1;
const byte keepPositionFor1Second = 2;

byte myState = waitForKeyPress;

// State-Machine  State-Machine  State-Machine  State-Machine  State-Machine  State-Machine 
void myStateMachine() {
  char customKey = customKeypad.getKey();
  
  switch (myState) {

    case waitForKeyPress:
      if (customKey != NO_KEY) {
        Serial.print(customKey);
        Serial.println ( " button pressed ");
        myState = turnServo;
      }
      break; // jump immidiately to end of switch-case

    case turnServo:
      Myservo.write(180); // writeMicroseconds would requiere values between 1000 and 2000
      MyServoTimer = millis();  // store actual timestamp in timer-variable
      myState = keepPositionFor1Second;
      break; // jump immidiately to end of switch-case

    case keepPositionFor1Second:
      if ( TimePeriodIsOver(MyServoTimer,1000) ) {
        // if more than 1000 milliseconds of time have passed by
        Myservo.write(0);
        myState = waitForKeyPress; // set state-machine to state waitForKeyPress
      }  
      break; // jump immidiately to end of switch-case
  } // END of switch-case
} // end of function State-Machine  


void setup() {
  Serial.begin(9600);
  Myservo.attach(4); // you can use an IO-pin only for ONE purpose 
  Serial.println("Setup-Start");
}


void loop() {
  myStateMachine(); // call state-machine over and over endlessly
}