Closed loop RC Servo Example

// Closed Loop position exercise with modified RC Servo
// This sketch is NOT for an industrial closed loop stepper motor (servo)
// The RC servo needs to have a wire connected to the Wiper of the position POT for feedback
// Three Position States and Stop or Moving State

#include      <VarSpeedServo.h>
#include      <OneButton.h>
const uint8_t closedLed = 13, midLed = 12, openLed = 11,
              movingLed = 10, servoPin = 3;
OneButton     button(9, true);
VarSpeedServo myservo;  // create servo object to control a servo
char          inChar;
String        readString;
uint16_t      closedState=0, midState=HIGH, openState=0, movingState=0,
              pos=90, lastPos=90, val;
bool          mid = true, opened = false, closed = false, moving = false,
              updateState = true, inputs = false;
long          previousTime = 0;
void setup() {
  Serial.begin(115200);
  pinMode(closedLed, OUTPUT);
  pinMode(midLed, OUTPUT);
  pinMode(openLed, OUTPUT);
  pinMode(movingLed, OUTPUT);
  digitalWrite(closedLed, closedState);
  digitalWrite(midLed, midState);
  digitalWrite(openLed, openState);
  digitalWrite(movingLed, movingState);
  myservo.attach(servoPin);
  myservo.write(pos, 10);
  button.attachDoubleClick(doubleclick);
  button.attachClick(singleclick);
  button.attachLongPressStop(longclick);
  Serial.println("Startup Position = MIDWAY");
  delay(100);
}
void doubleclick() {
  if(pos != 90){
    pos = 90;
    moving = true;
  }else{
    Serial.print("Already at MIDWAY position: ");
  }
}
void singleclick(){
  if(pos != 180){
    pos = 180;
    moving = true;
  }else{
    Serial.print("Already at OPENED position: ");
  }
}
void longclick(){
  if(pos != 0){
    pos = 0;
    moving = true;
  }else{
    Serial.print("Already at CLOSED position: ");
  }
}

void loop() {
  if(inputs){
    button.tick();
    while (Serial.available()) {
      inChar = Serial.read();  //gets one byte from serial buffer
      readString += inChar; //makes the String readString
      delay(2);  //slow looping to allow buffer to fill with next character
    }
    if (readString.length() > 0) {
        //Serial.print("Go to: ");
        //Serial.println(readString);  //so you can see the captured String
        pos = readString.toInt();  //convert readString into a number
        readString="";
            if(pos < 0 || pos > 180){
                Serial.println("Invalid Value, Please try again!");
            }else if(lastPos == pos){
                Serial.print("Already at the position: ");Serial.println(pos);
            }else{
                moving = true;
            }
    }
  }

  if(millis() - previousTime > 50){
    previousTime = millis();
    val = analogRead(A0);
    int closedVal = constrain(val, 0, 100);
    int midVal = constrain(val, 250, 270);
    int openedVal = constrain(val, 540, 700);

    if(moving){
        if(pos == 0){
            Serial.println("Go to CLOSED position");
            closed = true;
        }else if(pos == 90){
            Serial.println("Go to MIDWAY position");
            mid = true;
        }else if(pos == 180){
            Serial.println("Go to OPENED position");
            opened = true;
        }
        movingState = HIGH;
        closedState = LOW;
        midState = LOW;
        openState = LOW;
        updateState = true;
        moving = false;
    }
    if(closed){
        if(val == closedVal){
            movingState = LOW;
            closedState = HIGH;
            midState = LOW;
            openState = LOW;
            updateState = true;
            Serial.println("Position now CLOSED\n");
            lastPos = pos;
            inputs = true;
            closed = false;
        }
    }
    if(mid){
        if(val == midVal){
            movingState = LOW;
            closedState = LOW;
            midState = HIGH;
            openState = LOW;
            updateState = true;
            Serial.println("Position now MIDWAY\n");
            lastPos = pos;
            inputs = true;
            mid = false;
        }
    }
    if(opened){
        if(val == openedVal){
            movingState = LOW;
            closedState = LOW;
            midState = LOW;
            openState = HIGH;
            updateState = true;
            Serial.println("Position now OPENED\n");
            lastPos = pos;
            inputs = true;
            opened = false;
        }
    }
    if(updateState){
        myservo.write(pos, 20);
        digitalWrite(closedLed, closedState);
        digitalWrite(midLed, midState);
        digitalWrite(openLed, openState);
        digitalWrite(movingLed, movingState);
        updateState = false;
      }
  }
}