92 lines
2.4 KiB
C++
92 lines
2.4 KiB
C++
#include "Roboid.h"
|
|
|
|
#include "LinearAlgebra/FloatSingle.h"
|
|
#include "NetworkSync.h"
|
|
|
|
#include <string.h>
|
|
|
|
// #define RC_DEBUG
|
|
|
|
#ifdef RC_DEBUG
|
|
#include <Arduino.h>
|
|
#endif
|
|
|
|
Roboid::Roboid() : Thing() {
|
|
#ifdef RC_DEBUG
|
|
Serial.begin(115200);
|
|
#endif
|
|
this->type = (unsigned char)RoboidType;
|
|
this->perception = new Perception();
|
|
this->perception->roboid = this;
|
|
this->propulsion = new Propulsion();
|
|
this->networkSync = nullptr;
|
|
this->position = Spherical16::zero;
|
|
this->orientation = SwingTwist16::identity;
|
|
this->lastUpdateTimeMs = 0;
|
|
}
|
|
|
|
Roboid::Roboid(Propulsion *propulsion) : Roboid() {
|
|
this->propulsion = propulsion;
|
|
if (propulsion != nullptr)
|
|
propulsion->roboid = this;
|
|
}
|
|
|
|
void Roboid::Update(unsigned long currentTimeMs) {
|
|
if (this->lastUpdateTimeMs == 0)
|
|
this->lastUpdateTimeMs = currentTimeMs;
|
|
|
|
// if (perception != nullptr)
|
|
// perception->Update(currentTimeMs);
|
|
|
|
if (propulsion != nullptr) {
|
|
propulsion->Update(currentTimeMs);
|
|
|
|
float deltaTime = (float)(currentTimeMs - lastUpdateTimeMs) / 1000;
|
|
|
|
this->angularVelocity = this->propulsion->GetAngularVelocity();
|
|
SwingTwist16 rotation =
|
|
SwingTwist16::AngleAxis(this->angularVelocity.distance * deltaTime,
|
|
this->angularVelocity.direction);
|
|
SetOrientation(this->orientation * rotation);
|
|
|
|
this->linearVelocity = this->propulsion->GetVelocity();
|
|
Spherical16 translation =
|
|
this->orientation * this->linearVelocity * deltaTime;
|
|
SetPosition(this->position + translation);
|
|
}
|
|
|
|
if (children != nullptr) {
|
|
for (unsigned char childIx = 0; childIx < this->childCount; childIx++)
|
|
children[childIx]->Update(currentTimeMs);
|
|
}
|
|
|
|
if (networkSync != nullptr)
|
|
networkSync->NetworkUpdate(this);
|
|
|
|
lastUpdateTimeMs = currentTimeMs;
|
|
}
|
|
|
|
void Roboid::AddChild(Thing *child) {
|
|
Thing::AddChild(child);
|
|
if (child->IsSensor()) {
|
|
Sensor *childSensor = (Sensor *)child;
|
|
this->perception->AddSensor(childSensor);
|
|
}
|
|
}
|
|
|
|
void Passer::RoboidControl::Roboid::Release(Thing *child) {
|
|
if (RemoveChild(child) != nullptr) {
|
|
child->position = this->position;
|
|
child->orientation = this->orientation;
|
|
// this creates an new thing, I wish I could avoid this.
|
|
this->perception->AddTrackedObject(nullptr, child);
|
|
}
|
|
}
|
|
|
|
void Roboid::LoadModel(const char *url) {
|
|
this->modelUrl = url;
|
|
if (this->networkSync == nullptr)
|
|
return;
|
|
|
|
this->networkSync->Download(url);
|
|
} |