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

@@ -4,7 +4,7 @@
 #include <Arduino.h>
 #endif
 
-UInt8 NetworkSync::CreatePoseMsg(Roboid* roboid, UInt8* buffer) {
+UInt8 NetworkSync::CreatePoseMsg(Roboid *roboid, UInt8 *buffer) {
   Vector3 position = roboid->GetPosition();
 
   UInt8 bufferSize = 3 + 12;
@@ -15,14 +15,13 @@ UInt8 NetworkSync::CreatePoseMsg(Roboid* roboid, UInt8* buffer) {
   return bufferSize;
 }
 
-void NetworkSync::SendVector3(unsigned char* data, int startIndex, Vector3 v) {
+void NetworkSync::SendVector3(unsigned char *data, int startIndex, Vector3 v) {
   SendSingle100(data, startIndex, v.x);
   SendSingle100(data, startIndex + 4, v.y);
   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;
@@ -32,16 +31,16 @@ void NetworkSync::SendSingle100(unsigned char* data,
   // }
 }
 
-void NetworkSync::SendInt32(unsigned char* data, int startIndex, Int32 value) {
+void NetworkSync::SendInt32(unsigned char *data, int startIndex, Int32 value) {
   for (unsigned char ix = 0; ix < 4; ix++) {
-    data[startIndex + ix] = ((unsigned char*)&value)[ix];
+    data[startIndex + ix] = ((unsigned char *)&value)[ix];
   }
 }
 
 void NetworkSync::PublishTrackedObjects(SendBuffer sendBuffer,
-                                        TrackedObject** objects) {
+                                        TrackedObject **objects) {
   for (unsigned char objIx = 0; objIx < Perception::maxObjectCount; objIx++) {
-    TrackedObject* obj = objects[objIx];
+    TrackedObject *obj = objects[objIx];
     if (obj == nullptr)
       continue;
     // if (obj->sensor->type == Thing::ExternalType)
@@ -54,7 +53,7 @@ void NetworkSync::PublishTrackedObjects(SendBuffer sendBuffer,
 }
 
 void NetworkSync::PublishTrackedObject(SendBuffer sendBuffer,
-                                       TrackedObject* object) {
+                                       TrackedObject *object) {
   Vector2 worldPosition2 = Vector2::Rotate(
       Vector2::forward * object->position.distance, -object->position.angle);
   Vector3 worldPosition3 = Vector3(worldPosition2.x, 0, worldPosition2.y);
@@ -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);
@@ -80,13 +79,13 @@ void NetworkSync::PublishTrackedObject(SendBuffer sendBuffer,
 #endif
 }
 
-void NetworkSync::SendPoseMsg(SendBuffer sendBuffer, Roboid* roboid) {
+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
@@ -116,7 +115,7 @@ void NetworkSync::SendPoseMsg(SendBuffer sendBuffer, Roboid* roboid) {
 #endif
 }
 
-void NetworkSync::SendDestroyObject(SendBuffer sendBuffer, TrackedObject* obj) {
+void NetworkSync::SendDestroyObject(SendBuffer sendBuffer, TrackedObject *obj) {
 #ifdef RC_DEBUG
   Serial.print("Send Destroy ");
   Serial.println((int)obj->id);

Propulsion.cpp

@@ -8,21 +8,19 @@ 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) {
+Motor *Propulsion::GetMotor(unsigned int motorId) {
   if (motorId >= this->motorCount)
     return nullptr;
 
-  Motor* motor = this->motors[motorId];
+  Motor *motor = this->motors[motorId];
   return motor;
 }
 
 void Propulsion::Update(float currentTimeMs) {
   for (unsigned char motorIx = 0; motorIx < this->motorCount; motorIx++) {
-    Motor* motor = this->motors[motorIx];
+    Motor *motor = this->motors[motorIx];
     if (motor == nullptr)
       continue;
 
@@ -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

@@ -18,7 +18,7 @@ class Roboid;
 /// for a robot. This base class does not implement the functions to move the
 /// Roboid around.
 class Propulsion {
- public:
+public:
   /// @brief Default Constructor for Propulsion
   Propulsion();
 
@@ -33,7 +33,7 @@ class Propulsion {
   /// @param motorIx The index of the motor
   /// @return Returns the motor or a nullptr when no motor with the given index
   /// could be found
-  Motor* GetMotor(unsigned int motorIx);
+  Motor *GetMotor(unsigned int motorIx);
   /// @brief Get the Placement of a specific Motor
   /// @param motorIx The index of the Motor
   /// @return Returns the Placement or a nullptr when no Placement with the give
@@ -66,33 +66,39 @@ 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;
+  Roboid *roboid = nullptr;
 
- protected:
+protected:
   /// @brief The number of motors used for Propulsion
   unsigned int motorCount = 0;
   /// @brief The Placement of the motors used for Propulsion
   // Placement *placement = nullptr;
-  Motor** motors = nullptr;
+  Motor **motors = nullptr;
+
+  Vector3 targetVelocity;
+  float targetYawVelocity;
 };
 
 } // namespace RoboidControl

VectorAlgebra

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