Get Polar velocity

DifferentialDrive.cpp

@@ -1,6 +1,6 @@
 #include "DifferentialDrive.h"
-
 #include "FloatSingle.h"
+#include <Arduino.h>
 
 DifferentialDrive::DifferentialDrive(){};
 
@@ -10,17 +10,18 @@ DifferentialDrive::DifferentialDrive(Placement leftMotorPlacement,
   this->placement = new Placement[2];
   this->placement[0] = leftMotorPlacement;
   this->placement[1] = rightMotorPlacement;
-  Motor* leftMotor = (Motor*)leftMotorPlacement.thing;
+  Motor *leftMotor = (Motor *)leftMotorPlacement.thing;
   leftMotor->direction = Motor::Direction::CounterClockwise;
-  Motor* rightMotor = (Motor*)rightMotorPlacement.thing;
+  Motor *rightMotor = (Motor *)rightMotorPlacement.thing;
   rightMotor->direction = Motor::Direction::Clockwise;
 }
 
 void DifferentialDrive::SetTargetSpeeds(float leftSpeed, float rightSpeed) {
+  Serial.printf("motor count %d\n", this->motorCount);
   for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
-    Thing* thing = placement[motorIx].thing;
+    Thing *thing = placement[motorIx].thing;
     if (thing->type == Thing::UncontrolledMotorType) {
-      Motor* motor = (Motor*)thing;
+      Motor *motor = (Motor *)thing;
       if (motor == nullptr)
         continue;
 
@@ -43,9 +44,19 @@ void DifferentialDrive::SetTwistSpeed(Vector2 linear, float yaw) {
   SetTwistSpeed(linear.y, yaw);
 }
 
-void DifferentialDrive::SetTwistSpeed(Vector3 linear,
-                                      float yaw,
-                                      float pitch,
+void DifferentialDrive::SetTwistSpeed(Vector3 linear, float yaw, float pitch,
                                       float roll) {
   SetTwistSpeed(linear.z, yaw);
 }
+
+Polar DifferentialDrive::GetVelocity() {
+  Motor *leftMotor = (Motor *)placement[0].thing;
+  Motor *rightMotor = (Motor *)placement[1].thing;
+  float leftSpeed = leftMotor->GetSpeed();
+  float rightSpeed = rightMotor->GetSpeed();
+  float speed = (leftSpeed + rightSpeed) / 2;
+  float direction = speed >= 0 ? 0 : 180;
+  float distance = abs(speed);
+  Polar velocity = Polar(direction, distance);
+  return velocity;
+}

DifferentialDrive.h

@@ -56,6 +56,7 @@ class DifferentialDrive : public Propulsion {
                              float yaw = 0.0F,
                              float pitch = 0.0F,
                              float roll = 0.0F);
+  virtual Polar GetVelocity() override;
 };
 
 }  // namespace RoboidControl