/*
   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;
  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 setup() {
  Serial.begin(115200);
  myservo.begin(servoPin);          // start the servo object
  pinMode(openPin, INPUT_PULLUP);
  pinMode(closePin, INPUT_PULLUP);
}

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

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

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