Improved velocity functions

DifferentialDrive.cpp

@@ -47,28 +47,35 @@ void DifferentialDrive::SetMotorTargetSpeeds(float leftSpeed,
   };
 }
 
-void DifferentialDrive::SetTwistSpeed(float forward, float yaw) {
+void DifferentialDrive::SetVelocity(float forwardVelocity) {
-  float leftSpeed =
+  targetVelocity = Vector3::forward * forwardVelocity;
-      Float::Clamp(forward - yaw, -1, 1); // revolutions per second
+  Update();
-  float rightSpeed =
+  // float leftSpeed = Float::Clamp(forwardVelocity - targetYawVelocity, -1,
-      Float::Clamp(forward + yaw, -1, 1); // revolutions per second
+  //                                1); // revolutions per second
+  // float rightSpeed = Float::Clamp(forwardVelocity + targetYawVelocity, -1,
+  //                                 1); // revolutions per second
 
-  float leftMotorSpeed = leftSpeed / rpsToMs;   // meters per second
+  // float leftMotorSpeed = leftSpeed / rpsToMs;   // meters per second
-  float rightMotorSpeed = rightSpeed / rpsToMs; // meters per second
+  // float rightMotorSpeed = rightSpeed / rpsToMs; // meters per second
 
-  SetMotorTargetSpeeds(leftMotorSpeed, rightMotorSpeed);
+  // SetMotorTargetSpeeds(leftMotorSpeed, rightMotorSpeed);
 }
 
-void DifferentialDrive::SetTwistSpeed(Vector2 linear, float yaw) {
+void DifferentialDrive::SetAngularVelocity(float yawVelocity) {
-  SetTwistSpeed(linear.y, yaw);
+  targetYawVelocity = yawVelocity;
+  Update();
+  // float leftSpeed = Float::Clamp(currentVelocity - yawVelocity, -1,
+  //                                1); // revolutions per second
+  // float rightSpeed = Float::Clamp(currentVelocity + yawVelocity, -1,
+  //                                 1); // revolutions per second
+
+  // float leftMotorSpeed = leftSpeed / rpsToMs;   // meters per second
+  // float rightMotorSpeed = rightSpeed / rpsToMs; // meters per second
+
+  // SetMotorTargetSpeeds(leftMotorSpeed, rightMotorSpeed);
 }
 
-void DifferentialDrive::SetTwistSpeed(Vector3 linear, float yaw, float pitch,
+Polar DifferentialDrive::GetActualVelocity() {
-                                      float roll) {
-  SetTwistSpeed(linear.z, yaw);
-}
-
-Polar DifferentialDrive::GetVelocity() {
   Motor *leftMotor = motors[0];
   Motor *rightMotor = motors[1];
   float leftSpeed = leftMotor->GetActualSpeed();   // in revolutions per second
@@ -84,7 +91,7 @@ Polar DifferentialDrive::GetVelocity() {
   return velocity;
 }
 
-float DifferentialDrive::GetAngularVelocity() {
+float DifferentialDrive::GetActualYawVelocity() {
   Motor *leftMotor = motors[0];
   Motor *rightMotor = motors[1];
   float leftSpeed = leftMotor->GetActualSpeed();   // in revolutions per second
@@ -101,3 +108,16 @@ float DifferentialDrive::GetAngularVelocity() {
 
   return angularVelocity;
 }
+
+void DifferentialDrive::Update() {
+  float forwardVelocity = targetVelocity.Forward();
+  float leftSpeed = Float::Clamp(forwardVelocity - targetYawVelocity, -1,
+                                 1); // revolutions per second
+  float rightSpeed = Float::Clamp(forwardVelocity + targetYawVelocity, -1,
+                                  1); // revolutions per second
+
+  float leftMotorSpeed = leftSpeed / rpsToMs;   // meters per second
+  float rightMotorSpeed = rightSpeed / rpsToMs; // meters per second
+
+  SetMotorTargetSpeeds(leftMotorSpeed, rightMotorSpeed);
+}

DifferentialDrive.h

@@ -36,7 +36,7 @@ public:
   /// @param leftSpeed The target speed of the left Motor
   /// @param rightSpeed The target speed of the right Motor
   void SetMotorTargetSpeeds(float leftSpeed, float rightSpeed);
-
+  /*
     /// @brief Controls the motors through forward and rotation speeds
     /// @param forward The target forward speed of the Roboid
     /// @param yaw The target rotation speed of the Roboid
@@ -58,6 +58,15 @@ public:
     /// forward, float yaw) function.
     virtual void SetTwistSpeed(Vector3 linear, float yaw = 0.0F,
                                float pitch = 0.0F, float roll = 0.0F);
+  */
+
+  /// @brief Controls the motors through forward velocity of the roboid
+  /// @param forwardVelocity The target forward speed of the Roboid
+  virtual void SetVelocity(float forwardVelocity);
+
+  /// @brief Controls the motors through rotation velocity of the roboid
+  /// @param yawVelocity The target rotation speed of the Roboid
+  virtual void SetAngularVelocity(float yawVelocity);
 
   /// @brief Calculate the linear velocity of the roboid based on the wheel
   /// velocities
@@ -66,7 +75,7 @@ public:
   /// information
   /// @remark This will be more expanded/detailed in a future version of Roboid
   /// Control
-  virtual Polar GetVelocity() override;
+  virtual Polar GetActualVelocity() override;
   /// @brief Calculate the angular velocity of the roboid based on the wheel
   /// velocities
   /// @return The angular speed of the roboid in local space
@@ -74,7 +83,9 @@ public:
   /// information
   /// @remark This will be more expanded/detailed in a future version of Roboid
   /// Control
-  virtual float GetAngularVelocity() override;
+  virtual float GetActualYawVelocity() override;
+
+  virtual void Update();
 
 protected:
   float wheelDiameter = 1.0F;   // in meters

NetworkSync.cpp

@@ -21,8 +21,7 @@ void NetworkSync::SendVector3(unsigned char* data, int startIndex, Vector3 v) {
   SendSingle100(data, startIndex + 8, v.z);
 }
 
-void NetworkSync::SendSingle100(unsigned char* data,
+void NetworkSync::SendSingle100(unsigned char *data, int startIndex,
-                                int startIndex,
                                 float value) {
   // Sends a float with truncated 2 decimal precision
   Int32 intValue = value * 100;
@@ -72,7 +71,7 @@ void NetworkSync::PublishTrackedObject(SendBuffer sendBuffer,
   UInt16 bufferSize = 3 + 12;
   UInt8 buffer[bufferSize] = {
       PoseMsg,
-      object->id,
+      (UInt8)object->id,
       Pose_Position,
   };
   SendVector3(buffer, 3, worldPosition3);
@@ -81,12 +80,12 @@ void NetworkSync::PublishTrackedObject(SendBuffer sendBuffer,
 }
 
 void NetworkSync::SendPoseMsg(SendBuffer sendBuffer, Roboid *roboid) {
-  Polar velocity = roboid->propulsion->GetVelocity();
+  Polar velocity = roboid->propulsion->GetActualVelocity();
   Vector2 worldVelocity2 =
       Vector2::Rotate(Vector2::forward * velocity.distance, velocity.angle);
   Vector3 worldVelocity3 = Vector3(worldVelocity2.x, 0, worldVelocity2.y);
 
-  float angularVelocity = roboid->propulsion->GetAngularVelocity();
+  float angularVelocity = roboid->propulsion->GetActualYawVelocity();
   Vector3 worldAngularVelocity = Vector3(0, angularVelocity, 0);
 
 #ifdef RC_DEBUG

Propulsion.cpp

@@ -8,9 +8,7 @@ Propulsion::Propulsion() {
   this->motorCount = 0;
 }
 
-unsigned int Propulsion::GetMotorCount() {
+unsigned int Propulsion::GetMotorCount() { return this->motorCount; }
-  return this->motorCount;
-}
 
 Motor *Propulsion::GetMotor(unsigned int motorId) {
   if (motorId >= this->motorCount)
@@ -30,28 +28,29 @@ void Propulsion::Update(float currentTimeMs) {
   }
 }
 
-/*
+void Propulsion::SetTargetVelocity(float forwardVelocity) {
-void Propulsion::SetTwistSpeed(float forward, float yaw) {}
+  this->targetVelocity = Vector3::forward * forwardVelocity;
+}
+void Propulsion::SetTargetVelocity(Vector2 horizontalVelocity) {
+  this->targetVelocity = Vector3::FromHorizontal(horizontalVelocity);
+}
+void Propulsion::SetTargetVelocity(Vector3 velocity) {
+  this->targetVelocity = velocity;
+}
+// void Propulsion::SetTargetVelocity(Polar velocity) {}
+// void Propulsion::SetTargetVelocity(Spherical velocity) {}
 
-void Propulsion::SetTwistSpeed(Vector2 linear, float yaw) {}
+void Propulsion::SetTargetYawVelocity(float yawVelocity) {
+  this->targetYawVelocity = yawVelocity;
+}
+// void Propulsion::SetAngularVelocity(float yaw, float pitch, float roll) {}
 
-void Propulsion::SetTwistSpeed(Vector3 linear, float yaw, float pitch,
+float Propulsion::GetTargetForwardVelocity() {
-                               float roll) {}
+  return this->targetVelocity.Forward();
-*/
-
-void Propulsion::SetVelocity(float velocity) {}
-void Propulsion::SetVelocity(Vector2 velocity) {}
-void Propulsion::SetVelocity(Vector3 verlocity) {}
-void Propulsion::SetVelocity(Polar velocity) {}
-void Propulsion::SetVelocity(Spherical velocity) {}
-
-void Propulsion::SetAngularVelocity(float yaw) {}
-void Propulsion::SetAngularVelocity(float yaw, float pitch, float roll) {}
-
-Polar Propulsion::GetVelocity() {
-  return Polar(0, 0);
 }
 
-float Propulsion::GetAngularVelocity() {
+float Propulsion::GetActualForwardVelocity() { return 0; }
-  return 0;
+
-}
+Polar Propulsion::GetActualVelocity() { return Polar(0, 0); }
+
+float Propulsion::GetActualYawVelocity() { return 0; }

Propulsion.h

@@ -66,23 +66,26 @@ class Propulsion {
                                float pitch = 0.0F,
                                float roll = 0.0F);
   */
-  virtual void SetVelocity(float velocity);
+  virtual void SetTargetVelocity(float velocity);
-  virtual void SetVelocity(Polar velocity);
+  // virtual void SetTargetVelocity(Polar velocity);
-  virtual void SetVelocity(Spherical velocity);
+  // virtual void SetTargetVelocity(Spherical velocity);
-  virtual void SetVelocity(Vector2 velocity);
+  virtual void SetTargetVelocity(Vector2 velocity);
-  virtual void SetVelocity(Vector3 velocity);
+  virtual void SetTargetVelocity(Vector3 velocity);
 
-  virtual void SetAngularVelocity(float yaw);
+  virtual void SetTargetYawVelocity(float yaw);
-  virtual void SetAngularVelocity(float yaw, float pitch, float roll);
+  // virtual void SetTargetAngularVelocity(float yaw, float pitch, float roll);
 
+  virtual float GetTargetForwardVelocity();
+
+  virtual float GetActualForwardVelocity();
   /// @brief Retrieve the current velocity of the roboid
   /// @return The velocity in polar coordinates
   /// The actual units of the velocity depend on the implementation
-  virtual Polar GetVelocity();
+  virtual Polar GetActualVelocity();
   /// @brief Retrieve the current angular velocity of the roboid
   /// @return The angular velocity
   /// The actual unit of the angular velocity depend on the implementation
-  virtual float GetAngularVelocity();
+  virtual float GetActualYawVelocity();
 
   /// @brief The roboid of this propulsion system
   Roboid *roboid = nullptr;
@@ -93,6 +96,9 @@ class Propulsion {
   /// @brief The Placement of the motors used for Propulsion
   // Placement *placement = nullptr;
   Motor **motors = nullptr;
+
+  Vector3 targetVelocity;
+  float targetYawVelocity;
 };
 
 } // namespace RoboidControl

VectorAlgebra

@@ -1 +1 @@
-Subproject commit dc9d4ee42e42bff2fc051ffb48197232d01e53d6
+Subproject commit fd252d4b454548d6dd8eed17c0cae543f9655c18