/*
   move Servos with rampup/rampdown speed
   ramp done by time

   by noiasca
   2022-10-29 RampServo - https://wokwi.com/projects/346855291487257172 for https://forum.arduino.cc/t/problem-mit-der-servobewegung-bei-mobatools/1047250/11
   2022-09-12 SmoothServo https://wokwi.com/projects/342593181270934099  for  https://forum.arduino.cc/t/mehrere-servos-synchron-ansteuern/1030870/20
*/

#include <Servo.h>

constexpr uint8_t openPin {4};     // GPIO for servo movement
constexpr uint8_t closePin {3};    // GPIO for another servo movement
constexpr uint8_t servoAPin {8};   // GPIO for servo A
constexpr uint8_t servoBPin {9};   // GPIO for servo B

// make your own class of servos
class RampServo
{
  protected:
    const uint8_t startInterval {10};  // slow speed for start
    uint16_t target {90};              // target angle
    uint16_t current {90};             // current angle
    uint8_t interval {startInterval};  // delay time
    uint32_t previousMillis {0};       // last movement
    uint16_t minInterval {5};          // interval after rampUp/before ramp/down (=max Speed)
    uint8_t rampUp{30};                // angle used for ramp up
    uint8_t rampDown{10};              // angle used for ramp down
    uint16_t angleStart{90};           // what was the start angle

  public:
    Servo servo;

    void begin(const byte pin)
    {
      servo.attach(pin);
      servo.write(target);   // bring the servo to a defined angle
    }

    void set(uint16_t target, uint16_t minInterval = 1)
    {
      this->target = target;
      this->minInterval = minInterval;
      angleStart = current;
      interval = startInterval;
    }

    //      ____
    // ____/    \____
    //     u    d
    void update(uint32_t currentMillis = millis())
    {
      if (currentMillis - previousMillis > interval && current != target)  // slow down the servos
      {
        previousMillis = currentMillis;
        int16_t total = angleStart - target;   // total way
        int16_t remaining = current - target;  // rest way
        total = abs(total);
        remaining = abs(remaining);
        if (remaining < rampDown)
        {
          //Serial.println(F("ramp Down"));
          if (interval < startInterval) interval++;  //rampDown --> geschwidigkeit vermindern / interval erhöhen
        }
        else if (remaining > rampUp)
        {
          //Serial.println(F("ramp Up"));
          if (interval > minInterval) interval--;  //rampUp --> geschwindigkeit erhöhen/interval reduzieren
        }
        //Serial.print(F(" interval=")); Serial.println(interval);
        if (target < current)
        {
          current--;
        }
        else if (target > current)
        {
          current++;
        }
        servo.write(current);
      }
    }
};

RampServo servoA;  // create a servo object
RampServo servoB;

void setup()
{
  Serial.begin(115200);
  servoA.begin(servoAPin);          // start the servo object
  servoB.begin(servoBPin);
  pinMode(openPin, INPUT_PULLUP);
  pinMode(closePin, INPUT_PULLUP);
}

void doorOpen()
{
  Serial.println(F("move open"));
  servoA.set(0);     // new angle
  servoB.set(0, 2);  // new angle, newInterval --> this servo is slower
  //servoA.servo.write(90); // hardcoded write
}

void doorClose()
{
  Serial.println(F("move close"));
  servoA.set(180);
  servoB.set(180, 5);
}

void loop()
{
  // read buttons, sensors...
  if (digitalRead(openPin) == LOW) doorOpen();
  if (digitalRead(closePin) == LOW) doorClose();

  // call all servos
  uint32_t currentMillis = millis();
  servoA.update(currentMillis);  // call the update method in loop
  servoB.update(currentMillis);
}