macca-nz

Closed loop RC Servo Example

Nov 22nd, 2021 (edited)
1,399
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. // Closed Loop position exercise with modified RC Servo
  2. // This sketch is NOT for an industrial closed loop stepper motor (servo)
  3. // The RC servo needs to have a wire connected to the Wiper of the position POT for feedback
  4. // Three Position States and Stop or Moving State
  5.  
  6. #include      <VarSpeedServo.h>
  7. #include      <OneButton.h>
  8. const uint8_t closedLed = 13, midLed = 12, openLed = 11,
  9.               movingLed = 10, servoPin = 3;
  10. OneButton     button(9, true);
  11. VarSpeedServo myservo;  // create servo object to control a servo
  12. char          inChar;
  13. String        readString;
  14. uint16_t      closedState=0, midState=HIGH, openState=0, movingState=0,
  15.               pos=90, lastPos=90, val;
  16. bool          mid = true, opened = false, closed = false, moving = false,
  17.               updateState = true, inputs = false;
  18. long          previousTime = 0;
  19. void setup() {
  20.   Serial.begin(115200);
  21.   pinMode(closedLed, OUTPUT);
  22.   pinMode(midLed, OUTPUT);
  23.   pinMode(openLed, OUTPUT);
  24.   pinMode(movingLed, OUTPUT);
  25.   digitalWrite(closedLed, closedState);
  26.   digitalWrite(midLed, midState);
  27.   digitalWrite(openLed, openState);
  28.   digitalWrite(movingLed, movingState);
  29.   myservo.attach(servoPin);
  30.   myservo.write(pos, 10);
  31.   button.attachDoubleClick(doubleclick);
  32.   button.attachClick(singleclick);
  33.   button.attachLongPressStop(longclick);
  34.   Serial.println("Startup Position = MIDWAY");
  35.   delay(100);
  36. }
  37. void doubleclick() {
  38.   if(pos != 90){
  39.     pos = 90;
  40.     moving = true;
  41.   }else{
  42.     Serial.print("Already at MIDWAY position: ");
  43.   }
  44. }
  45. void singleclick(){
  46.   if(pos != 180){
  47.     pos = 180;
  48.     moving = true;
  49.   }else{
  50.     Serial.print("Already at OPENED position: ");
  51.   }
  52. }
  53. void longclick(){
  54.   if(pos != 0){
  55.     pos = 0;
  56.     moving = true;
  57.   }else{
  58.     Serial.print("Already at CLOSED position: ");
  59.   }
  60. }
  61.  
  62. void loop() {
  63.   if(inputs){
  64.     button.tick();
  65.     while (Serial.available()) {
  66.       inChar = Serial.read();  //gets one byte from serial buffer
  67.       readString += inChar; //makes the String readString
  68.       delay(2);  //slow looping to allow buffer to fill with next character
  69.     }
  70.     if (readString.length() > 0) {
  71.         //Serial.print("Go to: ");
  72.         //Serial.println(readString);  //so you can see the captured String
  73.         pos = readString.toInt();  //convert readString into a number
  74.         readString="";
  75.             if(pos < 0 || pos > 180){
  76.                 Serial.println("Invalid Value, Please try again!");
  77.             }else if(lastPos == pos){
  78.                 Serial.print("Already at the position: ");Serial.println(pos);
  79.             }else{
  80.                 moving = true;        
  81.             }
  82.     }
  83.   }
  84.  
  85.   if(millis() - previousTime > 50){
  86.     previousTime = millis();
  87.     val = analogRead(A0);
  88.     int closedVal = constrain(val, 0, 100);
  89.     int midVal = constrain(val, 250, 270);
  90.     int openedVal = constrain(val, 540, 700);
  91.  
  92.     if(moving){
  93.         if(pos == 0){
  94.             Serial.println("Go to CLOSED position");
  95.             closed = true;      
  96.         }else if(pos == 90){
  97.             Serial.println("Go to MIDWAY position");
  98.             mid = true;
  99.         }else if(pos == 180){
  100.             Serial.println("Go to OPENED position");
  101.             opened = true;
  102.         }
  103.         movingState = HIGH;
  104.         closedState = LOW;
  105.         midState = LOW;
  106.         openState = LOW;
  107.         updateState = true;
  108.         moving = false;
  109.     }
  110.     if(closed){  
  111.         if(val == closedVal){
  112.             movingState = LOW;
  113.             closedState = HIGH;
  114.             midState = LOW;
  115.             openState = LOW;
  116.             updateState = true;
  117.             Serial.println("Position now CLOSED\n");
  118.             lastPos = pos;
  119.             inputs = true;
  120.             closed = false;
  121.         }
  122.     }
  123.     if(mid){
  124.         if(val == midVal){
  125.             movingState = LOW;
  126.             closedState = LOW;
  127.             midState = HIGH;
  128.             openState = LOW;
  129.             updateState = true;
  130.             Serial.println("Position now MIDWAY\n");
  131.             lastPos = pos;
  132.             inputs = true;
  133.             mid = false;
  134.         }
  135.     }
  136.     if(opened){
  137.         if(val == openedVal){              
  138.             movingState = LOW;
  139.             closedState = LOW;
  140.             midState = LOW;
  141.             openState = HIGH;
  142.             updateState = true;
  143.             Serial.println("Position now OPENED\n");
  144.             lastPos = pos;
  145.             inputs = true;
  146.             opened = false;
  147.         }
  148.     }
  149.     if(updateState){
  150.         myservo.write(pos, 20);
  151.         digitalWrite(closedLed, closedState);
  152.         digitalWrite(midLed, midState);
  153.         digitalWrite(openLed, openState);
  154.         digitalWrite(movingLed, movingState);
  155.         updateState = false;
  156.       }
  157.   }
  158. }
  159.  
RAW Paste Data Copied