From ae629745623bdb1ecbaa7839eb6177cbcca417ec Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <pascal@passervr.com>
Date: Mon, 29 Jan 2024 10:47:00 +0100
Subject: [PATCH] Generic velocity control for servos

---
 ServoMotor.cpp | 76 +++++++++++++++++++++++++++++++++++++++++++++++++-
 ServoMotor.h   | 29 ++++++++++++++++++-
 2 files changed, 103 insertions(+), 2 deletions(-)

diff --git a/ServoMotor.cpp b/ServoMotor.cpp
index c0a7781..7978534 100644
--- a/ServoMotor.cpp
+++ b/ServoMotor.cpp
@@ -1,3 +1,77 @@
 #include "ServoMotor.h"
 
-ServoMotor::ServoMotor() {}
\ No newline at end of file
+#include "VectorAlgebra/FloatSingle.h"
+
+ServoMotor::ServoMotor() { this->controlMode = ControlMode::Position; }
+
+void ServoMotor::SetTargetAngle(float targetAngle) {
+  targetAngle = Float::Clamp(targetAngle, minAngle, maxAngle);
+
+  if (maxVelocity == 0.0F) {
+    SetAngle(targetAngle);
+  } else if (targetAngle != this->targetAngle) {
+    // if the new target is the same, the limited angle is not overwritten
+    this->limitedTargetAngle = this->targetAngle;
+  }
+
+  this->controlMode = ControlMode::Position;
+  this->targetAngle = targetAngle;
+}
+
+float ServoMotor::GetTargetAngle() { return this->targetAngle; }
+
+void ServoMotor::SetMaximumVelocity(float maxVelocity) {
+  this->maxVelocity = maxVelocity;
+}
+
+void ServoMotor::SetTargetVelocity(float velocity) {
+  targetVelocity =
+      Float::Clamp(targetVelocity, -this->maxVelocity, maxVelocity);
+
+  this->controlMode = ControlMode::Velocity;
+  this->targetVelocity = targetVelocity;
+  this->hasTargetAngle = false; // can't we use the controlMode for this?
+}
+
+float ServoMotor::GetTargetVelocity() { return this->targetVelocity; }
+
+void ServoMotor::Update(float currentTimeMs) {
+  if (this->lastUpdateTimeMs == 0 || currentTimeMs < this->lastUpdateTimeMs) {
+    this->lastUpdateTimeMs = currentTimeMs;
+    return;
+  }
+
+  float deltaTime = (currentTimeMs - this->lastUpdateTimeMs) / 1000.0F;
+
+  if (controlMode == ControlMode::Position) {
+    if (maxVelocity == 0.0F || hasTargetAngle == false) {
+      this->lastUpdateTimeMs = currentTimeMs;
+      return;
+
+    } else {
+      float angleStep = maxVelocity * deltaTime;
+      float deltaAngle = this->targetAngle - this->limitedTargetAngle;
+      if (deltaAngle < 0)
+        deltaAngle = -deltaAngle;
+
+      if (deltaAngle < angleStep) {
+        this->limitedTargetAngle = this->targetAngle;
+        SetAngle(targetAngle);
+      } else {
+        limitedTargetAngle += angleStep;
+      }
+      SetAngle(limitedTargetAngle);
+
+      this->lastUpdateTimeMs = currentTimeMs;
+      return;
+    }
+  }
+
+  float newAngle = this->targetAngle + targetVelocity * deltaTime;
+  newAngle = Float::Clamp(newAngle, minAngle, maxAngle);
+
+  ServoMotor::SetTargetAngle(newAngle);
+  SetAngle(newAngle);
+
+  this->lastUpdateTimeMs = currentTimeMs;
+}
\ No newline at end of file
diff --git a/ServoMotor.h b/ServoMotor.h
index 748d502..a32583e 100644
--- a/ServoMotor.h
+++ b/ServoMotor.h
@@ -9,7 +9,34 @@ class ServoMotor : public Thing {
 public:
   ServoMotor();
 
-  virtual void SetTargetAngle(float angle) = 0;
+  float minAngle = -90.0F;
+  float maxAngle = 90.0F;
+
+  enum ControlMode { Position, Velocity };
+  ControlMode controlMode = ControlMode::Position;
+
+  virtual void SetTargetAngle(float angle);
+  virtual float GetTargetAngle();
+
+  virtual void SetMaximumVelocity(float maxVelocity);
+
+  virtual void SetTargetVelocity(float velocity);
+  virtual float GetTargetVelocity();
+
+  virtual void Update(float currentTimeMs);
+
+protected:
+  bool hasTargetAngle = false;
+  float targetAngle = 0.0F;
+
+  float maxVelocity = 0.0F;
+
+  float targetVelocity = 0.0F;
+  float limitedTargetAngle = 0.0F;
+
+  float lastUpdateTimeMs = 0.0F;
+
+  virtual void SetAngle(float angle) = 0;
 };
 
 } // namespace RoboidContol