From 6576a38f9b26661dd1c4bace0c486bf0323e64f9 Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <pascal@passervr.com>
Date: Wed, 17 Apr 2024 15:21:37 +0200
Subject: [PATCH] Add SetVelocity

---
 DifferentialDrive.cpp | 4 ++++
 DifferentialDrive.h   | 4 +++-
 2 files changed, 7 insertions(+), 1 deletion(-)

diff --git a/DifferentialDrive.cpp b/DifferentialDrive.cpp
index cdc2d79..3b1aabe 100644
--- a/DifferentialDrive.cpp
+++ b/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];
diff --git a/DifferentialDrive.h b/DifferentialDrive.h
index a3526b8..0424a5c 100644
--- a/DifferentialDrive.h
+++ b/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