From 610121f944d49bec1631cb9bd6f35582b8134741 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Thu, 7 Nov 2024 13:59:24 +0100 Subject: [PATCH] Velocity refactoring --- DifferentialDrive.cpp | 7 ++-- DifferentialDrive.h | 2 +- NetworkSync.cpp | 26 +++++++++++---- NetworkSync.h | 77 ++++++++++++++++++++----------------------- Propulsion.cpp | 6 ++-- Propulsion.h | 6 ++-- Roboid.cpp | 19 ++++++----- Thing.cpp | 1 + Thing.h | 2 ++ 9 files changed, 82 insertions(+), 64 deletions(-) diff --git a/DifferentialDrive.cpp b/DifferentialDrive.cpp index 344f6d6..9413c72 100644 --- a/DifferentialDrive.cpp +++ b/DifferentialDrive.cpp @@ -73,7 +73,7 @@ void DifferentialDrive::SetVelocity(Polar velocity) { SetTwistSpeed(velocity.distance, velocity.angle.InDegrees()); } -Polar DifferentialDrive::GetVelocity() { +Spherical16 DifferentialDrive::GetVelocity() { Motor *leftMotor = motors[0]; Motor *rightMotor = motors[1]; float leftSpeed = leftMotor->GetActualSpeed(); // in revolutions per second @@ -85,8 +85,9 @@ Polar DifferentialDrive::GetVelocity() { float direction = speed >= 0 ? 0.0F : 180.0F; float magnitude = fabsf(speed); - Polar velocity = Polar( - magnitude, Angle::Degrees(direction)); // Polar(direction, magnitude); + Spherical16 velocity = + Spherical16(magnitude, Angle16::Degrees(direction), + Angle16::Degrees(0)); // Polar(direction, magnitude); return velocity; } diff --git a/DifferentialDrive.h b/DifferentialDrive.h index 0424a5c..781d6f9 100644 --- a/DifferentialDrive.h +++ b/DifferentialDrive.h @@ -68,7 +68,7 @@ public: /// information /// @remark This will be more expanded/detailed in a future version of Roboid /// Control - virtual Polar GetVelocity() override; + virtual Spherical16 GetVelocity() override; /// @brief Calculate the angular velocity of the roboid based on the wheel /// velocities /// @return The angular speed of the roboid in local space diff --git a/NetworkSync.cpp b/NetworkSync.cpp index dc45925..a74409d 100644 --- a/NetworkSync.cpp +++ b/NetworkSync.cpp @@ -1,6 +1,6 @@ #include "NetworkSync.h" -#define RC_DEBUG 1 +// #define RC_DEBUG 1 #ifdef RC_DEBUG #include @@ -39,6 +39,7 @@ void NetworkSync::ReceiveNetworkId() { #ifdef RC_DEBUG SERIALPORT.printf("Received network Id %d\n", this->networkId); #endif + SendThing(roboid); SendName(roboid); SendModel(roboid); for (unsigned char childIx = 0; childIx < roboid->childCount; childIx++) { @@ -64,6 +65,21 @@ void NetworkSync::PublishState(Roboid *roboid) { PublishPerception(roboid); } +void NetworkSync::SendThing(Thing *thing) { + if (thing == nullptr) + return; + + unsigned char ix = 0; + buffer[ix++] = CreateMsg; + buffer[ix++] = thing->id; + buffer[ix++] = thing->type; + SendBuffer(ix); + +#ifdef RC_DEBUG + printf("Sent Thing [%d/%d] %d\n", networkId, buffer[1], buffer[2]); +#endif +} + void NetworkSync::NewObject(InterestingThing *thing) { if (thing == nullptr || thing->networkId != 0x00) return; @@ -314,11 +330,9 @@ void NetworkSync::PublishTrackedObject(Roboid *roboid, } void NetworkSync::SendPoseMsg(Buffer sendBuffer, Roboid *roboid) { - // SERIALPORT.printf("old send pose?\n"); - Polar velocity = roboid->propulsion->GetVelocity(); - Vector2 worldVelocity2 = - Vector2::Rotate(Vector2::forward * velocity.distance, velocity.angle); - Vector3 worldVelocity3 = Vector3(worldVelocity2.x, 0, worldVelocity2.y); + Spherical16 velocity = roboid->propulsion->GetVelocity(); + Spherical16 worldVelocity = roboid->orientation * velocity; + Vector3 worldVelocity3 = worldVelocity.ToVector3(); float angularVelocity = roboid->propulsion->GetAngularVelocity(); Vector3 worldAngularVelocity = Vector3(0, angularVelocity, 0); diff --git a/NetworkSync.h b/NetworkSync.h index a848c14..75286a8 100644 --- a/NetworkSync.h +++ b/NetworkSync.h @@ -10,22 +10,23 @@ namespace RoboidControl { /// @brief Interface for synchronizaing state between clients across a network class NetworkSync { - public: +public: NetworkSync() {}; - NetworkSync(Roboid* roboid); + NetworkSync(Roboid *roboid); unsigned char networkId = 0; /// @brief Retreive and send the roboid state /// @param roboid The roboid for which the state is updated - virtual void NetworkUpdate(Roboid* roboid) = 0; + virtual void NetworkUpdate(Roboid *roboid) = 0; /// @brief Inform that the given object is no longer being tracked /// @param obj - virtual void SendDestroyThing(InterestingThing* obj); - virtual void NewObject(InterestingThing* obj); - void SendName(Roboid* roboid); - virtual void SendModel(Roboid* obj); - void SendModel(Thing* thing); + virtual void SendDestroyThing(InterestingThing *obj); + virtual void NewObject(InterestingThing *obj); + void SendThing(Thing *thing); + void SendName(Roboid *roboid); + virtual void SendModel(Roboid *obj); + void SendModel(Thing *thing); /// @brief The id of a Pose message static const unsigned char PoseMsg = 0x10; @@ -57,71 +58,65 @@ class NetworkSync { static const unsigned char DeviceMsg = 0xA0; static const unsigned char NetworkIdMsg = 0xA1; - typedef void (*Buffer)(UInt8* buffer, UInt16 bufferSize); + typedef void (*Buffer)(UInt8 *buffer, UInt16 bufferSize); - void ReceiveMessage(Roboid* roboid, unsigned char bytecount); + void ReceiveMessage(Roboid *roboid, unsigned char bytecount); void ReceiveNetworkId(); - void PublishState(Roboid* roboid); + void PublishState(Roboid *roboid); - void SendInvestigate(InterestingThing* thing); + void SendInvestigate(InterestingThing *thing); - void SendPoseMsg(Buffer sendBuffer, Roboid* roboid); + void SendPoseMsg(Buffer sendBuffer, Roboid *roboid); // void SendDestroyThing(Buffer sendBuffer, InterestingThing* obj); // void PublishNewObject(); - void PublishRelativeThing(Thing* thing, bool recurse = false); + void PublishRelativeThing(Thing *thing, bool recurse = false); - void PublishPerception(Roboid* roboid); - void PublishTrackedObjects(Roboid* roboid, InterestingThing** objects); + void PublishPerception(Roboid *roboid); + void PublishTrackedObjects(Roboid *roboid, InterestingThing **objects); virtual void SendPosition(Spherical16 worldPosition) {}; virtual void SendPose(Spherical16 worldPosition, SwingTwist16 worldOrientation) {}; // void SendPose(Roboid* roboid, bool recurse = true); - void SendPose(Thing* thing, bool recurse = true); + void SendPose(Thing *thing, bool recurse = true); - virtual void SendText(const char* s); + virtual void SendText(const char *s); void SendInt(const int x); - protected: - Roboid* roboid; - NetworkPerception* networkPerception; +protected: + Roboid *roboid; + NetworkPerception *networkPerception; - void PublishState(Sensor* sensor); + void PublishState(Sensor *sensor); - void PublishTrackedObject(Roboid* roboid, InterestingThing* object); + void PublishTrackedObject(Roboid *roboid, InterestingThing *object); // void PublishRelativeObject(Buffer sendBuffer, // UInt8 parentId, // InterestingThing* object); - void SendSingle100(unsigned char* data, unsigned int startIndex, float value); - void SendFloat16(unsigned char* data, unsigned char* startIndex, float value); + void SendSingle100(unsigned char *data, unsigned int startIndex, float value); + void SendFloat16(unsigned char *data, unsigned char *startIndex, float value); - void SendInt32(unsigned char* data, unsigned int startIndex, Int32 value); - void SendAngle8(unsigned char* data, - unsigned int startIndex, + void SendInt32(unsigned char *data, unsigned int startIndex, Int32 value); + void SendAngle8(unsigned char *data, unsigned int startIndex, const float value); // void SendAngle16(unsigned char *data, unsigned int startIndex, // const float value); // void SendAngle32(unsigned char *data, unsigned int startIndex, // const float value); - void SendVector3(unsigned char* data, - unsigned char* startIndex, + void SendVector3(unsigned char *data, unsigned char *startIndex, const Vector3 v); - void SendQuaternion(unsigned char* data, - const int startIndex, + void SendQuaternion(unsigned char *data, const int startIndex, const Quaternion q); - void SendPolar(unsigned char* data, unsigned char* startIndex, Polar p); - void SendSpherical16(unsigned char* data, - unsigned char* startIndex, + void SendPolar(unsigned char *data, unsigned char *startIndex, Polar p); + void SendSpherical16(unsigned char *data, unsigned char *startIndex, Spherical16 s); - void SendSwingTwist(unsigned char* data, - unsigned char* startIndex, + void SendSwingTwist(unsigned char *data, unsigned char *startIndex, const SwingTwist16 r); - void SendQuat32(unsigned char* data, - unsigned char* startIndex, + void SendQuat32(unsigned char *data, unsigned char *startIndex, const Quaternion q); unsigned char buffer[256]; @@ -131,5 +126,5 @@ class NetworkSync { void PublishDevice(); }; -} // namespace RoboidControl -} // namespace Passer \ No newline at end of file +} // namespace RoboidControl +} // namespace Passer \ No newline at end of file diff --git a/Propulsion.cpp b/Propulsion.cpp index 035ce82..932d139 100644 --- a/Propulsion.cpp +++ b/Propulsion.cpp @@ -35,8 +35,10 @@ void Propulsion::SetTwistSpeed(Vector2 linear, float yaw) {} void Propulsion::SetTwistSpeed(Vector3 linear, float yaw, float pitch, float roll) {} -void Propulsion::SetVelocity(Polar velocity) {} +void Propulsion::SetVelocity(Spherical16 velocity) { + this->linearVelocity = velocity; +} -Polar Propulsion::GetVelocity() { return Polar::zero; } +Spherical16 Propulsion::GetVelocity() { return this->linearVelocity; } float Propulsion::GetAngularVelocity() { return 0; } \ No newline at end of file diff --git a/Propulsion.h b/Propulsion.h index 6800d41..fcc4b49 100644 --- a/Propulsion.h +++ b/Propulsion.h @@ -59,12 +59,12 @@ public: virtual void SetTwistSpeed(Vector3 linear, float yaw = 0.0F, float pitch = 0.0F, float roll = 0.0F); - virtual void SetVelocity(Polar velocity); + virtual void SetVelocity(Spherical16 velocity); /// @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 Spherical16 GetVelocity(); /// @brief Retrieve the current angular velocity of the roboid /// @return The angular velocity /// The actual unit of the angular velocity depend on the implementation @@ -79,6 +79,8 @@ protected: /// @brief The Placement of the motors used for Propulsion // Placement *placement = nullptr; Motor **motors = nullptr; + + Spherical16 linearVelocity = Spherical16::zero; }; } // namespace RoboidControl diff --git a/Roboid.cpp b/Roboid.cpp index 9156bfb..dedef27 100644 --- a/Roboid.cpp +++ b/Roboid.cpp @@ -18,11 +18,10 @@ Roboid::Roboid() : Thing(0) { this->type = RoboidType; this->perception = new Perception(); this->perception->roboid = this; - this->propulsion = nullptr; + this->propulsion = new Propulsion(); this->networkSync = nullptr; - - this->roboidPosition = Spherical16::zero; - this->roboidOrientation = SwingTwist16::identity; + this->position = Spherical16::zero; + this->orientation = SwingTwist16::identity; } Roboid::Roboid(Propulsion *propulsion) : Roboid() { @@ -43,10 +42,11 @@ void Roboid::Update(unsigned long currentTimeMs) { float deltaTime = (float)(currentTimeMs - lastUpdateTimeMs) / 1000; // Conversion from old units - Polar polarVelocity = this->propulsion->GetVelocity(); - this->linearVelocity = Spherical16( - polarVelocity.distance, - Angle16::Degrees(polarVelocity.angle.InDegrees()), Angle16()); + // Polar polarVelocity = this->propulsion->GetVelocity(); + // this->linearVelocity = Spherical16( + // polarVelocity.distance, + // Angle16::Degrees(polarVelocity.angle.InDegrees()), Angle16()); + this->linearVelocity = this->propulsion->GetVelocity(); float oldAngular = this->propulsion->GetAngularVelocity(); this->angularVelocity = Spherical16(oldAngular, Angle16(), Angle16::Degrees(90)); @@ -54,7 +54,8 @@ void Roboid::Update(unsigned long currentTimeMs) { // SetPosition(this->position + this->orientation * Spherical16::forward * // this->linearVelocity.distance * // deltaTime); - // this->roboidPosition = this->position; // assuming the roboid is the root + // this->roboidPosition = this->position; // assuming the roboid is the + // root this->position += this->orientation * Spherical16::forward * this->linearVelocity.distance * deltaTime; diff --git a/Thing.cpp b/Thing.cpp index 3138a0e..2a94cc6 100644 --- a/Thing.cpp +++ b/Thing.cpp @@ -21,6 +21,7 @@ const unsigned int Thing::ControlledMotorType = const unsigned int Thing::UncontrolledMotorType = MotorType | (unsigned int)Type::UncontrolledMotor; const unsigned int Thing::ServoType = (unsigned int)Type::Servo; +const unsigned int Thing::HumanoidType = (unsigned int)Type::Humanoid; bool Thing::IsMotor() { return (type & Thing::MotorType) != 0; } diff --git a/Thing.h b/Thing.h index a426315..b660528 100644 --- a/Thing.h +++ b/Thing.h @@ -32,6 +32,7 @@ public: static const unsigned int UncontrolledMotorType; /// @brief The type of an object received from the network static const unsigned int ServoType; + static const unsigned int HumanoidType; /// @brief Check if the Thing is a Motor /// @return True when the Thing is a Motor and False otherwise @@ -118,6 +119,7 @@ protected: UncontrolledMotor, Servo, // Other + Humanoid, ExternalSensor, };