diff --git a/LinearAlgebra b/LinearAlgebra index f1c70c7..1ea65d5 160000 --- a/LinearAlgebra +++ b/LinearAlgebra @@ -1 +1 @@ -Subproject commit f1c70c756ebfd58864acbbede234e62d93cc741d +Subproject commit 1ea65d56b1ee69407ff0ac2f9e1f232fed7e57e5 diff --git a/NetworkPerception.cpp b/NetworkPerception.cpp index fa7e4c2..2e15712 100644 --- a/NetworkPerception.cpp +++ b/NetworkPerception.cpp @@ -2,7 +2,7 @@ #include "NetworkSync.h" -// #define RC_DEBUG true +#define RC_DEBUG true #if RC_DEBUG #include #endif @@ -149,7 +149,7 @@ void NetworkPerception::ReceivePoseMsg(unsigned char* data, Roboid* roboid) { return ReceiveSphere(data, roboid); #if RC_DEBUG - printf("Received PoseMsg [%d/%d]\n", networkId, objectId); + // printf("Received PoseMsg [%d/%d]\n", networkId, objectId); #endif SwingTwist16 roboidOrientation = roboid->GetOrientation(); diff --git a/NetworkSync.cpp b/NetworkSync.cpp index 2efbc06..fc3039e 100644 --- a/NetworkSync.cpp +++ b/NetworkSync.cpp @@ -1,9 +1,14 @@ #include "NetworkSync.h" -// #define RC_DEBUG 1 +#define RC_DEBUG 1 #ifdef RC_DEBUG #include +#if ESP32 +#define SERIALPORT Serial0 +#else +#define SERIALPORT Serial +#endif #endif #include "LinearAlgebra/Angle8.h" @@ -34,7 +39,7 @@ void NetworkSync::ReceiveMessage(Roboid* roboid, unsigned char bytecount) { void NetworkSync::ReceiveNetworkId() { this->networkId = buffer[1]; #ifdef RC_DEBUG - Serial.printf("_Received network Id %d\n", this->networkId); + SERIALPORT.printf("_Received network Id %d\n", this->networkId); #endif SendName(roboid); SendModel(roboid); @@ -111,7 +116,8 @@ void NetworkSync::SendName(Roboid* roboid) { SendBuffer(ix); #ifdef RC_DEBUG - printf("Sent Name [%d/%d] %s\n", networkId, buffer[1], roboid->name); + SERIALPORT.printf("Sent Name [%d/%d] %s\n", networkId, buffer[1], + roboid->name); #endif } @@ -173,34 +179,6 @@ void NetworkSync::SendDestroyThing(InterestingThing* thing) { #endif } -// void NetworkSync::SendPose(Roboid* roboid, bool recurse) { -// if (networkId == 0) // We're not connected to a site yet -// return; - -// if (roboid->GetVelocity().magnitude() > 0) { -// unsigned char ix = 0; -// buffer[ix++] = PoseMsg; -// buffer[ix++] = 0x00; -// buffer[ix++] = Pose_Position | Pose_Orientation; -// SendSpherical(buffer, &ix, -// Spherical::FromVector3(roboid->GetPosition())); SendQuat32(buffer, &ix, -// roboid->GetOrientation()); SendBuffer(ix); -// } - -// #if RC_DEBUG -// printf("Sent PoseMsg [%d/%d]\n", networkId, buffer[1]); -// #endif - -// if (recurse) { -// for (unsigned char childIx = 0; childIx < roboid->childCount; childIx++) -// { -// Thing* child = roboid->GetChild(childIx); -// if (child != nullptr) -// SendPose(child, true); -// } -// } -// } - void NetworkSync::SendPose(Thing* thing, bool recurse) { if (networkId == 0) // We're not connected to a site yet return; @@ -217,7 +195,7 @@ void NetworkSync::SendPose(Thing* thing, bool recurse) { #if RC_DEBUG if (thing->id == 0) - printf("Sent PoseMsg Thing [%d/%d]\n", networkId, buffer[1]); + SERIALPORT.printf("Sent PoseMsg Thing [%d/%d]\n", networkId, buffer[1]); #endif } @@ -237,7 +215,7 @@ void NetworkSync::PublishClient() { SendBuffer(ix); #ifdef RC_DEBUG - printf("Sent new Client\n"); + SERIALPORT.println("Sent new Client"); #endif } @@ -293,29 +271,19 @@ void NetworkSync::PublishTrackedObject(Roboid* roboid, return; } - // if (object->parentId != 0) - // return PublishRelativeObject(object); - Spherical16 roboidPosition = roboid->GetPosition(); SwingTwist16 roboidOrientation = roboid->GetOrientation(); - // Vector3 localPosition = object->position.ToVector3(); - Vector3 localPosition = object->position.ToVector3(); Spherical16 worldPosition = - roboidPosition + roboidOrientation * object->position; // localPosition; - SwingTwist16 worldOrientation = - roboidOrientation * object->orientation; //.ToQuaternion(); + roboidPosition + roboidOrientation * object->position; + SwingTwist16 worldOrientation = roboidOrientation * object->orientation; unsigned char ix = 0; buffer[ix++] = PoseMsg; // Position2DMsg; buffer[ix++] = object->id; // objectId; buffer[ix++] = Pose_Position | Pose_Orientation; - SendSpherical16(buffer, &ix, - worldPosition); // Spherical::FromVector3(worldPosition)); - SendQuat32(buffer, &ix, worldOrientation.ToQuaternion()); - // SendPolar(buffer, &ix, polar); // 3 bytes - // SendVector3(buffer, &ix, worldPosition); - // SendQuat32(buffer, &ix, worldOrientation); + SendSpherical16(buffer, &ix, worldPosition); + SendSwingTwist(buffer, &ix, worldOrientation); SendBuffer(ix); #if RC_DEBUG @@ -323,11 +291,6 @@ void NetworkSync::PublishTrackedObject(Roboid* roboid, #endif object->updated = false; - -#if RC_DEBUG - // printf("PublishTrackedObj [%d/%d] (%f %f)\n", object->networkId, buffer[1], - // worldPosition.Right(), worldPosition.Forward()); -#endif } void NetworkSync::SendPoseMsg(Buffer sendBuffer, Roboid* roboid) { @@ -405,6 +368,9 @@ void NetworkSync::SendText(const char* s) { buffer[ix++] = s[urlIx]; SendBuffer(ix); +#ifdef RC_DEBUG + printf("Sent Text %s\n", s); +#endif } void NetworkSync::SendInt(const int x) { @@ -458,13 +424,11 @@ void NetworkSync::SendSpherical16(unsigned char* data, SendFloat16(data, startIndex, s.distance); } -// void NetworkSync::SendSpherical16(unsigned char* data, -// unsigned char* startIndex, -// Spherical16 s) { -// SendAngle8(data, (*startIndex)++, s.horizontal.ToFloat()); -// SendAngle8(data, (*startIndex)++, s.vertical.ToFloat()); -// SendFloat16(data, startIndex, s.distance); -// } +void NetworkSync::SendSwingTwist(unsigned char* data, + unsigned char* ix, + const SwingTwist16 r) { + SendQuat32(buffer, ix, r.ToQuaternion()); +} void NetworkSync::SendQuat32(unsigned char* data, unsigned char* startIndex, @@ -520,10 +484,8 @@ void NetworkSync::SendSingle100(unsigned char* data, // Sends a float with truncated 2 decimal precision Int32 intValue = value * 100; SendInt32(data, startIndex, intValue); - // for (unsigned char ix = 0; ix < 4; ix++) { - // data[startIndex + ix] = ((unsigned char *)&intValue)[ix]; - // } } + void NetworkSync::SendFloat16(unsigned char* data, unsigned char* startIndex, float value) { diff --git a/NetworkSync.h b/NetworkSync.h index 44a88f0..07ef9d2 100644 --- a/NetworkSync.h +++ b/NetworkSync.h @@ -114,13 +114,12 @@ class NetworkSync { const Quaternion q); void SendPolar(unsigned char* data, unsigned char* startIndex, Polar p); - // void SendSpherical(unsigned char* data, - // unsigned char* startIndex, - // Spherical s); void SendSpherical16(unsigned char* data, unsigned char* startIndex, Spherical16 s); - // void SendSpherical32(unsigned char *data, int startIndex, Spherical s); + void SendSwingTwist(unsigned char* data, + unsigned char* startIndex, + const SwingTwist16 r); void SendQuat32(unsigned char* data, unsigned char* startIndex, const Quaternion q); diff --git a/Roboid.cpp b/Roboid.cpp index 8f872b9..591e405 100644 --- a/Roboid.cpp +++ b/Roboid.cpp @@ -45,20 +45,19 @@ void Roboid::Update(unsigned long currentTimeMs) { propulsion->Update(currentTimeMs); float deltaTime = (float)(currentTimeMs - lastUpdateTimeMs) / 1000; - SwingTwist16 roboidOrientation = this->GetOrientation(); + SetPosition(this->worldPosition + - roboidOrientation * Spherical16::forward * + this->worldOrientation * Spherical16::forward * this->propulsion->GetVelocity().distance * deltaTime); - SetOrientation( - roboidOrientation * - SwingTwist16::AngleAxis(this->propulsion->GetAngularVelocity(), - Spherical16::up)); + SetOrientation(this->worldOrientation * + SwingTwist16::AngleAxis( + this->propulsion->GetAngularVelocity() * deltaTime, + Spherical16::up)); } if (childCount > 0 && children != nullptr) { - for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { + for (unsigned char childIx = 0; childIx < this->childCount; childIx++) children[childIx]->Update(currentTimeMs); - } } if (networkSync != nullptr)