Add SetVelocity

DifferentialDrive.cpp

@@ -68,6 +68,10 @@ void DifferentialDrive::SetTwistSpeed(Vector3 linear, float yaw, float pitch,
   SetTwistSpeed(linear.z, yaw);
 }
 
+void DifferentialDrive::SetVelocity(Polar velocity) {
+  SetTwistSpeed(velocity.distance, velocity.angle);
+}
+
 Polar DifferentialDrive::GetVelocity() {
   Motor *leftMotor = motors[0];
   Motor *rightMotor = motors[1];

DifferentialDrive.h

@@ -59,6 +59,8 @@ public:
   virtual void SetTwistSpeed(Vector3 linear, float yaw = 0.0F,
                              float pitch = 0.0F, float roll = 0.0F);
 
+  virtual void SetVelocity(Polar velocity);
+
   /// @brief Calculate the linear velocity of the roboid based on the wheel
   /// velocities
   /// @return The velocity of the roboid in local space
@@ -80,7 +82,7 @@ protected:
   float wheelDiameter = 1.0F;   // in meters
   float wheelSeparation = 1.0F; // in meters;
 
-  float rpsToMs; // convert revolutions per second to meters per second
+  float rpsToMs = 1.0F; // convert revolutions per second to meters per second
 };
 
 } // namespace RoboidControl