diff --git a/ControlCore/LowLevelMessages.cpp b/ControlCore/LowLevelMessages.cpp index a72ada4..e80ebb1 100644 --- a/ControlCore/LowLevelMessages.cpp +++ b/ControlCore/LowLevelMessages.cpp @@ -2,6 +2,12 @@ #include "../float16/float16.h" +void LowLevelMessages::SendAngle8(unsigned char *buffer, unsigned char *ix, + const float angle) { + Angle8 packedAngle2 = Angle8::Degrees(angle); + buffer[(*ix)++] = packedAngle2.GetBinary(); +} + void LowLevelMessages::SendFloat16(unsigned char *buffer, unsigned char *ix, float value) { float16 value16 = float16(value); @@ -9,4 +15,33 @@ void LowLevelMessages::SendFloat16(unsigned char *buffer, unsigned char *ix, buffer[(*ix)++] = (binary >> 8) & 0xFF; buffer[(*ix)++] = binary & 0xFF; -} \ No newline at end of file +} + +void Passer::Control::LowLevelMessages::SendSpherical16(unsigned char *buffer, + unsigned char *ix, + Spherical16 s) { + SendFloat16(buffer, ix, s.distance); + SendAngle8(buffer, ix, s.direction.horizontal.InDegrees()); + SendAngle8(buffer, ix, s.direction.vertical.InDegrees()); +} + +void Passer::Control::LowLevelMessages::SendQuat32(unsigned char *buffer, + unsigned char *ix, + SwingTwist16 rotation) { + Quaternion q = rotation.ToQuaternion(); + unsigned char qx = (char)(q.x * 127 + 128); + unsigned char qy = (char)(q.y * 127 + 128); + unsigned char qz = (char)(q.z * 127 + 128); + unsigned char qw = (char)(q.w * 255); + if (q.w < 0) { + qx = -qx; + qy = -qy; + qz = -qz; + qw = -qw; + } + // Serial.printf(" (%d) %d:%d:%d:%d ", startIndex, qx, qy, qz, qw); + buffer[(*ix)++] = qx; + buffer[(*ix)++] = qy; + buffer[(*ix)++] = qz; + buffer[(*ix)++] = qw; +} diff --git a/ControlCore/LowLevelMessages.h b/ControlCore/LowLevelMessages.h index 67a6095..4af565d 100644 --- a/ControlCore/LowLevelMessages.h +++ b/ControlCore/LowLevelMessages.h @@ -1,11 +1,18 @@ #include "../LinearAlgebra/Spherical.h" +#include "../LinearAlgebra/SwingTwist.h" namespace Passer::Control { class LowLevelMessages { public: + static void SendAngle8(unsigned char *buffer, unsigned char *ix, + const float angle); static void SendFloat16(unsigned char *buffer, unsigned char *ix, float value); + static void SendSpherical16(unsigned char *buffer, unsigned char *ix, + Spherical16 s); + static void SendQuat32(unsigned char *buffer, unsigned char *ix, + SwingTwist16 q); }; } // namespace Passer::Control diff --git a/ControlCore/Messages.cpp b/ControlCore/Messages.cpp index 799e7d6..459c59e 100644 --- a/ControlCore/Messages.cpp +++ b/ControlCore/Messages.cpp @@ -130,4 +130,33 @@ unsigned char ModelUrlMsg::Serialize(unsigned char *buffer) { } // Model Url +#pragma endregion + +#pragma region PoseMsg + +#include +PoseMsg::PoseMsg(unsigned char networkId, unsigned char thingId, + unsigned char poseType, Spherical16 position, + SwingTwist16 orientation) { + this->networkId = networkId; + this->thingId = thingId; + this->position = position; + this->orientation = orientation; + this->poseType = poseType; +} + +unsigned char PoseMsg::Serialize(unsigned char *buffer) { + unsigned char ix = 0; + buffer[ix++] = PoseMsg::id; + buffer[ix++] = this->networkId; + buffer[ix++] = this->thingId; + buffer[ix++] = this->poseType; + LowLevelMessages::SendSpherical16(buffer, &ix, this->position); + printf("send spherical %f %f\n", this->position.distance, + this->position.direction.horizontal.InDegrees()); + LowLevelMessages::SendQuat32(buffer, &ix, this->orientation); + return ix; +} + +// Pose #pragma endregion \ No newline at end of file diff --git a/ControlCore/Messages.h b/ControlCore/Messages.h index 0033f0a..f62eb1e 100644 --- a/ControlCore/Messages.h +++ b/ControlCore/Messages.h @@ -1,4 +1,6 @@ #pragma once +#include "../LinearAlgebra/Spherical.h" +#include "../LinearAlgebra/SwingTwist.h" #include "../float16/float16.h" #include "CoreThing.h" #include "Participant.h" @@ -67,8 +69,10 @@ public: class ModelUrlMsg : public IMessage { public: static const unsigned char id = 0x90; + unsigned char networkId; unsigned char thingId; + float scale; unsigned char urlLength; const char *url; @@ -79,5 +83,27 @@ public: virtual unsigned char Serialize(unsigned char *buffer) override; }; +class PoseMsg : public IMessage { +public: + static const unsigned char id = 0x10; + unsigned char length = 4 + 4 + 4; + + unsigned char networkId; + unsigned char thingId; + + unsigned char poseType; + const unsigned char Pose_Position = 0x01; + const unsigned char Pose_Orientation = 0x02; + + Spherical16 position; + SwingTwist16 orientation; + + PoseMsg(unsigned char networkId, unsigned char thingId, + unsigned char poseType, Spherical16 position, + SwingTwist16 orientation); + + virtual unsigned char Serialize(unsigned char *buffer) override; +}; + } // namespace Passer::Control using namespace Passer::Control; \ No newline at end of file diff --git a/Messages.cpp b/Messages.cpp index 1c6f44d..5577ca4 100644 --- a/Messages.cpp +++ b/Messages.cpp @@ -61,13 +61,14 @@ void Messages::SendPolar(unsigned char *buffer, unsigned char *startIndex, SendSingle100(buffer, (*startIndex) + 1, p.distance); } -void Messages::SendSpherical16(unsigned char *buffer, unsigned char *startIndex, - Spherical16 s) { - // Receive first does distance... - SendAngle8(buffer, (*startIndex)++, s.direction.horizontal.InDegrees()); - SendAngle8(buffer, (*startIndex)++, s.direction.vertical.InDegrees()); - SendFloat16(buffer, startIndex, s.distance); -} +// void Messages::SendSpherical16(unsigned char *buffer, unsigned char +// *startIndex, +// Spherical16 s) { +// // Receive first does distance... +// SendAngle8(buffer, (*startIndex)++, s.direction.horizontal.InDegrees()); +// SendAngle8(buffer, (*startIndex)++, s.direction.vertical.InDegrees()); +// SendFloat16(buffer, startIndex, s.distance); +// } void Messages::SendSwingTwist(unsigned char *buffer, unsigned char *ix, const SwingTwist16 r) { diff --git a/Messages.h b/Messages.h index c9d9020..ab79673 100644 --- a/Messages.h +++ b/Messages.h @@ -32,8 +32,9 @@ public: static void SendPolar(unsigned char *buffer, unsigned char *startIndex, Polar p); - static void SendSpherical16(unsigned char *buffer, unsigned char *startIndex, - Spherical16 s); + // static void SendSpherical16(unsigned char *buffer, unsigned char + // *startIndex, + // Spherical16 s); static void SendSwingTwist(unsigned char *buffer, unsigned char *startIndex, const SwingTwist16 r); static void SendQuat32(unsigned char *buffer, unsigned char *startIndex, diff --git a/NetworkSync.cpp b/NetworkSync.cpp index 4ac9784..a3a0028 100644 --- a/NetworkSync.cpp +++ b/NetworkSync.cpp @@ -236,19 +236,23 @@ void NetworkSync::SendPose(Thing *thing, bool force, bool recurse) { thing->positionUpdated |= thing->GetLinearVelocity().distance > 0; thing->orientationUpdated |= thing->GetAngularVelocity().distance > 0; if (force || thing->positionUpdated || thing->orientationUpdated) { - unsigned char ix = 0; - buffer[ix++] = PoseMsg; - buffer[ix++] = this->networkId; - buffer[ix++] = thing->id; - unsigned char pattern = 0; + // unsigned char ix = 0; + // buffer[ix++] = PoseMsg; + // buffer[ix++] = this->networkId; + // buffer[ix++] = thing->id; + unsigned char poseType = 0; if (force || thing->positionUpdated) - pattern |= Pose_Position; + poseType |= Pose_Position; if (force || thing->orientationUpdated) - pattern |= Pose_Orientation; - buffer[ix++] = pattern; - Messages::SendSpherical16(buffer, &ix, thing->position); - Messages::SendQuat32(buffer, &ix, thing->orientation.ToQuaternion()); - SendBuffer(ix); + poseType |= Pose_Orientation; + // buffer[ix++] = poseType; + // Messages::SendSpherical16(buffer, &ix, thing->position); + // Messages::SendQuat32(buffer, &ix, thing->orientation.ToQuaternion()); + // SendBuffer(ix); + Passer::Control::PoseMsg msg = + Passer::Control::PoseMsg(this->networkId, thing->id, poseType, + thing->position, thing->orientation); + SendBuffer(msg.Serialize(this->buffer)); thing->positionUpdated = false; thing->orientationUpdated = false; @@ -353,14 +357,19 @@ void NetworkSync::PublishTrackedObject(Roboid *roboid, Spherical16 worldPosition = inv_originOrientation * (thing->position - originPosition); - unsigned char ix = 0; - buffer[ix++] = PoseMsg; // Position2DMsg; - buffer[ix++] = this->networkId; - buffer[ix++] = thing->id; // objectId; - buffer[ix++] = Pose_Position | Pose_Orientation; - Messages::SendSpherical16(buffer, &ix, worldPosition); - Messages::SendSwingTwist(buffer, &ix, worldOrientation); - SendBuffer(ix); + // unsigned char ix = 0; + // buffer[ix++] = PoseMsg; // Position2DMsg; + // buffer[ix++] = this->networkId; + // buffer[ix++] = thing->id; // objectId; + // buffer[ix++] = Pose_Position | Pose_Orientation; + // Messages::SendSpherical16(buffer, &ix, worldPosition); + // Messages::SendSwingTwist(buffer, &ix, worldOrientation); + // SendBuffer(ix); + + Passer::Control::PoseMsg msg = Passer::Control::PoseMsg( + this->networkId, thing->id, Pose_Position | Pose_Orientation, + worldPosition, worldOrientation); + SendBuffer(msg.Serialize(this->buffer)); #if RC_DEBUG printf("Sent Thing PoseMsg [%d/%d] %d\n", networkId, buffer[1], thing->type); diff --git a/Roboid.cpp b/Roboid.cpp index 3649202..ee02826 100644 --- a/Roboid.cpp +++ b/Roboid.cpp @@ -31,6 +31,7 @@ Roboid::Roboid(Propulsion *propulsion) : Roboid() { propulsion->roboid = this; } +#include void Roboid::Update(unsigned long currentTimeMs) { if (this->lastUpdateTimeMs == 0) this->lastUpdateTimeMs = currentTimeMs; @@ -41,22 +42,28 @@ void Roboid::Update(unsigned long currentTimeMs) { if (propulsion != nullptr) { propulsion->Update(currentTimeMs); + printf("orientation %f velocity %f\n", + orientation.swing.horizontal.InDegrees(), + linearVelocity.direction.horizontal); + float deltaTime = (float)(currentTimeMs - lastUpdateTimeMs) / 1000; - this->linearVelocity = this->propulsion->GetVelocity(); - - Spherical16 translation = - this->orientation * this->linearVelocity * deltaTime; - SetPosition(this->position + translation); - this->angularVelocity = this->propulsion->GetAngularVelocity(); SwingTwist16 rotation = SwingTwist16::AngleAxis(this->angularVelocity.distance * deltaTime, this->angularVelocity.direction); - // if (perception != nullptr) - // perception->UpdatePose(rotation); - SetOrientation(this->orientation * rotation); + + this->linearVelocity = this->propulsion->GetVelocity(); + printf(" orientation %f velocity %f\n", + orientation.swing.horizontal.InDegrees(), + linearVelocity.direction.horizontal); + Spherical16 translation = + this->orientation * this->linearVelocity * deltaTime; + SetPosition(this->position + translation); + printf(" orientation %f velocity %f\n", + orientation.swing.horizontal.InDegrees(), + linearVelocity.direction.horizontal); } if (children != nullptr) {