From 31bd49dde9a2c5188d2f2223ce678fd7166600fc Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Tue, 23 Jan 2024 10:58:33 +0100 Subject: [PATCH] Improved velocity functions --- DifferentialDrive.cpp | 54 +++++++++++++++++++++++++++------------- DifferentialDrive.h | 57 ++++++++++++++++++++++++++----------------- NetworkSync.cpp | 31 ++++++++++++----------- Propulsion.cpp | 53 ++++++++++++++++++++-------------------- Propulsion.h | 38 +++++++++++++++++------------ VectorAlgebra | 2 +- 6 files changed, 135 insertions(+), 100 deletions(-) diff --git a/DifferentialDrive.cpp b/DifferentialDrive.cpp index 86a5cf5..6d61b6c 100644 --- a/DifferentialDrive.cpp +++ b/DifferentialDrive.cpp @@ -47,28 +47,35 @@ void DifferentialDrive::SetMotorTargetSpeeds(float leftSpeed, }; } -void DifferentialDrive::SetTwistSpeed(float forward, float yaw) { - float leftSpeed = - Float::Clamp(forward - yaw, -1, 1); // revolutions per second - float rightSpeed = - Float::Clamp(forward + yaw, -1, 1); // revolutions per second +void DifferentialDrive::SetVelocity(float forwardVelocity) { + targetVelocity = Vector3::forward * forwardVelocity; + Update(); + // 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 + // float leftMotorSpeed = leftSpeed / rpsToMs; // meters per second + // float rightMotorSpeed = rightSpeed / rpsToMs; // meters per second - SetMotorTargetSpeeds(leftMotorSpeed, rightMotorSpeed); + // SetMotorTargetSpeeds(leftMotorSpeed, rightMotorSpeed); } -void DifferentialDrive::SetTwistSpeed(Vector2 linear, float yaw) { - SetTwistSpeed(linear.y, yaw); +void DifferentialDrive::SetAngularVelocity(float yawVelocity) { + 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, - float roll) { - SetTwistSpeed(linear.z, yaw); -} - -Polar DifferentialDrive::GetVelocity() { +Polar DifferentialDrive::GetActualVelocity() { 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 @@ -100,4 +107,17 @@ float DifferentialDrive::GetAngularVelocity() { float angularVelocity = degreesPerSecond; 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); } \ No newline at end of file diff --git a/DifferentialDrive.h b/DifferentialDrive.h index a3526b8..9ac9cec 100644 --- a/DifferentialDrive.h +++ b/DifferentialDrive.h @@ -36,28 +36,37 @@ 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 + virtual void SetTwistSpeed(float forward, float yaw) override; + /// @brief Controls the motors through forward and rotation speeds + /// @param linear The target linear speed of the Roboid + /// @param yaw The target rotation speed of the Roboid + /// @note As a DifferentialDrive cannot move sideward, this function has the + /// same effect as using the void SetTwistSpeed(float forward, float yaw) + /// function. + virtual void SetTwistSpeed(Vector2 linear, float yaw = 0.0F); + /// @brief Controls the motors through forward and rotation speeds + /// @param linear The target linear speed + /// @param yaw The target rotation speed around the vertical axis + /// @param pitch Pitch is not supported and is ignored + /// @param roll Roll is not supported and is ignores + /// @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, float yaw = 0.0F, + float pitch = 0.0F, float roll = 0.0F); + */ - /// @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 - virtual void SetTwistSpeed(float forward, float yaw) override; - /// @brief Controls the motors through forward and rotation speeds - /// @param linear The target linear speed of the Roboid - /// @param yaw The target rotation speed of the Roboid - /// @note As a DifferentialDrive cannot move sideward, this function has the - /// same effect as using the void SetTwistSpeed(float forward, float yaw) - /// function. - virtual void SetTwistSpeed(Vector2 linear, float yaw = 0.0F); - /// @brief Controls the motors through forward and rotation speeds - /// @param linear The target linear speed - /// @param yaw The target rotation speed around the vertical axis - /// @param pitch Pitch is not supported and is ignored - /// @param roll Roll is not supported and is ignores - /// @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, 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 diff --git a/NetworkSync.cpp b/NetworkSync.cpp index dfe6fd7..f4e5b21 100644 --- a/NetworkSync.cpp +++ b/NetworkSync.cpp @@ -4,25 +4,24 @@ #include #endif -UInt8 NetworkSync::CreatePoseMsg(Roboid* roboid, UInt8* buffer) { +UInt8 NetworkSync::CreatePoseMsg(Roboid *roboid, UInt8 *buffer) { Vector3 position = roboid->GetPosition(); UInt8 bufferSize = 3 + 12; buffer[0] = PoseMsg; - buffer[1] = 0; // ObjectId; + buffer[1] = 0; // ObjectId; buffer[2] = Pose_Position; SendVector3(buffer, 3, position); 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, - int startIndex, +void NetworkSync::SendSingle100(unsigned char *data, 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) { - Polar velocity = roboid->propulsion->GetVelocity(); +void NetworkSync::SendPoseMsg(SendBuffer sendBuffer, Roboid *roboid) { + 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 @@ -106,7 +105,7 @@ void NetworkSync::SendPoseMsg(SendBuffer sendBuffer, Roboid* roboid) { const unsigned int bufferSize = 3 + 12 + 12; unsigned char buffer[bufferSize] = { PoseMsg, - 0, // objectId; + 0, // objectId; Pose_LinearVelocity | Pose_AngularVelocity, }; SendVector3(buffer, 3, worldVelocity3); @@ -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); diff --git a/Propulsion.cpp b/Propulsion.cpp index 5b99852..6cebcb4 100644 --- a/Propulsion.cpp +++ b/Propulsion.cpp @@ -8,21 +8,19 @@ Propulsion::Propulsion() { this->motorCount = 0; } -unsigned int Propulsion::GetMotorCount() { - return this->motorCount; -} +unsigned int Propulsion::GetMotorCount() { 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::SetTwistSpeed(float forward, float yaw) {} +void Propulsion::SetTargetVelocity(float forwardVelocity) { + 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 roll) {} -*/ - -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::GetTargetForwardVelocity() { + return this->targetVelocity.Forward(); } -float Propulsion::GetAngularVelocity() { - return 0; -} \ No newline at end of file +float Propulsion::GetActualForwardVelocity() { return 0; } + +Polar Propulsion::GetActualVelocity() { return Polar(0, 0); } + +float Propulsion::GetActualYawVelocity() { return 0; } \ No newline at end of file diff --git a/Propulsion.h b/Propulsion.h index 7710430..6e9a705 100644 --- a/Propulsion.h +++ b/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,35 +66,41 @@ class Propulsion { float pitch = 0.0F, float roll = 0.0F); */ - virtual void SetVelocity(float velocity); - virtual void SetVelocity(Polar velocity); - virtual void SetVelocity(Spherical velocity); - virtual void SetVelocity(Vector2 velocity); - virtual void SetVelocity(Vector3 velocity); + virtual void SetTargetVelocity(float velocity); + // virtual void SetTargetVelocity(Polar velocity); + // virtual void SetTargetVelocity(Spherical velocity); + virtual void SetTargetVelocity(Vector2 velocity); + virtual void SetTargetVelocity(Vector3 velocity); - virtual void SetAngularVelocity(float yaw); - virtual void SetAngularVelocity(float yaw, float pitch, float roll); + virtual void SetTargetYawVelocity(float yaw); + // 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 -} // namespace Passer +} // namespace RoboidControl +} // namespace Passer using namespace Passer::RoboidControl; \ No newline at end of file diff --git a/VectorAlgebra b/VectorAlgebra index dc9d4ee..fd252d4 160000 --- a/VectorAlgebra +++ b/VectorAlgebra @@ -1 +1 @@ -Subproject commit dc9d4ee42e42bff2fc051ffb48197232d01e53d6 +Subproject commit fd252d4b454548d6dd8eed17c0cae543f9655c18