diff --git a/NetworkSync.h b/NetworkSync.h index 0ac983f..f732e74 100644 --- a/NetworkSync.h +++ b/NetworkSync.h @@ -42,6 +42,8 @@ public: void PublishTrackedObjects(SendBuffer sendBuffer, InterestingThing **objects); + virtual void SendPosition(Vector3 worldPosition){}; + protected: NetworkPerception *networkPerception; void PublishTrackedObject(SendBuffer sendBuffer, InterestingThing *object); diff --git a/Roboid.cpp b/Roboid.cpp index ea41c3e..f5a1384 100644 --- a/Roboid.cpp +++ b/Roboid.cpp @@ -2,6 +2,8 @@ #include "NetworkSync.h" +#define RC_DEBUG + #ifdef RC_DEBUG #include #endif @@ -10,12 +12,19 @@ Roboid::Roboid() { #ifdef RC_DEBUG Serial.begin(115200); #endif + this->perception = new Perception(); + this->perception->roboid = this; + this->propulsion = nullptr; + this->actuationRoot = nullptr; } Roboid::Roboid(Perception *perception, Propulsion *propulsion) : Roboid() { + if (this->perception != nullptr) + delete this->perception; this->perception = perception; if (perception != nullptr) perception->roboid = this; + this->propulsion = propulsion; if (propulsion != nullptr) propulsion->roboid = this; @@ -34,8 +43,10 @@ Roboid::Roboid(ServoMotor *actuationRoot) : actuationRoot(actuationRoot) {} void Roboid::Update(float currentTimeMs) { if (perception != nullptr) perception->Update(currentTimeMs); + if (propulsion != nullptr) propulsion->Update(currentTimeMs); + if (networkSync != nullptr) networkSync->NetworkUpdate(this); @@ -55,6 +66,9 @@ void Roboid::SetPosition(Vector3 newWorldPosition) { Polar polarTranslation = Polar(angle, distance); perception->UpdatePose(polarTranslation); this->worldPosition = newWorldPosition; + + if (networkSync != nullptr) + networkSync->SendPosition(this->worldPosition); } void Roboid::SetOrientation(Quaternion worldOrientation) {