Angular velocity support

DifferentialDrive.cpp

@@ -17,7 +17,6 @@ DifferentialDrive::DifferentialDrive(Placement leftMotorPlacement,
 }
 
 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;
     if (thing->type == Thing::UncontrolledMotorType) {
@@ -59,4 +58,13 @@ Polar DifferentialDrive::GetVelocity() {
   float distance = abs(speed);
   Polar velocity = Polar(direction, distance);
   return velocity;
+}
+
+float DifferentialDrive::GetAngularVelocity() {
+  Motor *leftMotor = (Motor *)placement[0].thing;
+  Motor *rightMotor = (Motor *)placement[1].thing;
+  float leftSpeed = leftMotor->GetSpeed();
+  float rightSpeed = rightMotor->GetSpeed();
+  float angularVelocity = leftSpeed - rightSpeed;
+  return angularVelocity;
 }

DifferentialDrive.h

@@ -15,7 +15,7 @@ namespace RoboidControl {
 /// * When just one wheel is spinning, the Roboid turnes while moving forward or
 /// backward.
 class DifferentialDrive : public Propulsion {
- public:
+public:
   /// @brief Default constructor
   DifferentialDrive();
   /// @brief Setup of the DifferentialDrive with the Placement of the motors
@@ -52,13 +52,12 @@ class DifferentialDrive : public Propulsion {
   /// @note As a DifferentialDrive cannot move sideward or vertical, this
   /// function has the same effect as using the void SetTwistSpeed(float
   /// forward, float yaw) function.
-  virtual void SetTwistSpeed(Vector3 linear,
-                             float yaw = 0.0F,
-                             float pitch = 0.0F,
-                             float roll = 0.0F);
+  virtual void SetTwistSpeed(Vector3 linear, float yaw = 0.0F,
+                             float pitch = 0.0F, float roll = 0.0F);
   virtual Polar GetVelocity() override;
+  virtual float GetAngularVelocity() override;
 };
 
-}  // namespace RoboidControl
-}  // namespace Passer
+} // namespace RoboidControl
+} // namespace Passer
 using namespace Passer::RoboidControl;