/* Slow Servo Movements
   based on request of https://forum.arduino.cc/t/i-want-the-servo-to-be-slower-how-do-i-control-the-speed-of-the-servo-servo-speed-control/1012695/
   
   by noiasca https://werner.rothschopf.net/microcontroller/202207_millis_slow_servo.htm
   2022-07-15
   modified using IMHO better self-explaining variable-names
   added comments that explain what the code does
*/

#include <Servo.h>

constexpr uint8_t openPin  = 4;   //  open door
constexpr uint8_t closePin = 3;   // force close
constexpr uint8_t servoPin = 8;   // GPIO for Servo

// make your own servo class
class SlowServo {
  protected:
    uint16_t targetAngle  = 90;       // target angle
    uint16_t currentAngle = 90;      // current angle
    uint8_t interval      = 50; // delay time in milliseconds waited between each mini-move of the servo
    uint32_t MillisAtStartOfPeriod = 0;
  public:
    Servo servo;

    void begin(byte pin) {
      servo.attach(pin);
    }

    void setSlowDownInterval(uint8_t newInterval) {
      interval = newInterval;
    }

    void setAngle(uint16_t newTargetAngle){
      targetAngle = newTargetAngle;
    }

    // if the  servo haven't yet reached its target position 
    // continue with slow moving
    void updateServoAngle() { 
      // check if more milliseconds than stored in variable interval have passed by
      if (millis() - MillisAtStartOfPeriod > interval) { 
        // if more milliseconds HAVE passed by
        MillisAtStartOfPeriod = millis(); // update variable that holds timestamp at MillisAtStartOfPeriod

        if (targetAngle < currentAngle) { // example targetAngle = 10 currentAngle 50 => decrease current
          currentAngle--;
          servo.write(currentAngle);
        }
        else if (targetAngle > currentAngle) { // example targetAngle = 160 currentAngle 110 => increase currentAngle
          currentAngle++;
          servo.write(currentAngle);
        }
      }
    }
};

SlowServo myservo;  // create a servo object

void setup() {
  Serial.begin(115200);
  myservo.begin(servoPin);          // start the servo object
  pinMode(openPin, INPUT_PULLUP);
  pinMode(closePin, INPUT_PULLUP);
  myservo.setSlowDownInterval(80);
}

void OpenDoor() {
  Serial.println(F("open"));
  myservo.setAngle(90);
  //myservo.servo.write(90); // hardcoded write
}

void CloseDoor() {
  Serial.println(F("close"));
  myservo.setAngle(0);
  // myservo.servo.write(0); // hardcoded write
}

void loop() {
  if (digitalRead(openPin) == LOW) {    
    OpenDoor(); // sets a new targetAngle
  }

  if (digitalRead(closePin) == LOW) {     
    CloseDoor(); // sets a new targetAngle.
  }
  // call the update method in loop 
  // As long as currentAngle is unequal to the targetAngle
  // the servo keeps continuing slow changing its angle
  myservo.updateServoAngle();  
}