Encoder can evaluate velocity

ControlledMotor.cpp

@@ -6,26 +6,41 @@ ControlledMotor::ControlledMotor(Motor *motor, Encoder *encoder)
     : ControlledMotor() {
   this->motor = motor;
   this->encoder = encoder;
+  // this->rotationDirection = Direction::Forward;
 }
 
-void ControlledMotor::SetTargetSpeed(float velocity) {
-  this->targetVelocity = velocity;
-  this->rotationDirection =
-      (targetVelocity < 0) ? Direction::Reverse : Direction::Forward;
+#include <Arduino.h>
+
+void ControlledMotor::SetTargetSpeed(float speed) {
+  this->targetSpeed = speed;
+  // this->rotationDirection =
+  //     (targetSpeed < 0) ? Direction::Reverse : Direction::Forward;
+  // this->direction = (targetSpeed < 0) ? Motor::Direction::CounterClockwise
+  //                                     : Motor::Direction::Clockwise;
 }
 
 void ControlledMotor::Update(float currentTimeMs) {
-  actualVelocity =
-      (int)rotationDirection * encoder->GetRevolutionsPerSecond(currentTimeMs);
-  float error = targetVelocity - velocity;
+  actualSpeed = encoder->GetRevolutionsPerSecond(currentTimeMs);
+  if (this->targetSpeed < 0)
+    actualSpeed = -actualSpeed;
+
+  // Serial.print("direction ");
+  // Serial.print(this->direction);
+  // Serial.print("  actualSpeed ");
+  // Serial.println(actualSpeed);
+  // Serial.print("  target ");
+  // Serial.println(this->targetSpeed);
+
+  float error = targetSpeed - 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
+  motor->SetTargetSpeed(
+      this->targetSpeed); // + acceleration); // or something like that
   this->lastUpdateTime = currentTimeMs;
 }
 
-float ControlledMotor::GetActualSpeed() { return actualVelocity; }
+float ControlledMotor::GetActualSpeed() { return actualSpeed; }
 
 bool ControlledMotor::Drive(float distance) {
   if (!driving) {

ControlledMotor.h

@@ -9,7 +9,7 @@ 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 {
+class ControlledMotor : public Motor {
 public:
   ControlledMotor();
   ControlledMotor(Motor *motor, Encoder *encoder);
@@ -23,15 +23,15 @@ public:
   float pidD = 0;
   float pidI = 0;
 
-  void Update(float currentTimeMs);
+  void Update(float currentTimeMs) override;
 
   /// @brief Set the target speed for the motor controller
   /// @param speed the target in revolutions per second.
-  virtual void SetTargetSpeed(float speed);
+  virtual void SetTargetSpeed(float speed) override;
 
   /// @brief Get the actual speed from the encoder
   /// @return The speed in revolutions per second
-  virtual float GetActualSpeed();
+  virtual float GetActualSpeed() override;
 
   bool Drive(float distance);
 
@@ -40,14 +40,14 @@ public:
 
 protected:
   float lastUpdateTime;
-  float targetVelocity;
-  float actualVelocity;
+  float targetSpeed;
+  float actualSpeed;
   float netDistance = 0;
   float startDistance = 0;
 
-  enum Direction { Forward = 1, Reverse = -1 };
+  // enum Direction { Forward = 1, Reverse = -1 };
 
-  Direction rotationDirection;
+  // Direction rotationDirection;
 
   bool driving = false;
   float targetDistance = 0;

DifferentialDrive.cpp

@@ -12,7 +12,8 @@ DifferentialDrive::DifferentialDrive(Motor *leftMotor, Motor *rightMotor,
   this->motors[0] = leftMotor;
   this->motors[1] = rightMotor;
 
-  float distance = separation / 2;
+  this->wheelSeparation = separation;
+  float distance = wheelSeparation / 2;
   leftMotor->direction = Motor::Direction::CounterClockwise;
   leftMotor->position.angle = -90;
   leftMotor->position.distance = distance;
@@ -21,19 +22,19 @@ DifferentialDrive::DifferentialDrive(Motor *leftMotor, Motor *rightMotor,
   rightMotor->position.distance = distance;
 }
 
+#include <Arduino.h>
+
 void DifferentialDrive::SetTargetSpeeds(float leftSpeed, float rightSpeed) {
   for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
     Motor *motor = motors[motorIx];
     if (motor == nullptr)
       continue;
 
-    if (motor->type == Thing::UncontrolledMotorType) {
-      float xPosition = motors[motorIx]->position.angle;
-      if (xPosition < 0)
-        motor->SetSpeed(leftSpeed);
-      else if (xPosition > 0)
-        motor->SetSpeed(rightSpeed);
-    }
+    float xPosition = motors[motorIx]->position.angle;
+    if (xPosition < 0)
+      motor->SetTargetSpeed(leftSpeed);
+    else if (xPosition > 0)
+      motor->SetTargetSpeed(rightSpeed);
   };
 }
 
@@ -52,23 +53,46 @@ void DifferentialDrive::SetTwistSpeed(Vector3 linear, float yaw, float pitch,
   SetTwistSpeed(linear.z, yaw);
 }
 
+#include <Arduino.h>
+
 Polar DifferentialDrive::GetVelocity() {
   Motor *leftMotor = motors[0];
   Motor *rightMotor = motors[1];
-  float leftSpeed = leftMotor->GetSpeed();
-  float rightSpeed = rightMotor->GetSpeed();
+  float leftSpeed = leftMotor->GetActualSpeed();   // in revolutions per second
+  float rightSpeed = rightMotor->GetActualSpeed(); // in revolutions per second
+  leftSpeed = leftSpeed * wheelDiameter * M_PI;    // in meters per second
+  rightSpeed = rightSpeed * wheelDiameter * M_PI;  // in meters per second
   float speed = (leftSpeed + rightSpeed) / 2;
+
   float direction = speed >= 0 ? 0.0F : 180.0F;
-  float distance = fabsf(speed);
-  Polar velocity = Polar(direction, distance);
+  float magnitude = fabsf(speed);
+  Polar velocity = Polar(direction, magnitude);
   return velocity;
 }
 
 float DifferentialDrive::GetAngularVelocity() {
   Motor *leftMotor = motors[0];
   Motor *rightMotor = motors[1];
-  float leftSpeed = leftMotor->GetSpeed();
-  float rightSpeed = rightMotor->GetSpeed();
-  float angularVelocity = leftSpeed - rightSpeed;
+  float leftSpeed = leftMotor->GetActualSpeed();   // in revolutions per second
+  float rightSpeed = rightMotor->GetActualSpeed(); // in revolutions per second
+  leftSpeed = leftSpeed * wheelDiameter * M_PI;    // in meters per second
+  rightSpeed = rightSpeed * wheelDiameter * M_PI;  // in meters per second
+
+  float angularSpeed = (rightSpeed - leftSpeed) / 2;
+  float angularDistance = wheelSeparation / 2 * M_PI;
+  float rotationsPerSecond = angularSpeed / angularDistance;
+  float degreesPerSecond = 360 * rotationsPerSecond;
+  float angularVelocity = degreesPerSecond;
+
+  // Serial.print(leftSpeed);
+  // Serial.print("  -  ");
+  // Serial.println(rightSpeed);
+
+  // Serial.print(angularSpeed);
+  // Serial.print("  ");
+  // Serial.print(angularDistance);
+  // Serial.print("  ");
+  // Serial.println(rotationsPerSecond);
+
   return angularVelocity;
 }

DifferentialDrive.h

@@ -73,6 +73,9 @@ public:
   /// @remark This will be more expanded/detailed in a future version of Roboid
   /// Control
   virtual float GetAngularVelocity() override;
+
+  float wheelDiameter = 1.0F;   // in meters
+  float wheelSeparation = 1.0F; // in meters;
 };
 
 } // namespace RoboidControl

Motor.cpp

@@ -2,14 +2,12 @@
 
 #include <time.h>
 
-Motor::Motor() {
-  type = (int)Thing::UncontrolledMotorType;
-}
+Motor::Motor() { type = (int)Thing::UncontrolledMotorType; }
 
-float Motor::GetSpeed() {
-  return this->currentTargetSpeed;
-}
+float Motor::GetActualSpeed() { return this->currentTargetSpeed; }
 
-void Motor::SetSpeed(float targetSpeed) {
+void Motor::SetTargetSpeed(float targetSpeed) {
   this->currentTargetSpeed = targetSpeed;
 }
+
+void Motor::Update(float currentTimeMs) {}

Motor.h

@@ -22,11 +22,13 @@ public:
   /// @brief Set the target motor speed
   /// @param speed The speed between -1 (full backward), 0 (stop) and 1 (full
   /// forward)
-  virtual void SetSpeed(float speed);
+  virtual void SetTargetSpeed(float speed);
   /// @brief Get the current target speed of the motor
   /// @return The speed between -1 (full backward), 0 (stop) and 1 (full
   /// forward)
-  virtual float GetSpeed();
+  virtual float GetActualSpeed();
+
+  virtual void Update(float currentTimeMs);
 
 protected:
   float currentTargetSpeed = 0;

Propulsion.cpp

@@ -4,7 +4,7 @@
 #include "VectorAlgebra/FloatSingle.h"
 
 Propulsion::Propulsion() {
-  this->motors == nullptr;
+  this->motors = nullptr;
   this->motorCount = 0;
 }
 
@@ -18,7 +18,15 @@ Motor *Propulsion::GetMotor(unsigned int motorId) {
   return motor;
 }
 
-void Propulsion::Update(float currentTimeMs) {}
+void Propulsion::Update(float currentTimeMs) {
+  for (unsigned char motorIx = 0; motorIx < this->motorCount; motorIx++) {
+    Motor *motor = this->motors[motorIx];
+    if (motor == nullptr)
+      continue;
+
+    motor->Update(currentTimeMs);
+  }
+}
 
 void Propulsion::SetTwistSpeed(float forward, float yaw) {}
 

Roboid.cpp

@@ -16,6 +16,8 @@ Roboid::Roboid(ServoMotor **actuation) : actuation(actuation) {}
 void Roboid::Update(float currentTimeMs) {
   if (perception != nullptr)
     perception->Update(currentTimeMs);
+  if (propulsion != nullptr)
+    propulsion->Update(currentTimeMs);
   if (networkSync != nullptr)
     networkSync->NetworkUpdate(this);
 }