Refacto servoMotor

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;
-}
+}