63 lines
1.7 KiB
C++
63 lines
1.7 KiB
C++
#include "Roboid.h"
|
|
|
|
#include "NetworkSync.h"
|
|
|
|
#ifdef RC_DEBUG
|
|
#include <Arduino.h>
|
|
#endif
|
|
|
|
Roboid::Roboid() {
|
|
#ifdef RC_DEBUG
|
|
Serial.begin(115200);
|
|
#endif
|
|
}
|
|
|
|
Roboid::Roboid(Perception *perception, Propulsion *propulsion) : Roboid() {
|
|
this->perception = perception;
|
|
perception->roboid = this;
|
|
this->propulsion = propulsion;
|
|
propulsion->roboid = this;
|
|
}
|
|
|
|
Roboid::Roboid(Perception *perception, ServoMotor *actuationRoot) : Roboid() {
|
|
this->perception = perception;
|
|
perception->roboid = this;
|
|
this->propulsion = nullptr;
|
|
|
|
this->actuationRoot = actuationRoot;
|
|
}
|
|
|
|
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);
|
|
|
|
if (actuationRoot != nullptr)
|
|
actuationRoot->Update(currentTimeMs);
|
|
}
|
|
|
|
Vector3 Roboid::GetPosition() { return this->worldPosition; }
|
|
|
|
Quaternion Roboid::GetOrientation() { return this->worldOrientation; }
|
|
|
|
void Roboid::SetPosition(Vector3 newWorldPosition) {
|
|
Vector3 translation = newWorldPosition - this->worldPosition;
|
|
float distance = translation.magnitude();
|
|
float angle = Vector3::SignedAngle(this->worldOrientation * Vector3::forward,
|
|
translation, Vector3::up);
|
|
Polar polarTranslation = Polar(angle, distance);
|
|
perception->UpdatePose(polarTranslation);
|
|
this->worldPosition = newWorldPosition;
|
|
}
|
|
|
|
void Roboid::SetOrientation(Quaternion worldOrientation) {
|
|
Quaternion delta =
|
|
Quaternion::Inverse(this->worldOrientation) * worldOrientation;
|
|
perception->UpdatePose(delta);
|
|
this->worldOrientation = worldOrientation;
|
|
} |