/*
   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
*/

#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 target = 90;         // target angle
    uint16_t current = 90;        // current angle
    uint8_t interval = 50;        // delay time
    uint32_t previousMillis = 0;  // time management
  public:
    Servo servo;

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

    void setSpeed(uint8_t newSpeed) {
      interval = newSpeed;
    }

    void set(uint16_t newTarget) {
      target = newTarget;
    }

    void update() {
      if (millis() - previousMillis > interval) {
        previousMillis = millis();
        if (target < current) {
          current--;
          servo.write(current);
        }
        else if (target > current) {
          current++;
          servo.write(current);
        }
      }
    }
};

SlowServo myservo;  // create a servo object

void doorOpen() {
  Serial.println(F("open"));
  myservo.set(90);
  //myservo.servo.write(90); // hardcoded - direct acccess - write
}

void doorClose() {
  Serial.println(F("close"));
  myservo.set(0);
}

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

void loop() {
  if (digitalRead(openPin) == LOW) {
    doorOpen();
  }
  if (digitalRead(closePin) == LOW) {
    doorClose();
  }
  myservo.update();  // call the update method in loop
}