Migrated to ControlCore
This commit is contained in:
Pascal Serrarens
parent
0a155874e4
commit
12fd89f3fb
@ -1,7 +1,5 @@
|
||||
#pragma once
|
||||
|
||||
#include "Participant.h"
|
||||
|
||||
namespace Passer::Control {
|
||||
|
||||
class CoreThing {
|
||||
@ -31,3 +29,4 @@ protected:
|
||||
};
|
||||
|
||||
} // namespace Passer::Control
|
||||
using namespace Passer::Control;
|
||||
@ -7,6 +7,14 @@ void LowLevelMessages::SendAngle8(unsigned char *buffer, unsigned char *ix,
|
||||
Angle8 packedAngle2 = Angle8::Degrees(angle);
|
||||
buffer[(*ix)++] = packedAngle2.GetBinary();
|
||||
}
|
||||
Angle8 LowLevelMessages::ReceiveAngle8(const unsigned char *buffer,
|
||||
unsigned char *startIndex) {
|
||||
unsigned char binary = buffer[(*startIndex)++];
|
||||
|
||||
Angle8 angle = Angle8::Binary(binary);
|
||||
|
||||
return angle;
|
||||
}
|
||||
|
||||
void LowLevelMessages::SendFloat16(unsigned char *buffer, unsigned char *ix,
|
||||
float value) {
|
||||
@ -16,14 +24,36 @@ void LowLevelMessages::SendFloat16(unsigned char *buffer, unsigned char *ix,
|
||||
buffer[(*ix)++] = (binary >> 8) & 0xFF;
|
||||
buffer[(*ix)++] = binary & 0xFF;
|
||||
}
|
||||
float LowLevelMessages::ReceiveFloat16(const unsigned char *buffer,
|
||||
unsigned char *startIndex) {
|
||||
unsigned char ix = *startIndex;
|
||||
unsigned short value = buffer[ix++] << 8 | buffer[ix++];
|
||||
float16 f = float16();
|
||||
f.setBinary(value);
|
||||
|
||||
void Passer::Control::LowLevelMessages::SendSpherical16(unsigned char *buffer,
|
||||
unsigned char *ix,
|
||||
*startIndex = ix;
|
||||
return f.toDouble();
|
||||
}
|
||||
|
||||
void 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());
|
||||
}
|
||||
Spherical16 LowLevelMessages::ReceiveSpherical16(const unsigned char *buffer,
|
||||
unsigned char *startIndex) {
|
||||
float distance = ReceiveFloat16(buffer, startIndex);
|
||||
|
||||
Angle8 horizontal8 = ReceiveAngle8(buffer, startIndex);
|
||||
Angle16 horizontal = Angle16::Binary(horizontal8.GetBinary() * 256);
|
||||
|
||||
Angle8 vertical8 = ReceiveAngle8(buffer, startIndex);
|
||||
Angle16 vertical = Angle16::Binary(vertical8.GetBinary() * 256);
|
||||
|
||||
Spherical16 s = Spherical16(distance, horizontal, vertical);
|
||||
return s;
|
||||
}
|
||||
|
||||
void Passer::Control::LowLevelMessages::SendQuat32(unsigned char *buffer,
|
||||
unsigned char *ix,
|
||||
@ -45,3 +75,14 @@ void Passer::Control::LowLevelMessages::SendQuat32(unsigned char *buffer,
|
||||
buffer[(*ix)++] = qz;
|
||||
buffer[(*ix)++] = qw;
|
||||
}
|
||||
|
||||
SwingTwist16 LowLevelMessages::ReceiveQuat32(const unsigned char *buffer,
|
||||
unsigned char *ix) {
|
||||
float qx = (buffer[(*ix)++] - 128.0F) / 127.0F;
|
||||
float qy = (buffer[(*ix)++] - 128.0F) / 127.0F;
|
||||
float qz = (buffer[(*ix)++] - 128.0F) / 127.0F;
|
||||
float qw = buffer[(*ix)++] / 255.0F;
|
||||
Quaternion q = Quaternion(qx, qy, qz, qw);
|
||||
SwingTwist16 s = SwingTwist16::FromQuaternion(q);
|
||||
return s;
|
||||
}
|
||||
@ -7,12 +7,23 @@ class LowLevelMessages {
|
||||
public:
|
||||
static void SendAngle8(unsigned char *buffer, unsigned char *ix,
|
||||
const float angle);
|
||||
static Angle8 ReceiveAngle8(const unsigned char *buffer,
|
||||
unsigned char *startIndex);
|
||||
|
||||
static void SendFloat16(unsigned char *buffer, unsigned char *ix,
|
||||
float value);
|
||||
static float ReceiveFloat16(const unsigned char *buffer,
|
||||
unsigned char *startIndex);
|
||||
|
||||
static void SendSpherical16(unsigned char *buffer, unsigned char *ix,
|
||||
Spherical16 s);
|
||||
static Spherical16 ReceiveSpherical16(const unsigned char *buffer,
|
||||
unsigned char *startIndex);
|
||||
|
||||
static void SendQuat32(unsigned char *buffer, unsigned char *ix,
|
||||
SwingTwist16 q);
|
||||
static SwingTwist16 ReceiveQuat32(const unsigned char *buffer,
|
||||
unsigned char *ix);
|
||||
};
|
||||
|
||||
} // namespace Passer::Control
|
||||
|
||||
@ -1,21 +1,81 @@
|
||||
#include "Messages.h"
|
||||
|
||||
#include "LowLevelMessages.h"
|
||||
#include "Participant.h"
|
||||
#include "string.h"
|
||||
|
||||
#pragma region IMessage
|
||||
|
||||
IMessage::IMessage() {}
|
||||
|
||||
// IMessage::IMessage(unsigned char *buffer) { Deserialize(buffer); }
|
||||
|
||||
unsigned char IMessage::Serialize(unsigned char *buffer) { return 0; }
|
||||
|
||||
bool IMessage::SendMsg(Participant *client, IMessage msg) {
|
||||
// return SendMsg(client, client.buffer, );nameLength
|
||||
return client->SendBuffer(msg.Serialize(client->buffer));
|
||||
// void IMessage::Deserialize(unsigned char *buffer) {}
|
||||
|
||||
// bool IMessage::SendMsg(Participant *client, IMessage msg) {
|
||||
// // return SendMsg(client, client.buffer, );nameLength
|
||||
// return client->SendBuffer(msg.Serialize(client->buffer));
|
||||
// }
|
||||
|
||||
unsigned char *IMessage::ReceiveMsg(unsigned char packetSize) {
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
bool IMessage::Publish(Participant *participant) {
|
||||
return participant->PublishBuffer(Serialize(participant->buffer));
|
||||
}
|
||||
bool IMessage::Send(Participant *participant) {
|
||||
return participant->SendBuffer(Serialize(participant->buffer));
|
||||
}
|
||||
|
||||
// IMessage
|
||||
#pragma endregion
|
||||
|
||||
#pragma region Client
|
||||
|
||||
ClientMsg::ClientMsg(unsigned char networkId) { this->networkId = networkId; }
|
||||
|
||||
unsigned char ClientMsg::Serialize(unsigned char *buffer) {
|
||||
unsigned char ix = 0;
|
||||
buffer[ix++] = this->id;
|
||||
buffer[ix++] = this->networkId;
|
||||
return ix;
|
||||
}
|
||||
|
||||
// bool ClientMsg::Send(Participant *participant, unsigned char networkId) {
|
||||
// ClientMsg msg = ClientMsg()
|
||||
// }
|
||||
// Client Msg
|
||||
#pragma endregion
|
||||
|
||||
#pragma region Network Id
|
||||
|
||||
NetworkIdMsg::NetworkIdMsg(unsigned char *buffer) {
|
||||
this->networkId = buffer[1];
|
||||
}
|
||||
|
||||
// void NetworkIdMsg::Deserialize(unsigned char *buffer) {
|
||||
// this->networkId = buffer[1];
|
||||
// }
|
||||
|
||||
NetworkIdMsg NetworkIdMsg::Receive(unsigned char *buffer,
|
||||
unsigned char bufferSize) {
|
||||
NetworkIdMsg msg = NetworkIdMsg(buffer);
|
||||
return msg;
|
||||
}
|
||||
|
||||
// Network Id
|
||||
#pragma endregion
|
||||
|
||||
#pragma region Investigate
|
||||
|
||||
InvestigateMsg::InvestigateMsg(unsigned char *buffer) {
|
||||
unsigned ix = 1; // first byte is msgId
|
||||
this->networkId = buffer[ix++];
|
||||
this->thingId = buffer[ix++];
|
||||
}
|
||||
InvestigateMsg::InvestigateMsg(unsigned char networkId, unsigned char thingId) {
|
||||
this->networkId = networkId;
|
||||
this->thingId = thingId;
|
||||
@ -29,17 +89,25 @@ unsigned char InvestigateMsg::Serialize(unsigned char *buffer) {
|
||||
return ix;
|
||||
}
|
||||
|
||||
bool InvestigateMsg::Send(Participant *client, unsigned char networkId,
|
||||
unsigned char thingId) {
|
||||
InvestigateMsg msg = InvestigateMsg(networkId, thingId);
|
||||
return SendMsg(client, msg);
|
||||
}
|
||||
// bool InvestigateMsg::Send(Participant *participant, unsigned char networkId,
|
||||
// unsigned char thingId) {
|
||||
// InvestigateMsg msg = InvestigateMsg(networkId, thingId);
|
||||
// return msg.Send(participant);
|
||||
// }
|
||||
|
||||
// Investigate
|
||||
#pragma endregion
|
||||
|
||||
#pragma region Thing
|
||||
|
||||
ThingMsg::ThingMsg(const unsigned char *buffer) {
|
||||
unsigned char ix = 1; // first byte is msg id
|
||||
this->networkId = buffer[ix++];
|
||||
this->thingId = buffer[ix++];
|
||||
this->thingType = buffer[ix++];
|
||||
this->parentId = buffer[ix++];
|
||||
}
|
||||
|
||||
ThingMsg::ThingMsg(unsigned char networkId, unsigned char thingId,
|
||||
unsigned char thingType, unsigned char parentId) {
|
||||
this->networkId = networkId;
|
||||
@ -58,12 +126,12 @@ unsigned char ThingMsg::Serialize(unsigned char *buffer) {
|
||||
return ix;
|
||||
}
|
||||
|
||||
bool ThingMsg::Send(Participant *client, unsigned char networkId,
|
||||
unsigned char thingId, unsigned char thingType,
|
||||
unsigned char parentId) {
|
||||
ThingMsg msg = ThingMsg(networkId, thingId, thingType, parentId);
|
||||
return SendMsg(client, msg);
|
||||
}
|
||||
// bool ThingMsg::Send(Participant *participant, unsigned char networkId,
|
||||
// unsigned char thingId, unsigned char thingType,
|
||||
// unsigned char parentId) {
|
||||
// ThingMsg msg = ThingMsg(networkId, thingId, thingType, parentId);
|
||||
// return msg.Send(participant);
|
||||
// }
|
||||
|
||||
// Thing
|
||||
#pragma endregion
|
||||
@ -90,17 +158,17 @@ unsigned char NameMsg::Serialize(unsigned char *buffer) {
|
||||
return ix;
|
||||
}
|
||||
|
||||
bool NameMsg::Send(Participant *client, CoreThing *thing,
|
||||
unsigned char nameLength) {
|
||||
if (thing->name == nullptr)
|
||||
return true; // nothing sent, but still a success!
|
||||
// bool NameMsg::Send(Participant *participant, CoreThing *thing,
|
||||
// unsigned char nameLength) {
|
||||
// if (thing->name == nullptr)
|
||||
// return true; // nothing sent, but still a success!
|
||||
|
||||
if (strlen(thing->name) == 0)
|
||||
return true;
|
||||
// if (strlen(thing->name) == 0)
|
||||
// return true;
|
||||
|
||||
NameMsg msg = NameMsg(thing->networkId, thing->id, thing->name, nameLength);
|
||||
return SendMsg(client, msg);
|
||||
}
|
||||
// NameMsg msg = NameMsg(thing->networkId, thing->id, thing->name,
|
||||
// nameLength); return msg.Send(participant);
|
||||
// }
|
||||
|
||||
// Name
|
||||
#pragma endregion
|
||||
@ -134,7 +202,6 @@ unsigned char ModelUrlMsg::Serialize(unsigned char *buffer) {
|
||||
|
||||
#pragma region PoseMsg
|
||||
|
||||
#include <Arduino.h>
|
||||
PoseMsg::PoseMsg(unsigned char networkId, unsigned char thingId,
|
||||
unsigned char poseType, Spherical16 position,
|
||||
SwingTwist16 orientation) {
|
||||
@ -144,6 +211,14 @@ PoseMsg::PoseMsg(unsigned char networkId, unsigned char thingId,
|
||||
this->orientation = orientation;
|
||||
this->poseType = poseType;
|
||||
}
|
||||
PoseMsg::PoseMsg(const unsigned char *buffer) {
|
||||
unsigned char ix = 1; // First byte is msg id
|
||||
this->networkId = buffer[ix++];
|
||||
this->thingId = buffer[ix++];
|
||||
this->poseType = buffer[ix++];
|
||||
this->position = LowLevelMessages::ReceiveSpherical16(buffer, &ix);
|
||||
this->orientation = LowLevelMessages::ReceiveQuat32(buffer, &ix);
|
||||
}
|
||||
|
||||
unsigned char PoseMsg::Serialize(unsigned char *buffer) {
|
||||
unsigned char ix = 0;
|
||||
@ -152,11 +227,66 @@ unsigned char PoseMsg::Serialize(unsigned char *buffer) {
|
||||
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
|
||||
|
||||
#pragma region CustomMsg
|
||||
|
||||
CustomMsg::CustomMsg(unsigned char *buffer) {
|
||||
unsigned char ix;
|
||||
this->networkId = buffer[ix++];
|
||||
this->thingId = buffer[ix++];
|
||||
this->dataSize = buffer[ix++];
|
||||
this->data =
|
||||
buffer + ix; // This is only valid because the code ensures the the msg
|
||||
// lifetime is shorter than the buffer lifetime...
|
||||
}
|
||||
|
||||
CustomMsg::CustomMsg(unsigned char networkId, unsigned char thingId,
|
||||
unsigned char *data, unsigned char dataSize) {
|
||||
this->networkId = networkId;
|
||||
this->thingId = thingId;
|
||||
this->dataSize = dataSize;
|
||||
this->data = data;
|
||||
}
|
||||
|
||||
unsigned char CustomMsg::Serialize(unsigned char *buffer) {
|
||||
unsigned char ix = 0;
|
||||
buffer[ix++] = this->id;
|
||||
buffer[ix++] = this->networkId;
|
||||
buffer[ix++] = this->thingId;
|
||||
for (int dataIx = 0; dataIx < this->dataSize; dataIx++)
|
||||
buffer[ix++] = this->data[dataIx];
|
||||
return ix;
|
||||
}
|
||||
|
||||
// void CustomMsg::Deserialize(unsigned char *buffer) {
|
||||
// unsigned char ix;
|
||||
// this->networkId = buffer[ix++];
|
||||
// this->thingId = buffer[ix++];
|
||||
// this->dataSize = buffer[ix++];
|
||||
// this->data = buffer + ix; // challenging: point directly into the buffer!
|
||||
|
||||
// // this->data = new unsigned char[this->dataSize]; // memory leak!
|
||||
// // for (unsigned char dataIx = 0; dataIx < this->dataSize; dataIx++)
|
||||
// // this->data[dataIx] = buffer[ix++];
|
||||
// }
|
||||
|
||||
// bool CustomMsg::Send(Participant *participant, unsigned char networkId,
|
||||
// unsigned char thingId, unsigned char *data,
|
||||
// unsigned char dataSize) {
|
||||
// CustomMsg msg = CustomMsg(networkId, thingId, data, dataSize);
|
||||
// return msg.Send(participant);
|
||||
// }
|
||||
|
||||
CustomMsg CustomMsg::Receive(unsigned char *buffer, unsigned char bufferSize) {
|
||||
CustomMsg msg = CustomMsg(buffer);
|
||||
return msg;
|
||||
}
|
||||
|
||||
// CustomMsg
|
||||
#pragma endregion
|
||||
|
||||
@ -1,17 +1,43 @@
|
||||
#pragma once
|
||||
|
||||
#include "../LinearAlgebra/Spherical.h"
|
||||
#include "../LinearAlgebra/SwingTwist.h"
|
||||
#include "../float16/float16.h"
|
||||
#include "CoreThing.h"
|
||||
#include "Participant.h"
|
||||
|
||||
namespace Passer::Control {
|
||||
|
||||
class Participant;
|
||||
|
||||
class IMessage {
|
||||
public:
|
||||
IMessage();
|
||||
virtual unsigned char Serialize(unsigned char *buffer);
|
||||
|
||||
static bool SendMsg(Participant *client, IMessage msg);
|
||||
static unsigned char *ReceiveMsg(unsigned char packetSize);
|
||||
|
||||
bool Publish(Participant *participant);
|
||||
bool Send(Participant *participant);
|
||||
};
|
||||
|
||||
class ClientMsg : public IMessage {
|
||||
public:
|
||||
static const unsigned char id = 0xA0;
|
||||
unsigned char networkId;
|
||||
|
||||
ClientMsg(unsigned char networkId);
|
||||
virtual unsigned char Serialize(unsigned char *buffer) override;
|
||||
};
|
||||
|
||||
class NetworkIdMsg : public IMessage {
|
||||
public:
|
||||
static const unsigned char id = 0xA1;
|
||||
static const unsigned char length = 2;
|
||||
unsigned char networkId;
|
||||
|
||||
NetworkIdMsg(unsigned char *buffer);
|
||||
|
||||
static NetworkIdMsg Receive(unsigned char *buffer, unsigned char bufferSize);
|
||||
};
|
||||
|
||||
class InvestigateMsg : public IMessage {
|
||||
@ -21,12 +47,10 @@ public:
|
||||
unsigned char networkId;
|
||||
unsigned char thingId;
|
||||
|
||||
InvestigateMsg(unsigned char *buffer);
|
||||
InvestigateMsg(unsigned char networkId, unsigned char thingId);
|
||||
|
||||
virtual unsigned char Serialize(unsigned char *buffer) override;
|
||||
|
||||
static bool Send(Participant *client, unsigned char networkId,
|
||||
unsigned char thingId);
|
||||
};
|
||||
|
||||
class ThingMsg : public IMessage {
|
||||
@ -38,14 +62,11 @@ public:
|
||||
unsigned char thingType;
|
||||
unsigned char parentId;
|
||||
|
||||
ThingMsg(const unsigned char *buffer);
|
||||
ThingMsg(unsigned char networkId, unsigned char thingId,
|
||||
unsigned char thingType, unsigned char parentId);
|
||||
|
||||
virtual unsigned char Serialize(unsigned char *buffer) override;
|
||||
|
||||
static bool Send(Participant *client, unsigned char networkId,
|
||||
unsigned char thingId, unsigned char thingType,
|
||||
unsigned char parentId);
|
||||
};
|
||||
|
||||
class NameMsg : public IMessage {
|
||||
@ -61,9 +82,6 @@ public:
|
||||
unsigned char nameLength);
|
||||
|
||||
virtual unsigned char Serialize(unsigned char *buffer) override;
|
||||
|
||||
static bool Send(Participant *client, CoreThing *thing,
|
||||
unsigned char nameLength);
|
||||
};
|
||||
|
||||
class ModelUrlMsg : public IMessage {
|
||||
@ -92,8 +110,10 @@ public:
|
||||
unsigned char thingId;
|
||||
|
||||
unsigned char poseType;
|
||||
const unsigned char Pose_Position = 0x01;
|
||||
const unsigned char Pose_Orientation = 0x02;
|
||||
static const unsigned char Pose_Position = 0x01;
|
||||
static const unsigned char Pose_Orientation = 0x02;
|
||||
static const unsigned char Pose_LinearVelocity = 0x04; // For future use
|
||||
static const unsigned char Pose_AngularVelocity = 0x08; // For future use
|
||||
|
||||
Spherical16 position;
|
||||
SwingTwist16 orientation;
|
||||
@ -101,9 +121,31 @@ public:
|
||||
PoseMsg(unsigned char networkId, unsigned char thingId,
|
||||
unsigned char poseType, Spherical16 position,
|
||||
SwingTwist16 orientation);
|
||||
PoseMsg(const unsigned char *buffer);
|
||||
|
||||
virtual unsigned char Serialize(unsigned char *buffer) override;
|
||||
};
|
||||
|
||||
class CustomMsg : public IMessage {
|
||||
public:
|
||||
static const unsigned char id = 0xB1;
|
||||
static const unsigned length = 3;
|
||||
|
||||
unsigned char networkId;
|
||||
unsigned char thingId;
|
||||
|
||||
unsigned char dataSize;
|
||||
unsigned char *data;
|
||||
|
||||
CustomMsg(unsigned char *buffer);
|
||||
CustomMsg(unsigned char networkId, unsigned char thingId, unsigned char *data,
|
||||
unsigned char dataSize);
|
||||
|
||||
virtual unsigned char Serialize(unsigned char *buffer) override;
|
||||
|
||||
static CustomMsg Receive(unsigned char *buffer, unsigned char bufferSize);
|
||||
};
|
||||
|
||||
} // namespace Passer::Control
|
||||
|
||||
using namespace Passer::Control;
|
||||
@ -1,10 +1,40 @@
|
||||
#include "Participant.h"
|
||||
|
||||
// bool Passer::Control::ControlClient::SendMsg(unsigned char *buffer,
|
||||
// unsigned char bufferSize) {
|
||||
// return false;
|
||||
// }
|
||||
|
||||
bool Participant::SendBuffer(unsigned char bufferSize) { return false; }
|
||||
|
||||
bool Participant::PublishBuffer(unsigned char bufferSize) { return false; }
|
||||
|
||||
void Participant::ReceiveData(unsigned char bufferSize) {
|
||||
unsigned char msgId = this->buffer[0];
|
||||
switch (msgId) {
|
||||
case NetworkIdMsg::id: {
|
||||
// NetworkIdMsg msg = NetworkIdMsg::Receive(this->buffer, bufferSize);
|
||||
NetworkIdMsg msg = NetworkIdMsg(this->buffer);
|
||||
ProcessNetworkIdMsg(msg);
|
||||
} break;
|
||||
case InvestigateMsg::id: {
|
||||
InvestigateMsg msg = InvestigateMsg(this->buffer);
|
||||
ProcessInvestigateMsg(msg);
|
||||
} break;
|
||||
case ThingMsg::id: {
|
||||
ThingMsg msg = ThingMsg(this->buffer);
|
||||
ProcessThingMsg(msg);
|
||||
} break;
|
||||
case PoseMsg::id: {
|
||||
PoseMsg msg = PoseMsg(this->buffer);
|
||||
ProcessPoseMsg(msg);
|
||||
} break;
|
||||
case CustomMsg::id: {
|
||||
CustomMsg msg = CustomMsg(this->buffer);
|
||||
ProcessCustomMsg(msg);
|
||||
} break;
|
||||
};
|
||||
}
|
||||
|
||||
void Participant::ProcessNetworkIdMsg(NetworkIdMsg msg) {}
|
||||
|
||||
void Participant::ProcessInvestigateMsg(InvestigateMsg msg) {}
|
||||
|
||||
void Participant::ProcessThingMsg(ThingMsg msg) {}
|
||||
|
||||
void Participant::ProcessCustomMsg(CustomMsg msg) {}
|
||||
@ -1,15 +1,24 @@
|
||||
#pragma once
|
||||
|
||||
#include "Messages.h"
|
||||
|
||||
namespace Passer::Control {
|
||||
|
||||
class Participant {
|
||||
public:
|
||||
// unsigned char *buffer;
|
||||
// bool SendMsg(unsigned char *buffer, unsigned char bufferSize);
|
||||
|
||||
unsigned char buffer[1024];
|
||||
|
||||
virtual bool SendBuffer(unsigned char bufferSize);
|
||||
virtual bool PublishBuffer(unsigned char bufferSize);
|
||||
|
||||
void ReceiveData(unsigned char bufferSize);
|
||||
|
||||
protected:
|
||||
virtual void ProcessNetworkIdMsg(NetworkIdMsg msg);
|
||||
virtual void ProcessInvestigateMsg(InvestigateMsg msg);
|
||||
virtual void ProcessThingMsg(ThingMsg msg);
|
||||
virtual void ProcessPoseMsg(PoseMsg msg);
|
||||
virtual void ProcessCustomMsg(CustomMsg msg);
|
||||
};
|
||||
|
||||
} // namespace Passer::Control
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
#include "DirectionalSensor.h"
|
||||
|
||||
#include "Messages.h"
|
||||
#include "ControlCore/LowLevelMessages.h"
|
||||
|
||||
DirectionalSensor::DirectionalSensor() : Sensor() {
|
||||
this->type = DirectionalSensor::Type;
|
||||
@ -11,5 +11,5 @@ Spherical16 DirectionalSensor::GetVector() { return Spherical16::zero; }
|
||||
|
||||
void DirectionalSensor::ProcessBytes(unsigned char *bytes) {
|
||||
unsigned char ix = 0;
|
||||
this->vector = Messages::ReceiveSpherical16(bytes, &ix);
|
||||
this->vector = LowLevelMessages::ReceiveSpherical16(bytes, &ix);
|
||||
}
|
||||
|
||||
@ -1,7 +1,7 @@
|
||||
#include "DistanceSensor.h"
|
||||
|
||||
// #include "NetworkPerception.h"
|
||||
#include "Messages.h"
|
||||
#include "ControlCore/LowLevelMessages.h"
|
||||
#include <math.h>
|
||||
|
||||
DistanceSensor::DistanceSensor() {
|
||||
@ -33,5 +33,5 @@ bool DistanceSensor::ObjectNearby() {
|
||||
|
||||
void DistanceSensor::ProcessBytes(unsigned char *bytes) {
|
||||
unsigned char ix = 0;
|
||||
this->distance = Messages::ReceiveFloat16(bytes, &ix);
|
||||
this->distance = LowLevelMessages::ReceiveFloat16(bytes, &ix);
|
||||
}
|
||||
|
||||
146
Messages.cpp
146
Messages.cpp
@ -1,146 +0,0 @@
|
||||
#include "Messages.h"
|
||||
|
||||
#include "float16/float16.h"
|
||||
|
||||
Angle8 Messages::ReceiveAngle8(unsigned char *buffer,
|
||||
unsigned char *startIndex) {
|
||||
unsigned char binary = buffer[(*startIndex)++];
|
||||
|
||||
Angle8 angle = Angle8::Binary(binary);
|
||||
|
||||
return angle;
|
||||
}
|
||||
|
||||
float Messages::ReceiveFloat16(unsigned char *buffer,
|
||||
unsigned char *startIndex) {
|
||||
unsigned char ix = *startIndex;
|
||||
unsigned short value = buffer[ix++] << 8 | buffer[ix++];
|
||||
float16 f = float16();
|
||||
f.setBinary(value);
|
||||
|
||||
*startIndex = ix;
|
||||
return f.toDouble();
|
||||
}
|
||||
|
||||
Spherical16 Messages::ReceiveSpherical16(unsigned char *buffer,
|
||||
unsigned char *startIndex) {
|
||||
float distance = ReceiveFloat16(buffer, startIndex);
|
||||
|
||||
Angle8 horizontal8 = ReceiveAngle8(buffer, startIndex);
|
||||
Angle16 horizontal = Angle16::Binary(horizontal8.GetBinary() * 256);
|
||||
|
||||
Angle8 vertical8 = ReceiveAngle8(buffer, startIndex);
|
||||
Angle16 vertical = Angle16::Binary(vertical8.GetBinary() * 256);
|
||||
|
||||
Spherical16 s = Spherical16(distance, horizontal, vertical);
|
||||
return s;
|
||||
}
|
||||
|
||||
void Messages::SendVector3(unsigned char *buffer, unsigned char *startIndex,
|
||||
const Vector3 v) {
|
||||
SendSingle100(buffer, *startIndex, v.Right());
|
||||
(*startIndex) += 4;
|
||||
SendSingle100(buffer, *startIndex, v.Up());
|
||||
(*startIndex) += 4;
|
||||
SendSingle100(buffer, *startIndex, v.Forward());
|
||||
(*startIndex) += 4;
|
||||
}
|
||||
|
||||
void Messages::SendQuaternion(unsigned char *buffer, const int startIndex,
|
||||
const Quaternion q) {
|
||||
Vector3 angles = Quaternion::ToAngles(q);
|
||||
int ix = startIndex;
|
||||
SendAngle8(buffer, ix++, angles.Right());
|
||||
SendAngle8(buffer, ix++, angles.Up());
|
||||
SendAngle8(buffer, ix++, angles.Forward());
|
||||
}
|
||||
|
||||
void Messages::SendPolar(unsigned char *buffer, unsigned char *startIndex,
|
||||
Polar p) {
|
||||
SendAngle8(buffer, *startIndex, (const float)p.angle.InDegrees());
|
||||
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::SendSwingTwist(unsigned char *buffer, unsigned char *ix,
|
||||
const SwingTwist16 r) {
|
||||
Quaternion q = r.ToQuaternion();
|
||||
SendQuat32(buffer, ix, q);
|
||||
}
|
||||
|
||||
void Messages::SendQuat32(unsigned char *buffer, unsigned char *startIndex,
|
||||
const Quaternion q) {
|
||||
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[(*startIndex)++] = qx;
|
||||
buffer[(*startIndex)++] = qy;
|
||||
buffer[(*startIndex)++] = qz;
|
||||
buffer[(*startIndex)++] = qw;
|
||||
}
|
||||
|
||||
void Messages::SendAngle8(unsigned char *buffer, unsigned int startIndex,
|
||||
const float angle) {
|
||||
Angle8 packedAngle2 = Angle8::Degrees(angle);
|
||||
buffer[startIndex] = packedAngle2.GetBinary();
|
||||
}
|
||||
|
||||
// void NetworkSync::SendAngle16(unsigned char *data, unsigned int startIndex,
|
||||
// const float angle) {
|
||||
// AngleUsing<signed short> packedAngle = AngleUsing<signed short>(angle);
|
||||
// signed short value = packedAngle.GetValue();
|
||||
// data[startIndex] = value >> 8;
|
||||
// data[startIndex + 1] = value & 0xFF;
|
||||
// }
|
||||
|
||||
// void NetworkSync::SendAngle32(unsigned char *data, unsigned int startIndex,
|
||||
// const float angle) {
|
||||
// AngleUsing<signed long> packedAngle = AngleUsing<signed long>(angle);
|
||||
// unsigned long value = packedAngle.GetValue();
|
||||
// data[startIndex] = value >> 24 & 0xFF;
|
||||
// data[startIndex + 1] = value >> 16 & 0xFF;
|
||||
// data[startIndex + 2] = value >> 8 & 0xFF;
|
||||
// data[startIndex + 3] = value & 0xFF;
|
||||
// // Serial.printf(" %lu=%d:%d:%d:%d ", value, data[startIndex],
|
||||
// // data[startIndex + 1], data[startIndex + 2],
|
||||
// // data[startIndex + 3]);
|
||||
// }
|
||||
|
||||
// void Messages::SendSingle100(unsigned char *data, unsigned int startIndex,
|
||||
// float value) {
|
||||
// // Sends a float with truncated 2 decimal precision
|
||||
// Int32 intValue = value * 100;
|
||||
// SendInt32(data, startIndex, intValue);
|
||||
// }
|
||||
|
||||
void Messages::SendFloat16(unsigned char *data, unsigned char *startIndex,
|
||||
float value) {
|
||||
float16 value16 = float16(value);
|
||||
short binary = value16.getBinary();
|
||||
|
||||
data[(*startIndex)++] = (binary >> 8) & 0xFF;
|
||||
data[(*startIndex)++] = binary & 0xFF;
|
||||
}
|
||||
|
||||
// void Messages::SendInt32(unsigned char *data, unsigned int startIndex,
|
||||
// Int32 value) {
|
||||
// for (unsigned char ix = 0; ix < 4; ix++) {
|
||||
// data[startIndex++] = ((unsigned char *)&value)[ix];
|
||||
// }
|
||||
// }
|
||||
46
Messages.h
46
Messages.h
@ -1,46 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include "Roboid.h"
|
||||
|
||||
namespace Passer {
|
||||
namespace RoboidControl {
|
||||
|
||||
class Messages {
|
||||
public:
|
||||
static Angle8 ReceiveAngle8(unsigned char *buffer, unsigned char *startIndex);
|
||||
static float ReceiveFloat16(unsigned char *buffer, unsigned char *startIndex);
|
||||
static Spherical16 ReceiveSpherical16(unsigned char *buffer,
|
||||
unsigned char *startIndex);
|
||||
|
||||
static void SendSingle100(unsigned char *buffer, unsigned int startIndex,
|
||||
float value);
|
||||
static void SendFloat16(unsigned char *buffer, unsigned char *startIndex,
|
||||
float value);
|
||||
|
||||
// void SendInt32(unsigned char *buffer, unsigned int startIndex, Int32
|
||||
// value);
|
||||
static void SendAngle8(unsigned char *buffer, unsigned int startIndex,
|
||||
const float value);
|
||||
// void SendAngle16(unsigned char *buffer, unsigned int startIndex,
|
||||
// const float value);
|
||||
// void SendAngle32(unsigned char *buffer, unsigned int startIndex,
|
||||
// const float value);
|
||||
static void SendVector3(unsigned char *buffer, unsigned char *startIndex,
|
||||
const Vector3 v);
|
||||
static void SendQuaternion(unsigned char *buffer, const int startIndex,
|
||||
const Quaternion q);
|
||||
|
||||
static void SendPolar(unsigned char *buffer, unsigned char *startIndex,
|
||||
Polar p);
|
||||
// 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,
|
||||
const Quaternion q);
|
||||
};
|
||||
|
||||
} // namespace RoboidControl
|
||||
} // namespace Passer
|
||||
using namespace Passer::RoboidControl;
|
||||
@ -1,350 +0,0 @@
|
||||
#include "NetworkPerception.h"
|
||||
|
||||
#include "ControlCore/Messages.h"
|
||||
#include "NetworkSync.h"
|
||||
#include "float16/float16.h"
|
||||
|
||||
#define RC_DEBUG true
|
||||
#if RC_DEBUG
|
||||
#include <Arduino.h>
|
||||
#endif
|
||||
|
||||
void NetworkPerception::ProcessPacket(Roboid *roboid, unsigned char *buffer,
|
||||
int packetsize) {
|
||||
#if RC_DEBUG
|
||||
// printf("packet received, type = 0x%02x %d %d %d %d %d\n", buffer[0],
|
||||
// buffer[2], buffer[2], buffer[3], buffer[4], buffer[5]);
|
||||
#endif
|
||||
|
||||
switch (buffer[0]) {
|
||||
case NetworkSync::ThingMsg:
|
||||
ReceiveThingMsg(buffer, roboid);
|
||||
break;
|
||||
case NetworkSync::InvestigateMsg:
|
||||
ReceiveInvestigateMsg(buffer, roboid);
|
||||
break;
|
||||
case NetworkSync::PoseMsg:
|
||||
ReceivePoseMsg(buffer, roboid);
|
||||
break;
|
||||
case NetworkSync::PoseTypeMsg:
|
||||
ReceiveTypedObject(buffer, roboid);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void NetworkPerception::ReceiveThingMsg(unsigned char *data, Roboid *roboid) {
|
||||
unsigned char networkId = data[1];
|
||||
unsigned char objectId = data[2];
|
||||
unsigned char objectType = data[3];
|
||||
if (networkId == roboid->networkSync->networkId)
|
||||
return;
|
||||
|
||||
printf("Received create message [%d/%d]\n", networkId, objectId);
|
||||
InterestingThing *thing =
|
||||
roboid->perception->FindTrackedObject(networkId, objectId);
|
||||
if (thing != nullptr) {
|
||||
thing->type = objectType;
|
||||
}
|
||||
printf("complete\n");
|
||||
}
|
||||
|
||||
void NetworkPerception::ReceiveInvestigateMsg(unsigned char *data,
|
||||
Roboid *roboid) {
|
||||
unsigned char networkId = data[1];
|
||||
unsigned char thingId = data[2];
|
||||
|
||||
// #if RC_DEBUG
|
||||
printf("Received InvestigateMsg [%d/%d]\n", networkId, thingId);
|
||||
// #endif
|
||||
|
||||
if (networkId != roboid->networkSync->networkId)
|
||||
// We only response to investigation requests for our own objects
|
||||
return;
|
||||
|
||||
if (thingId == 0x00) {
|
||||
// They are investigating the roboid itself!
|
||||
if (roboid->modelUrl != nullptr)
|
||||
// roboid->networkSync->SendModel(roboid);
|
||||
roboid->networkSync->SendThingInfo(roboid);
|
||||
} else {
|
||||
Thing *thing = roboid->FindChild(thingId, true);
|
||||
if (thing != nullptr)
|
||||
roboid->networkSync->SendThingInfo(thing);
|
||||
else {
|
||||
InterestingThing *interestingThing =
|
||||
roboid->perception->FindTrackedObject(0x00, thingId);
|
||||
if (interestingThing == nullptr)
|
||||
return;
|
||||
|
||||
roboid->networkSync->SendThingInfo(interestingThing, false);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void NetworkPerception::ReceivePlane(unsigned char *data, Roboid *roboid) {
|
||||
unsigned char networkId = data[1];
|
||||
unsigned char poseType = data[3];
|
||||
if ((poseType & NetworkSync::Pose_Position) == 0 ||
|
||||
(poseType & NetworkSync::Pose_Orientation) == 0)
|
||||
|
||||
// both position and orientation are required
|
||||
return;
|
||||
|
||||
Spherical16 worldPosition = Spherical16::FromVector3(ReceiveVector3(data, 4));
|
||||
/*
|
||||
Spherical16 roboidPosition = roboid->GetPosition();
|
||||
Spherical16 deltaPosition = worldPosition - roboidPosition;
|
||||
|
||||
float distance = deltaPosition.distance; // magnitude();
|
||||
if (roboid->perception->IsInteresting(distance) == false) {
|
||||
// printf(" plane not interesting\n");
|
||||
return;
|
||||
}
|
||||
|
||||
SwingTwist16 roboidOrientation = roboid->GetOrientation();
|
||||
Spherical16 localPosition =
|
||||
SwingTwist16::Inverse(roboidOrientation) * deltaPosition;
|
||||
// float angle =
|
||||
// Vector3::SignedAngle(Vector3::forward, localPosition, Vector3::up);
|
||||
|
||||
// Polar position = Polar(angle, distance);
|
||||
Spherical16 position =
|
||||
localPosition; // Spherical16::FromVector3(localPosition);
|
||||
*/
|
||||
SwingTwist16 originOrientation;
|
||||
Spherical16 originPosition;
|
||||
if (roboid->worldOrigin == nullptr) {
|
||||
originOrientation = SwingTwist16::identity;
|
||||
originPosition = Spherical16::zero;
|
||||
} else {
|
||||
originOrientation = roboid->worldOrigin->orientation;
|
||||
originPosition = roboid->worldOrigin->position;
|
||||
}
|
||||
Spherical16 position = originPosition + originOrientation * worldPosition;
|
||||
// printf("Received plane (%f %f %f) (%f %f %f) %f %f %f\n",
|
||||
// worldPosition.x,
|
||||
// worldPosition.y, worldPosition.z, roboidPosition.x,
|
||||
// roboidPosition.y, roboidPosition.z, position.distance,
|
||||
// (float)position.horizontalAngle, (float)position.verticalAngle);
|
||||
// printf("Received plane %f (%f %f)\n", position.distance,
|
||||
// (float)position.horizontalAngle, (float)position.verticalAngle);
|
||||
roboid->perception->AddTrackedObject(this, position, SwingTwist16::identity,
|
||||
0x80, 0x80, networkId);
|
||||
}
|
||||
|
||||
void NetworkPerception::ReceiveSphere(unsigned char *data, Roboid *roboid) {
|
||||
unsigned char networkId = data[1];
|
||||
|
||||
float radius = ReceiveFloat100(data, 3);
|
||||
|
||||
Spherical16 worldPosition = Spherical16::FromVector3(ReceiveVector3(data, 7));
|
||||
/*
|
||||
Spherical16 roboidPosition = roboid->GetPosition();
|
||||
Spherical16 deltaPosition = worldPosition - roboidPosition;
|
||||
|
||||
SwingTwist16 roboidOrientation = roboid->GetOrientation();
|
||||
Spherical16 localPosition =
|
||||
SwingTwist16::Inverse(roboidOrientation) * deltaPosition;
|
||||
|
||||
Spherical16 position =
|
||||
localPosition; // Spherical16::FromVector3(localPosition);
|
||||
*/
|
||||
SwingTwist16 originOrientation;
|
||||
Spherical16 originPosition;
|
||||
if (roboid->worldOrigin == nullptr) {
|
||||
originOrientation = SwingTwist16::identity;
|
||||
originPosition = Spherical16::zero;
|
||||
} else {
|
||||
originOrientation = roboid->worldOrigin->orientation;
|
||||
originPosition = roboid->worldOrigin->position;
|
||||
}
|
||||
Spherical16 position = originPosition + originOrientation * worldPosition;
|
||||
|
||||
roboid->perception->AddTrackedObject(this, position, SwingTwist16::identity,
|
||||
0x81, 0x81, networkId);
|
||||
}
|
||||
|
||||
void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) {
|
||||
if (roboid->networkSync->networkId == 0) // We're not connected to a site yet
|
||||
return;
|
||||
|
||||
unsigned char networkId = data[1];
|
||||
unsigned char objectId = data[2];
|
||||
unsigned char poseType = data[3];
|
||||
|
||||
if (networkId == roboid->networkSync->networkId)
|
||||
networkId = 0x00;
|
||||
|
||||
// if (objectId == 0x80)
|
||||
// return ReceivePlane(data, roboid);
|
||||
// else if (objectId == 0x81)
|
||||
// return ReceiveSphere(data, roboid);
|
||||
|
||||
#if RC_DEBUG
|
||||
// printf("Received PoseMsg [%d/%d]\n", networkId, objectId);
|
||||
#endif
|
||||
|
||||
// InterestingThing *thing =
|
||||
// roboid->perception->FindTrackedObject(networkId, objectId);
|
||||
// if (thing == nullptr) {
|
||||
// thing = roboid->perception->AddTrackedObject(this, position, orientation,
|
||||
// 0xFF, objectId, networkId);
|
||||
// if (thing->networkId != 0x00) {
|
||||
// // Unknown thing
|
||||
// InvestigateMsg::Send(thing);
|
||||
// // roboid->networkSync->SendInvestigate(thing);
|
||||
// }
|
||||
// }
|
||||
|
||||
// SwingTwist16 roboidOrientation = roboid->GetOrientation();
|
||||
// Spherical16 position = Spherical16::zero;
|
||||
// SwingTwist16 orientation = SwingTwist16::identity;
|
||||
|
||||
// Vector3 worldAngles = ReceiveVector3(data, 16);
|
||||
// SwingTwist16 worldOrientation = SwingTwist16(
|
||||
// Angle16::Degrees(worldAngles.Up()),
|
||||
// Angle16::Degrees(worldAngles.Right()), Angle16::Degrees(
|
||||
// worldAngles.Forward())); // Quaternion::Euler(worldAngles);
|
||||
|
||||
// if ((poseType & NetworkSync::Pose_Orientation) != 0) {
|
||||
// if (objectId == 0) {
|
||||
// // roboid->SetOrientation(worldOrientation);
|
||||
// if (roboid->worldOrigin == nullptr) {
|
||||
// printf("creating new origin\n");
|
||||
// roboid->worldOrigin = new Thing(0);
|
||||
// printf("created\n");
|
||||
// }
|
||||
|
||||
// roboid->worldOrigin->orientation =
|
||||
// SwingTwist16::Inverse(worldOrientation);
|
||||
// } else {
|
||||
// // orientation = SwingTwist16::Inverse(roboidOrientation) *
|
||||
// // worldOrientation; if (thing != nullptr) {
|
||||
// // thing->orientation = orientation;
|
||||
// // }
|
||||
// SwingTwist16 originOrientation;
|
||||
// Spherical16 originPosition;
|
||||
// if (roboid->worldOrigin == nullptr) {
|
||||
// originOrientation = SwingTwist16::identity;
|
||||
// originPosition = Spherical16::zero;
|
||||
// } else {
|
||||
// originOrientation = roboid->worldOrigin->orientation;
|
||||
// originPosition = roboid->worldOrigin->position;
|
||||
// }
|
||||
|
||||
// thing->orientation = originOrientation * worldOrientation;
|
||||
// }
|
||||
// }
|
||||
|
||||
// if ((poseType & NetworkSync::Pose_Position) != 0) {
|
||||
// Vector3 worldVector3 = ReceiveVector3(data, 4);
|
||||
// Spherical16 worldPosition = Spherical16::FromVector3(worldVector3);
|
||||
// if (objectId == 0) {
|
||||
// // roboid->SetPosition(worldPosition);
|
||||
// if (roboid->worldOrigin == nullptr) {
|
||||
// printf("creating new origin again\n");
|
||||
// roboid->worldOrigin = new Thing(0);
|
||||
// }
|
||||
|
||||
// roboid->worldOrigin->position =
|
||||
// roboid->worldOrigin->orientation * -worldPosition;
|
||||
// } else {
|
||||
// SwingTwist16 originOrientation;
|
||||
// Spherical16 originPosition;
|
||||
// if (roboid->worldOrigin == nullptr) {
|
||||
// originOrientation = SwingTwist16::identity;
|
||||
// originPosition = Spherical16::zero;
|
||||
// } else {
|
||||
// originOrientation = roboid->worldOrigin->orientation;
|
||||
// originPosition = roboid->worldOrigin->position;
|
||||
// }
|
||||
|
||||
// // SwingTwist16 roboidLocalOrientation =
|
||||
// // originOrientation * worldOrientation;
|
||||
|
||||
// thing->position = originPosition + originOrientation * worldPosition;
|
||||
|
||||
// /*
|
||||
// Spherical16 roboidPosition = roboid->GetPosition();
|
||||
|
||||
// // float distance = Vector3::Distance(roboidPosition, worldPosition);
|
||||
// // if (roboid->perception->IsInteresting(distance) == false) {
|
||||
// // // printf(" not interesting\n");
|
||||
// // return;
|
||||
// // }
|
||||
|
||||
// Spherical16 localPosition = SwingTwist16::Inverse(roboidOrientation) *
|
||||
// (worldPosition - roboidPosition);
|
||||
// Vector3 roboidVector3 = roboidPosition.ToVector3();
|
||||
// printf(" roboid position (%f %f %f)\n", roboidVector3.Right(),
|
||||
// roboidVector3.Up(), roboidVector3.Forward());
|
||||
// printf(" [%d/%d] worldPosition (%f %f %f) localPosition %f (%f %f)\n
|
||||
// ",
|
||||
// networkId, objectId, worldVector3.Right(), worldVector3.Up(),
|
||||
// worldVector3.Forward(), localPosition.distance,
|
||||
// localPosition.direction.horizontal.InDegrees(),
|
||||
// localPosition.direction.vertical.InDegrees());
|
||||
|
||||
// thing->position = localPosition;
|
||||
// */
|
||||
// }
|
||||
// }
|
||||
}
|
||||
|
||||
void NetworkPerception::ReceiveTypedObject(unsigned char *data,
|
||||
Roboid *roboid) {
|
||||
unsigned char objectType = data[1];
|
||||
unsigned char objectId = data[2];
|
||||
if (objectType == 0x02) { // lighthouse
|
||||
// We require position and orientation for lighthouses
|
||||
if (data[3] !=
|
||||
(NetworkSync::Pose_Position | NetworkSync::Pose_Orientation)) {
|
||||
return;
|
||||
}
|
||||
|
||||
Vector3 worldPosition = ReceiveVector3(data, 4);
|
||||
Vector3 worldAngles = ReceiveVector3(data, 16);
|
||||
Quaternion worldOrientation = Quaternion::Euler(worldAngles);
|
||||
|
||||
// roboid->perception->AddTrackedObject(this, objectType, worldPosition,
|
||||
// worldOrientation);
|
||||
// Sensor *sensor =
|
||||
// roboid->perception->FindSensorOfType(0x82); // Lighthouse type
|
||||
// sensor
|
||||
// LighthouseTracker *lhSensor = (LighthouseTracker *)sensor;
|
||||
// lhSensor->UpdateGeometry(objectId, worldPosition, worldOrientation);
|
||||
}
|
||||
}
|
||||
|
||||
Int32 NetworkPerception::ReceiveInt32(unsigned char *data, int startIndex) {
|
||||
Int32 a = Int32((UInt32)(data[startIndex + 3]) << 24 |
|
||||
(UInt32)(data[startIndex + 2]) << 16 |
|
||||
(UInt32)(data[startIndex + 1]) << 8 |
|
||||
(UInt32)(data[startIndex + 0]));
|
||||
return a;
|
||||
}
|
||||
|
||||
float NetworkPerception::ReceiveFloat100(unsigned char *data, int startIndex) {
|
||||
Int32 intValue = ReceiveInt32(data, startIndex);
|
||||
float f = (float)intValue / 100.0F;
|
||||
return f;
|
||||
}
|
||||
|
||||
Vector3 NetworkPerception::ReceiveVector3(unsigned char *data, int startIndex) {
|
||||
float x = ReceiveFloat100(data, startIndex);
|
||||
float y = ReceiveFloat100(data, startIndex + 4);
|
||||
float z = ReceiveFloat100(data, startIndex + 8);
|
||||
Vector3 v = Vector3(x, y, z);
|
||||
return v;
|
||||
}
|
||||
|
||||
float NetworkPerception::ReceiveFloat16(unsigned char *data,
|
||||
unsigned char *startIndex) {
|
||||
byte ix = *startIndex;
|
||||
unsigned short value = data[ix++] << 8 | data[ix];
|
||||
float16 f = float16();
|
||||
f.setBinary(value);
|
||||
|
||||
*startIndex = ix;
|
||||
return f.toDouble();
|
||||
}
|
||||
@ -1,32 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include "Roboid.h"
|
||||
#include "Sensor.h"
|
||||
#include "Types.h"
|
||||
|
||||
namespace Passer {
|
||||
namespace RoboidControl {
|
||||
|
||||
class NetworkPerception : public Sensor {
|
||||
public:
|
||||
void ProcessPacket(Roboid *roboid, unsigned char *buffer, int packetsize);
|
||||
|
||||
protected:
|
||||
void ReceiveThingMsg(unsigned char *data, Roboid *roboid);
|
||||
void ReceiveInvestigateMsg(unsigned char *data, Roboid *roboid);
|
||||
void ReceivePoseMsg(unsigned char *data, Roboid *roboid);
|
||||
void ReceiveTypedObject(unsigned char *data, Roboid *roboid);
|
||||
|
||||
void ReceivePlane(unsigned char *data, Roboid *roboid);
|
||||
void ReceiveSphere(unsigned char *data, Roboid *roboid);
|
||||
|
||||
Int32 ReceiveInt32(unsigned char *data, int startIndex);
|
||||
float ReceiveFloat100(unsigned char *data, int startIndex);
|
||||
Vector3 ReceiveVector3(unsigned char *data, int startIndex);
|
||||
|
||||
public:
|
||||
static float ReceiveFloat16(unsigned char *data, unsigned char *startIndex);
|
||||
};
|
||||
|
||||
} // namespace RoboidControl
|
||||
} // namespace Passer
|
||||
216
NetworkSync.cpp
216
NetworkSync.cpp
@ -1,7 +1,6 @@
|
||||
#include "NetworkSync.h"
|
||||
#include "Messages.h"
|
||||
|
||||
// #define RC_DEBUG 1
|
||||
#define RC_DEBUG 1
|
||||
|
||||
#ifdef RC_DEBUG
|
||||
#include <Arduino.h>
|
||||
@ -30,29 +29,32 @@ NetworkSync::NetworkSync(Roboid *roboid) {
|
||||
void NetworkSync::ReceiveMessage(Roboid *roboid, unsigned char bytecount) {
|
||||
// printf("Received msgId %d, length %d\n", buffer[0], bytecount);
|
||||
|
||||
networkPerception->ProcessPacket(roboid, buffer, bytecount);
|
||||
|
||||
ReceiveData(bytecount);
|
||||
switch (buffer[0]) {
|
||||
case NetworkIdMsg:
|
||||
ReceiveNetworkId();
|
||||
break;
|
||||
case CustomMsg:
|
||||
case CustomMsg::id:
|
||||
ReceiveCustom(bytecount);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void NetworkSync::PublishClient() {
|
||||
unsigned char ix = 0;
|
||||
buffer[ix++] = ClientMsg;
|
||||
buffer[ix++] = this->networkId;
|
||||
PublishBuffer(ix);
|
||||
ClientMsg msg = ClientMsg(this->networkId);
|
||||
msg.Publish(this);
|
||||
|
||||
#ifdef RC_DEBUG
|
||||
SERIALPORT.printf("Publish Device\n");
|
||||
#endif
|
||||
}
|
||||
|
||||
void NetworkSync::ProcessNetworkIdMsg(Passer::Control::NetworkIdMsg msg) {
|
||||
#ifdef RC_DEBUG
|
||||
SERIALPORT.printf("Received network Id %d\n", msg.networkId);
|
||||
#endif
|
||||
this->networkId = msg.networkId;
|
||||
SendThingInfo(roboid, true);
|
||||
printf("completed\n");
|
||||
}
|
||||
|
||||
void NetworkSync::ReceiveNetworkId() {
|
||||
this->networkId = buffer[1];
|
||||
#ifdef RC_DEBUG
|
||||
@ -62,16 +64,50 @@ void NetworkSync::ReceiveNetworkId() {
|
||||
printf("completed\n");
|
||||
}
|
||||
|
||||
// void NetworkSync::SendInvestigate(InterestingThing *thing) {
|
||||
// unsigned char ix = 0;
|
||||
// buffer[ix++] = InvestigateMsg;
|
||||
// buffer[ix++] = thing->networkId;
|
||||
// buffer[ix++] = thing->id;
|
||||
// SendBuffer(ix);
|
||||
// #ifdef RC_DEBUG
|
||||
// printf("Sent Investigate [%d/%d]\n", thing->networkId, thing->id);
|
||||
void NetworkSync::ProcessInvestigateMsg(InvestigateMsg msg) {
|
||||
// #if RC_DEBUG
|
||||
printf("Received InvestigateMsg [%d/%d]\n", msg.networkId, msg.thingId);
|
||||
// #endif
|
||||
// }
|
||||
|
||||
if (networkId != roboid->networkSync->networkId)
|
||||
// We only response to investigation requests for our own objects
|
||||
return;
|
||||
|
||||
if (msg.thingId == 0x00) {
|
||||
// They are investigating the roboid itself!
|
||||
if (roboid->modelUrl != nullptr)
|
||||
// roboid->networkSync->SendModel(roboid);
|
||||
roboid->networkSync->SendThingInfo(roboid);
|
||||
} else {
|
||||
Thing *thing = roboid->FindChild(msg.thingId, true);
|
||||
if (thing != nullptr)
|
||||
roboid->networkSync->SendThingInfo(thing);
|
||||
else {
|
||||
InterestingThing *interestingThing =
|
||||
roboid->perception->FindTrackedObject(0x00, msg.thingId);
|
||||
if (interestingThing == nullptr)
|
||||
return;
|
||||
|
||||
roboid->networkSync->SendThingInfo(interestingThing, false);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void NetworkSync::ProcessThingMsg(ThingMsg msg) {
|
||||
if (msg.networkId == roboid->networkSync->networkId)
|
||||
// Do not process messages for you own things
|
||||
return;
|
||||
|
||||
printf("Received Thing message [%d/%d]\n", msg.networkId, msg.thingId);
|
||||
InterestingThing *thing =
|
||||
roboid->perception->FindTrackedObject(msg.networkId, msg.thingId);
|
||||
if (thing != nullptr) {
|
||||
thing->type = msg.thingType;
|
||||
}
|
||||
printf("complete\n");
|
||||
}
|
||||
|
||||
void NetworkSync::ProcessPoseMsg(PoseMsg msg) {}
|
||||
|
||||
void NetworkSync::ReceiveCustom(unsigned char packetSize) {
|
||||
unsigned char ix = 1; // first byte is the msgId
|
||||
@ -124,23 +160,10 @@ void NetworkSync::SendThing(Thing *thing) {
|
||||
if (thing == nullptr)
|
||||
return;
|
||||
|
||||
// unsigned char ix = 0;
|
||||
// buffer[ix++] = ThingMsg;
|
||||
// buffer[ix++] = this->networkId;
|
||||
// buffer[ix++] = thing->id;
|
||||
// buffer[ix++] = (unsigned char)thing->type;
|
||||
// Thing *parentThing = thing->GetParent();
|
||||
// if (parentThing != nullptr)
|
||||
// buffer[ix++] = parentThing->id;
|
||||
// else
|
||||
// buffer[ix++] = 0x00;
|
||||
// SendBuffer(ix);
|
||||
|
||||
Thing *parent = thing->GetParent();
|
||||
Passer::Control::ThingMsg msg =
|
||||
Passer::Control::ThingMsg(this->networkId, thing->id, thing->type,
|
||||
ThingMsg msg = ThingMsg(this->networkId, thing->id, thing->type,
|
||||
parent == nullptr ? 0 : parent->id);
|
||||
SendBuffer(msg.Serialize(this->buffer));
|
||||
msg.Send(this);
|
||||
|
||||
#ifdef RC_DEBUG
|
||||
printf("Sent Thing [%d/%d] %d\n", networkId, thing->id, (byte)thing->type);
|
||||
@ -155,21 +178,8 @@ void NetworkSync::SendName(Thing *thing) {
|
||||
if (len > 255)
|
||||
return;
|
||||
|
||||
// unsigned char ix = 0;
|
||||
// buffer[ix++] = NameMsg;
|
||||
// buffer[ix++] = this->networkId;
|
||||
// buffer[ix++] = thing->id;
|
||||
|
||||
// buffer[ix++] = len;
|
||||
// for (unsigned char nameIx = 0; nameIx < len; nameIx++)
|
||||
// buffer[ix++] = thing->name[nameIx];
|
||||
|
||||
// SendBuffer(ix);
|
||||
Passer::Control::NameMsg msg =
|
||||
Passer::Control::NameMsg(this->networkId, thing->id, thing->name, len);
|
||||
SendBuffer(msg.Serialize(this->buffer));
|
||||
|
||||
// Passer::Control::NameMsg::Send(this, thing);
|
||||
NameMsg msg = NameMsg(this->networkId, thing->id, thing->name, len);
|
||||
msg.Send(this);
|
||||
|
||||
#ifdef RC_DEBUG
|
||||
SERIALPORT.printf("Sent Name [%d/%d] %s\n", networkId, buffer[1],
|
||||
@ -185,27 +195,14 @@ void NetworkSync::SendModel(Thing *thing) {
|
||||
if (len > 255)
|
||||
return;
|
||||
|
||||
// unsigned char ix = 0;
|
||||
// buffer[ix++] = ModelMsg;
|
||||
// buffer[ix++] = this->networkId;
|
||||
// buffer[ix++] = thing->id; // objectId
|
||||
// // Spherical16 s = Spherical16::zero; // Spherical(thing->modelPosition);
|
||||
// // Messages::SendSpherical16(buffer, &ix, s);
|
||||
// Messages::SendFloat16(buffer, &ix, thing->modelScale);
|
||||
|
||||
// buffer[ix++] = len;
|
||||
// for (int urlIx = 0; urlIx < len; urlIx++)
|
||||
// buffer[ix++] = thing->modelUrl[urlIx];
|
||||
|
||||
// SendBuffer(ix);
|
||||
Passer::Control::ModelUrlMsg msg = Passer::Control::ModelUrlMsg(
|
||||
this->networkId, thing->id, len, thing->modelUrl, thing->modelScale);
|
||||
SendBuffer(msg.Serialize(this->buffer));
|
||||
ModelUrlMsg msg = ModelUrlMsg(this->networkId, thing->id, len,
|
||||
thing->modelUrl, thing->modelScale);
|
||||
msg.Send(this);
|
||||
}
|
||||
|
||||
void NetworkSync::SendCustom(Thing *thing) {
|
||||
unsigned char ix = 0;
|
||||
buffer[ix++] = NetworkSync::CustomMsg;
|
||||
buffer[ix++] = CustomMsg::id;
|
||||
buffer[ix++] = this->networkId;
|
||||
buffer[ix++] = thing->id;
|
||||
thing->SendBytes(buffer, &ix);
|
||||
@ -236,23 +233,14 @@ 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 poseType = 0;
|
||||
if (force || thing->positionUpdated)
|
||||
poseType |= Pose_Position;
|
||||
poseType |= PoseMsg::Pose_Position;
|
||||
if (force || thing->orientationUpdated)
|
||||
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));
|
||||
poseType |= PoseMsg::Pose_Orientation;
|
||||
PoseMsg msg = PoseMsg(this->networkId, thing->id, poseType, thing->position,
|
||||
thing->orientation);
|
||||
msg.Send(this);
|
||||
|
||||
thing->positionUpdated = false;
|
||||
thing->orientationUpdated = false;
|
||||
@ -278,30 +266,17 @@ void NetworkSync::SendPose(Thing *thing, bool force, bool recurse) {
|
||||
}
|
||||
}
|
||||
|
||||
void NetworkSync::PublishState(Sensor *sensor) {
|
||||
float *value = (float *)sensor->GetValue();
|
||||
if (value == nullptr)
|
||||
return;
|
||||
|
||||
unsigned char ix = 0;
|
||||
buffer[ix++] = StateMsg;
|
||||
buffer[ix++] = sensor->type;
|
||||
Messages::SendFloat16(buffer, &ix, *value);
|
||||
PublishBuffer(ix);
|
||||
}
|
||||
|
||||
void NetworkSync::PublishPerception(Roboid *roboid) {
|
||||
Perception *perception = roboid->perception;
|
||||
if (perception == nullptr)
|
||||
return;
|
||||
// Perception *perception = roboid->perception;
|
||||
// if (perception == nullptr)
|
||||
// return;
|
||||
|
||||
for (unsigned int sensorIx = 0; sensorIx < perception->sensorCount;
|
||||
sensorIx++) {
|
||||
Sensor *sensor = perception->sensors[sensorIx];
|
||||
if (sensor == nullptr)
|
||||
continue;
|
||||
PublishState(sensor);
|
||||
}
|
||||
// for (unsigned int sensorIx = 0; sensorIx < perception->sensorCount;
|
||||
// sensorIx++) {
|
||||
// Sensor *sensor = perception->sensors[sensorIx];
|
||||
// if (sensor == nullptr)
|
||||
// continue;
|
||||
// }
|
||||
PublishTrackedObjects(roboid, roboid->perception->GetTrackedObjects());
|
||||
}
|
||||
|
||||
@ -328,16 +303,6 @@ void NetworkSync::PublishTrackedObject(Roboid *roboid,
|
||||
if (thing == nullptr || thing->updated == false || thing->networkId != 0x00) {
|
||||
return;
|
||||
}
|
||||
// printf("publish tracked\n");
|
||||
/*
|
||||
Spherical16 roboidPosition = roboid->GetPosition();
|
||||
SwingTwist16 roboidOrientation = roboid->GetOrientation();
|
||||
|
||||
Spherical16 worldPosition =
|
||||
roboidPosition + roboidOrientation * object->position;
|
||||
SwingTwist16 worldOrientation = roboidOrientation * object->orientation;
|
||||
*/
|
||||
|
||||
SwingTwist16 inv_originOrientation;
|
||||
Spherical16 originPosition;
|
||||
if (roboid->worldOrigin == nullptr) {
|
||||
@ -357,17 +322,8 @@ 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);
|
||||
|
||||
Passer::Control::PoseMsg msg = Passer::Control::PoseMsg(
|
||||
this->networkId, thing->id, Pose_Position | Pose_Orientation,
|
||||
PoseMsg msg = PoseMsg(this->networkId, thing->id,
|
||||
PoseMsg::Pose_Position | PoseMsg::Pose_Orientation,
|
||||
worldPosition, worldOrientation);
|
||||
SendBuffer(msg.Serialize(this->buffer));
|
||||
|
||||
@ -414,13 +370,3 @@ void NetworkSync::SendInt(const int x) {
|
||||
|
||||
SendBuffer(ix);
|
||||
}
|
||||
|
||||
// Low-level functions
|
||||
|
||||
// bool NetworkSync::SendBuffer(unsigned char bufferSize) { return false; }
|
||||
|
||||
// bool NetworkSync::PublishBuffer(unsigned char bufferSize) { return false; }
|
||||
|
||||
// void NetworkSync::SendCBuffer(unsigned char bufferSize, unsigned char
|
||||
// *buffer) {
|
||||
//}
|
||||
@ -1,8 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "ControlCore/Messages.h"
|
||||
#include "ControlCore/Participant.h"
|
||||
#include "NetworkPerception.h"
|
||||
#include "Perception.h"
|
||||
#include "Roboid.h"
|
||||
#include "Types.h"
|
||||
@ -31,52 +29,20 @@ public:
|
||||
void SendModel(Thing *thing);
|
||||
void SendCustom(Thing *thing);
|
||||
|
||||
/// @brief The id of a Pose message
|
||||
static const unsigned char PoseMsg = 0x10;
|
||||
static const unsigned char PoseTypeMsg = 0x11;
|
||||
static const unsigned char RelativePoseMsg = 0x12;
|
||||
static const unsigned char StateMsg = 0x18;
|
||||
/// @brief A bit pattern for the pose, stating that this message contains a
|
||||
/// position in world coordinates
|
||||
static const unsigned char Pose_Position = 0x01;
|
||||
/// @brief A bit pattern for the pose, stating that this message contains an
|
||||
/// orientation in world coordinates
|
||||
static const unsigned char Pose_Orientation = 0x02;
|
||||
/// @brief A bit pattern for the pose, stating that this messsage contains a
|
||||
/// linear velocity in world coordinates
|
||||
static const unsigned char Pose_LinearVelocity = 0x04;
|
||||
/// @brief A bit pattern for the pose, stating that this message contains an
|
||||
/// angular velocity in world coordinates
|
||||
static const char Pose_AngularVelocity = 0x08;
|
||||
|
||||
static const unsigned char DestroyMsg = 0x20;
|
||||
|
||||
static const unsigned char AngVelocity2DMsg = 0x46;
|
||||
static const unsigned char Position2DMsg = 0x61;
|
||||
static const unsigned char Velocity2DMsg = 0x62;
|
||||
static const unsigned char ThingMsg = 0x80;
|
||||
static const unsigned char InvestigateMsg = 0x81;
|
||||
static const unsigned char ModelMsg = 0x90;
|
||||
static const unsigned char NameMsg = 0x91;
|
||||
static const unsigned char ClientMsg = 0xA0;
|
||||
static const unsigned char NetworkIdMsg = 0xA1;
|
||||
static const unsigned char CustomMsg = 0xB1;
|
||||
|
||||
typedef void (*Buffer)(UInt8 *buffer, UInt16 bufferSize);
|
||||
|
||||
void ReceiveMessage(Roboid *roboid, unsigned char bytecount);
|
||||
|
||||
void PublishState(Roboid *roboid);
|
||||
|
||||
// void SendInvestigate(InterestingThing *thing);
|
||||
|
||||
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 force = 0, bool recurse = true);
|
||||
|
||||
virtual void SendText(const char *s);
|
||||
@ -86,23 +52,16 @@ public:
|
||||
|
||||
protected:
|
||||
Roboid *roboid;
|
||||
NetworkPerception *networkPerception;
|
||||
|
||||
virtual void ProcessNetworkIdMsg(Passer::Control::NetworkIdMsg msg) override;
|
||||
virtual void ProcessInvestigateMsg(InvestigateMsg msg) override;
|
||||
virtual void ProcessThingMsg(ThingMsg msg) override;
|
||||
virtual void ProcessPoseMsg(PoseMsg msg) override;
|
||||
|
||||
void ReceiveNetworkId();
|
||||
void ReceiveCustom(unsigned char packetSize);
|
||||
|
||||
void PublishState(Sensor *sensor);
|
||||
|
||||
void PublishTrackedObject(Roboid *roboid, InterestingThing *object);
|
||||
// void PublishRelativeObject(Buffer sendBuffer,
|
||||
// UInt8 parentId,
|
||||
// InterestingThing* object);
|
||||
|
||||
// unsigned char buffer[256];
|
||||
// virtual bool SendBuffer(unsigned char bufferSize);
|
||||
// virtual bool PublishBuffer(unsigned char bufferSize);
|
||||
|
||||
// virtual void SendCBuffer(unsigned char bufferSize, unsigned char *buffer);
|
||||
|
||||
void PublishClient();
|
||||
};
|
||||
|
||||
11
Roboid.cpp
11
Roboid.cpp
@ -31,7 +31,6 @@ Roboid::Roboid(Propulsion *propulsion) : Roboid() {
|
||||
propulsion->roboid = this;
|
||||
}
|
||||
|
||||
#include <Arduino.h>
|
||||
void Roboid::Update(unsigned long currentTimeMs) {
|
||||
if (this->lastUpdateTimeMs == 0)
|
||||
this->lastUpdateTimeMs = currentTimeMs;
|
||||
@ -42,10 +41,6 @@ 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->angularVelocity = this->propulsion->GetAngularVelocity();
|
||||
@ -55,15 +50,9 @@ void Roboid::Update(unsigned long currentTimeMs) {
|
||||
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) {
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user