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) {
@@ -60,3 +59,12 @@ Polar DifferentialDrive::GetVelocity() {
   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

@@ -52,11 +52,10 @@ 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,
+  virtual void SetTwistSpeed(Vector3 linear, float yaw = 0.0F,
-                             float yaw = 0.0F,
+                             float pitch = 0.0F, float roll = 0.0F);
-                             float pitch = 0.0F,
-                             float roll = 0.0F);
   virtual Polar GetVelocity() override;
+  virtual float GetAngularVelocity() override;
 };
 
 } // namespace RoboidControl