RoboidControl/RoboidControl-cpp
RoboidControl/RoboidControl-cpp
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Minor improvements

Browse Source
This commit is contained in:
Pascal Serrarens 2024-09-20 15:14:47 +02:00
parent 7d4c26d10b
commit 9de2fa6492
5 changed files with 37 additions and 77 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
LinearAlgebra

@ -1 +1 @@
Subproject commit f1c70c756ebfd58864acbbede234e62d93cc741d
Subproject commit 1ea65d56b1ee69407ff0ac2f9e1f232fed7e57e5

4
NetworkPerception.cpp
View File

@ -2,7 +2,7 @@
#include "NetworkSync.h"
// #define RC_DEBUG true
#define RC_DEBUG true
#if RC_DEBUG
#include <Arduino.h>
#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();

86
NetworkSync.cpp
View File

@ -1,9 +1,14 @@
#include "NetworkSync.h"
// #define RC_DEBUG 1
#define RC_DEBUG 1
#ifdef RC_DEBUG
#include <Arduino.h>
#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) {

7
NetworkSync.h
View File

@ -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);

15
Roboid.cpp
View File

@ -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)