Merge branch 'Improved-servo-support' into 'main'

Improved servo support

See merge request passer/cpp/roboidcontrol!3

AbsoluteEncoder.cpp

@@ -0,0 +1,2 @@
+#include "AbsoluteEncoder.h"
+

AbsoluteEncoder.h

@@ -5,11 +5,12 @@
 namespace Passer {
 namespace RoboidContol {
 
-class FeedbackServo : public ServoMotor {
+class AbsoluteEncoder {
 public:
-  FeedbackServo();
+  AbsoluteEncoder() {}
 
   virtual float GetActualAngle() = 0;
+  virtual float GetActualVelocity() = 0;
 };
 
 } // namespace RoboidContol

Encoder.cpp

@@ -1,18 +1,21 @@
+/*
 #include "Encoder.h"
 
-Encoder::Encoder(unsigned char pulsesPerRevolution,
-                 unsigned char distancePerRotation) {
-  //: Encoder::Encoder() {
+IncrementalEncoder::IncrementalEncoder(unsigned char pulsesPerRevolution,
+                                       unsigned char distancePerRotation) {
   this->pulsesPerRevolution = pulsesPerRevolution;
   this->distancePerRevolution = distancePerRotation;
 }
 
-int Encoder::GetPulseCount() { return 0; }
+int IncrementalEncoder::GetPulseCount() { return 0; }
 
-float Encoder::GetDistance() { return 0; }
+float IncrementalEncoder::GetDistance() { return 0; }
 
-float Encoder::GetPulsesPerSecond(float currentTimeMs) { return 0; }
+float IncrementalEncoder::GetPulsesPerSecond(float currentTimeMs) { return 0; }
 
-float Encoder::GetRevolutionsPerSecond(float currentTimeMs) { return 0; }
+float IncrementalEncoder::GetRevolutionsPerSecond(float currentTimeMs) {
+  return 0;
+}
 
-float Encoder::GetSpeed(float currentTimeMs) { return 0; }
+float IncrementalEncoder::GetSpeed(float currentTimeMs) { return 0; }
+*/

Encoder.h

@@ -1,19 +1,25 @@
 #pragma once
 
+#include "IncrementalEncoder.h"
+
 namespace Passer {
 namespace RoboidControl {
 
-/// @brief An Encoder measures the rotations of an axle using a rotary sensor
-/// Some encoders are able to detect direction, while others can not.
-class Encoder {
+// Deprecated, use explicit IncrementalEncoder in the future
+using Encoder = IncrementalEncoder;
+
+/*
+/// @brief An Encoder measures the rotations of an axle using a rotary
+/// sensor Some encoders are able to detect direction, while others can not.
+class IncrementalEncoder {
 public:
   /// @brief Creates a sensor which measures distance from pulses
   /// @param pulsesPerRevolution The number of pulse edges which make a
   /// full rotation
   /// @param distancePerRevolution The distance a wheel travels per full
   /// rotation
-  Encoder(unsigned char pulsesPerRevolution = 1,
-          unsigned char distancePerRevolution = 1);
+  IncrementalEncoder(unsigned char pulsesPerRevolution = 1,
+                     unsigned char distancePerRevolution = 1);
 
   /// @brief Get the total number of pulses since the previous call
   /// @return The number of pulses, is zero or greater
@@ -45,7 +51,7 @@ public:
   /// meter
   unsigned char distancePerRevolution = 1;
 };
-
+*/
 } // namespace RoboidControl
 } // namespace Passer
-using namespace Passer::RoboidControl;
+using namespace Passer::RoboidControl;

FeedbackServo.cpp

@@ -1,3 +0,0 @@
-#include "FeedbackServo.h"
-
-FeedbackServo::FeedbackServo() {}

IncrementalEncoder.cpp

@@ -0,0 +1,19 @@
+#include "Encoder.h"
+
+IncrementalEncoder::IncrementalEncoder(unsigned char pulsesPerRevolution,
+                                       unsigned char distancePerRotation) {
+  this->pulsesPerRevolution = pulsesPerRevolution;
+  this->distancePerRevolution = distancePerRotation;
+}
+
+int IncrementalEncoder::GetPulseCount() { return 0; }
+
+float IncrementalEncoder::GetDistance() { return 0; }
+
+float IncrementalEncoder::GetPulsesPerSecond(float currentTimeMs) { return 0; }
+
+float IncrementalEncoder::GetRevolutionsPerSecond(float currentTimeMs) {
+  return 0;
+}
+
+float IncrementalEncoder::GetSpeed(float currentTimeMs) { return 0; }

IncrementalEncoder.h

@@ -0,0 +1,51 @@
+#pragma once
+
+namespace Passer {
+namespace RoboidControl {
+
+/// @brief An Encoder measures the rotations of an axle using a rotary
+/// sensor Some encoders are able to detect direction, while others can not.
+class IncrementalEncoder {
+public:
+  /// @brief Creates a sensor which measures distance from pulses
+  /// @param pulsesPerRevolution The number of pulse edges which make a
+  /// full rotation
+  /// @param distancePerRevolution The distance a wheel travels per full
+  /// rotation
+  IncrementalEncoder(unsigned char pulsesPerRevolution = 1,
+                     unsigned char distancePerRevolution = 1);
+
+  /// @brief Get the total number of pulses since the previous call
+  /// @return The number of pulses, is zero or greater
+  virtual int GetPulseCount();
+  /// @brief Get the pulse speed since the previous call
+  /// @param currentTimeMs The clock time in milliseconds
+  /// @return The average pulses per second in the last period.
+  virtual float GetPulsesPerSecond(float currentTimeMs);
+
+  /// @brief Get the distance traveled since the previous call
+  /// @return The distance in meters.
+  virtual float GetDistance();
+
+  /// @brief Get the rotation speed since the previous call
+  /// @param currentTimeMs The clock time in milliseconds
+  /// @return The speed in rotations per second
+  virtual float GetRevolutionsPerSecond(float currentTimeMs);
+
+  /// @brief Get the speed since the previous call
+  /// @param currentTimeMs The clock time in milliseconds
+  /// @return The speed in meters per second.
+  /// @note this value is dependent on the accurate setting of the
+  /// pulsesPerRevolution and distancePerRevolution parameters;
+  virtual float GetSpeed(float currentTimeMs);
+
+  /// @brief The numer of pulses corresponding to a full rotation of the axle
+  unsigned char pulsesPerRevolution = 1;
+  /// @brief The number of revolutions which makes the wheel move forward 1
+  /// meter
+  unsigned char distancePerRevolution = 1;
+};
+
+} // namespace RoboidControl
+} // namespace Passer
+using namespace Passer::RoboidControl;

ServoMotor.cpp

@@ -1,3 +1,86 @@
 #include "ServoMotor.h"
 
-ServoMotor::ServoMotor() {}
+#include "VectorAlgebra/FloatSingle.h"
+
+ServoMotor::ServoMotor() {
+  this->controlMode = ControlMode::Position;
+  this->targetAngle = 0;
+  this->hasTargetAngle = false;
+}
+
+void ServoMotor::SetTargetAngle(float angle) {
+  angle = Float::Clamp(angle, minAngle, maxAngle);
+
+  if (maxVelocity == 0.0F || hasTargetAngle == false) {
+    SetAngle(angle);
+    this->limitedTargetAngle = angle;
+  } else if (angle != 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 = angle;
+  this->hasTargetAngle = true;
+}
+
+float ServoMotor::GetTargetAngle() { return this->targetAngle; }
+
+void ServoMotor::SetMaximumVelocity(float maxVelocity) {
+  this->maxVelocity = maxVelocity;
+}
+
+void ServoMotor::SetTargetVelocity(float targetVelocity) {
+  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;
+      float absDeltaAngle = (deltaAngle < 0) ? -deltaAngle : deltaAngle;
+
+      if (absDeltaAngle < angleStep) {
+        this->limitedTargetAngle = this->targetAngle;
+        SetAngle(targetAngle);
+      } else {
+        if (deltaAngle < 0)
+          limitedTargetAngle -= angleStep;
+        else
+          limitedTargetAngle += angleStep;
+      }
+      SetAngle(limitedTargetAngle);
+
+      this->lastUpdateTimeMs = currentTimeMs;
+      return;
+    }
+
+  } else { // Velocity Control
+    float newAngle = this->targetAngle + targetVelocity * deltaTime;
+    newAngle = Float::Clamp(newAngle, minAngle, maxAngle);
+
+    ServoMotor::SetTargetAngle(newAngle);
+    SetAngle(newAngle);
+
+    this->lastUpdateTimeMs = currentTimeMs;
+  }
+}

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