Restore controlled motor files

ControlledMotor.cpp

@@ -0,0 +1,39 @@
+#include "ControlledMotor.h"
+
+ControlledMotor::ControlledMotor() { this->type = Thing::ControlledMotorType; }
+
+ControlledMotor::ControlledMotor(Motor *motor, Encoder *encoder)
+    : ControlledMotor() {
+  this->motor = motor;
+  this->encoder = encoder;
+}
+
+void ControlledMotor::SetTargetSpeed(float velocity) {
+  this->targetVelocity = velocity;
+  this->rotationDirection =
+      (targetVelocity < 0) ? Direction::Reverse : Direction::Forward;
+}
+
+void ControlledMotor::Update(float currentTimeMs) {
+  actualVelocity =
+      (int)rotationDirection * encoder->GetRevolutionsPerSecond(currentTimeMs);
+  float error = targetVelocity - velocity;
+
+  float timeStep = currentTimeMs - lastUpdateTime;
+  float acceleration = error * timeStep * pidP; // Just P is used at this moment
+  motor->SetSpeed(targetVelocity + acceleration); // or something like that
+  this->lastUpdateTime = currentTimeMs;
+}
+
+float ControlledMotor::GetActualSpeed() { return actualVelocity; }
+
+bool ControlledMotor::Drive(float distance) {
+  if (!driving) {
+    targetDistance = distance;
+    startDistance = encoder->GetDistance();
+    driving = true;
+  }
+  float totalDistance = encoder->GetDistance() - startDistance;
+  bool completed = totalDistance > targetDistance;
+  return completed;
+}

ControlledMotor.h

@@ -0,0 +1,59 @@
+#pragma once
+
+#include "Encoder.h"
+#include "Motor.h"
+
+namespace Passer {
+namespace RoboidControl {
+
+/// @brief A motor with speed control
+/// It uses a feedback loop from an encoder to regulate the speed
+/// The speed is measured in revolutions per second.
+class ControlledMotor : public Thing {
+public:
+  ControlledMotor();
+  ControlledMotor(Motor *motor, Encoder *encoder);
+
+  inline static bool CheckType(Thing *thing) {
+    return (thing->type & (int)Thing::Type::ControlledMotor) != 0;
+  }
+  float velocity;
+
+  float pidP = 1;
+  float pidD = 0;
+  float pidI = 0;
+
+  void Update(float currentTimeMs);
+
+  /// @brief Set the target speed for the motor controller
+  /// @param speed the target in revolutions per second.
+  void SetTargetSpeed(float speed);
+
+  /// @brief Get the actual speed from the encoder
+  /// @return The speed in revolutions per second
+  float GetActualSpeed();
+
+  bool Drive(float distance);
+
+  Motor *motor;
+  Encoder *encoder;
+
+protected:
+  float lastUpdateTime;
+  float targetVelocity;
+  float actualVelocity;
+  float netDistance = 0;
+  float startDistance = 0;
+
+  enum Direction { Forward = 1, Reverse = -1 };
+
+  Direction rotationDirection;
+
+  bool driving = false;
+  float targetDistance = 0;
+  float lastEncoderPosition = 0;
+};
+
+} // namespace RoboidControl
+} // namespace Passer
+using namespace Passer::RoboidControl;

Encoder.cpp

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

Encoder.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 Encoder {
+public:
+  /// @brief Creates a sensor which measures distance from pulses
+  /// @param transitionsPerRevolution The number of pulse edges which make a
+  /// full rotation
+  /// @param distancePerRevolution The distance a wheel travels per full
+  /// rotation
+  Encoder(unsigned char transitionsPerRevolution = 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
+  /// transitionsPerRevolution and distancePerRevolution parameters;
+  virtual float GetSpeed(float currentTimeMs);
+
+  /// @brief The numer of pulses corresponding to a full rotation of the axle
+  unsigned char transitionsPerRevolution = 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;

Servo.h

@@ -0,0 +1,10 @@
+#pragma once
+
+#include "ControlledMotor.h"
+
+class Servo : public ControlledMotor {
+public:
+  Servo();
+
+  virtual void SetTargetAngle(float angle) = 0;
+};