lib/ServoMotorComponent/src/ServoMotorComponent.cpp

@@ -1,50 +1,56 @@
 #include "ServoMotorComponent.h"
 
-ServoMotorComponent::ServoMotorComponent(int PIN) {
+ServoMotorComponent::ServoMotorComponent(int PIN,  unsigned long updatePeriod) {
     this->PIN = PIN;
-    this->position = 30;
-    this->speed = 100;
-    this->easing = 0.5;
     this->myservo.attach(PIN);
+    this->desiredposition = Position::MIDDLE;
+    this->currentPosition = 0;
+    this->lastUpTime = millis();
+    this->updatePeriod = updatePeriod;
 }
 
-void ServoMotorComponent::goLeft() {
-    this->myservo.write(this->PIN, 52, this->speed, this->easing);
-    this->position = 0;
+int ServoMotorComponent::getCurrentPosition() {
+    return this->myservo.read(this->PIN);
 }
 
-void ServoMotorComponent::goRight() {
-        this->myservo.write(this->PIN, 18, this->speed, this->easing);
-
-    this->position = 180;
+void ServoMotorComponent::setCurrentPosition() {
+    this->currentPosition = this->myservo.read(this->PIN);
 }
 
-void ServoMotorComponent::goMiddle() {
-    this->myservo.write(this->PIN, 32, this->speed, this->easing);
-    this->position = 90;
+void ServoMotorComponent::setDesiredPosition(Position desiredPosition) {
+    switch (desiredPosition) {
+        case Position::LEFT:
+            this->desiredposition = 52;
+            break;
+        case Position::MIDDLE:
+            this->desiredposition = 32;
+            break;
+        case Position::RIGHT:
+            this->desiredposition = 18;
+            break;
+        default:
+            break;
+    }
 }
 
-int ServoMotorComponent::getPosition() {
-    return this->position;
+
+void ServoMotorComponent::refresh() {
+    if (this->desiredposition == this->currentPosition 
+    ||  millis() - this->lastUpTime <= this->updatePeriod) return;
+
+    if (this->currentPosition > this->desiredposition) {
+        this->currentPosition--;
+    }
+    if (this->currentPosition < this->desiredposition) {
+        this->currentPosition++;
+    }
+    this->lastUpTime = millis();
+    this->myservo.write(this->PIN, this->currentPosition);
 }
 
+
 void ServoMotorComponent::setPin(int PIN) {
     this->PIN = PIN;
     this->myservo.attach(PIN);
     return;
-}
-
-void ServoMotorComponent::setSpeed(int speed) {
-    this->speed = speed;
-    return;
-}
-
-void ServoMotorComponent::pause() {
-    this->myservo.pause();
-    return;
-}
-
-void ServoMotorComponent::setEasing(float easing) {
-    this->easing = easing;
-    return;
-}
+}

lib/ServoMotorComponent/src/ServoMotorComponent.h

@@ -3,26 +3,30 @@
 
 #include <Servo.h>
 
+enum Position {
+    LEFT,
+    MIDDLE,
+    RIGHT
+};
+
 class ServoMotorComponent
 {
 public:
-    ServoMotorComponent(int PIN);
+    ServoMotorComponent(int PIN, unsigned long updatePeriod);
     ~ServoMotorComponent() = default;
-    void goLeft();
-    void goRight();
-    void goMiddle();
-    int getPosition();
+    int getCurrentPosition();
+    void setCurrentPosition();
+    void setDesiredPosition(Position desiredPosition);
     void setPin(int PIN);
-    void pause();
-    void setSpeed(int speed);
-    void setEasing(float easing);
+    void refresh();
 
 private:
     int PIN;
-    int position;
-    int speed;
-    float easing;
+    int currentPosition;
+    int desiredposition;
     Servo myservo;
+    unsigned long lastUpTime;
+    unsigned long updatePeriod;
 };