/*
  Auto correct servo between 15RPM and 30RPM for Walter

  https://wokwi.com/projects/396018983767404545 
  https://forum.arduino.cc/t/control-velocidad-de-servo-5-5kg-giro-360-entre-15-y-30-rpm/1251242

  Make the servo arm hit a limit switch (button).
  Use a "button debounce" sketch to read the switch.
  Count the "button" presses.
  Record button press 1.
  Record button press 2.
  The difference is the TIME per REVOLUTION.

  Continuous servo have different and imperfect values.
  These values are approximate:

  myservo.write(0);   // full-speed counter-clockwise
  myservo.write(45);  // half-speed counter-clockwise
  myservo.write(90);  // stop
  myservo.write(135); // half-speed clockwise
  myservo.write(180); // full-speed clockwise

  SERVO RPM to MS/REV
  15RPM = 0.00025 = 4000ms/rev (msPerRev)
  30RPM = 0.00050 = 2000ms/rev (msPerRev)

  ADJUSTING SERVO SPEED
  if (msPerRev > 4000) ( < 15RPM) increase servo speed
  if (msPerRev < 2000) ( > 30RPM) decrease servo speed
*/

#include "Servo.h"  // add servo library
Servo myservo;      // Create a servo object

byte servoPin = 3;   // servo signal pin
byte buttonPin = 2;  // limit switch

bool lastButtonReading,  // old button reading LOW/pressed or HIGH/not pressed
     newButtonReading,      // new button reading
     currentButtonState,    // new button state
     count;                 // where to store next time reading

int speed = 90;  // start at "stop" speed

unsigned long timer,  // when the button was pressed
         timeout = 50,       // debounce time for a ringing button
         time0,              // first rotation
         time1,              // second and subsequent rotations
         msPerRev;           // difference between first and second rotation is ms-per-revolution

void setup() {
  Serial.begin(115200);              // start Serial Monitor debugging
  myservo.attach(servoPin);          // attach servo to servo object
  myservo.write(90);                 // stop the motor
  pinMode(buttonPin, INPUT_PULLUP);  // microswitch
}

void loop() {
  buttonRead();  // read the limit switch
}

void buttonRead() {
  newButtonReading = digitalRead(buttonPin);    // read button pin
  if (newButtonReading != lastButtonReading) {  // if button pin reading changes...
    timer = millis();                           // ...start a timer
    lastButtonReading = newButtonReading;       // ... and store current state
  }

  if ((millis() - timer) > timeout) {  // if button state change was longer than timeout
    if (currentButtonState == HIGH &&  // ... while STATE is NOT pressed
        newButtonReading == LOW) {     // ... and button IS pressed

      if (!time0 && !count) {  // if time0 is empty...
        time0 = millis();      // ...store the time
        Serial.print(" time0:");
        Serial.print(time0);
        Serial.println(" 'time1' will be the new 'time0'");
      }

      if (!time1 && count) {  // if time1 is empty...
        time1 = millis();     // ...store the time
        Serial.print(" time1:");
        Serial.print(time1);
      }

      if (time0 && time1) {        // two time stamps have readings
        msPerRev = time1 - time0;  // difference in times is the RPM
        Serial.print(" msPerRev:");
        Serial.print(msPerRev);
        servoWrite();  // change servo speed

        // After the first two rotations, make every rotation calibrate the RPM
        time0 = time1;  // move second timer to first timer
        time1 = 0;      // clear second timer
      }
      
      count = 1;  // force storing millis() into time1 only
    }
    currentButtonState = newButtonReading;  // change the button state
  }
}

void servoWrite() {
  if (msPerRev > 4000) {  // < 15RPM
    speed++;              // increase RP<
    if (speed > 180)      // maximum clockwise speed is 90 + 90 = 180
      speed = 180;
  }
  if (msPerRev < 2000) {  // > 30RPM
    speed--;              // decrease RP<
    if (speed < 91)       // minimum clockwise speed 90 + 1 = 91
      speed = 91;
  }
  myservo.write(speed);  // change clockwise servo rpm
  Serial.print(" speed:");
  Serial.println(speed);
}