139 lines
4.1 KiB
C++
139 lines
4.1 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() {
|
|
#ifdef RC_DEBUG
|
|
Serial.begin(115200);
|
|
#endif
|
|
this->perception = new Perception();
|
|
this->perception->roboid = this;
|
|
this->propulsion = nullptr;
|
|
this->networkSync = nullptr;
|
|
this->actuation = nullptr;
|
|
this->worldPosition = Vector3::zero;
|
|
// this->worldOrientation = Quaternion::identity;
|
|
this->worldAngleAxis = AngleAxis();
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
Roboid::Roboid(Perception *perception, ServoMotor *actuation) : Roboid() {
|
|
this->perception = perception;
|
|
perception->roboid = this;
|
|
this->propulsion = nullptr;
|
|
|
|
this->actuation = actuation;
|
|
}
|
|
|
|
Roboid::Roboid(ServoMotor *actuation) : actuation(actuation) {}
|
|
|
|
void Roboid::Update(float currentTimeMs) {
|
|
if (perception != nullptr)
|
|
perception->Update(currentTimeMs);
|
|
|
|
if (propulsion != nullptr) {
|
|
propulsion->Update(currentTimeMs);
|
|
|
|
float deltaTime = (currentTimeMs - lastUpdateTimeMs) / 1000;
|
|
Quaternion roboidOrientation = this->GetOrientation();
|
|
SetPosition(this->worldPosition +
|
|
roboidOrientation * Vector3::forward *
|
|
this->propulsion->GetVelocity().distance * deltaTime);
|
|
SetOrientation(roboidOrientation *
|
|
Quaternion::AngleAxis(this->propulsion->GetAngularVelocity(),
|
|
Vector3::up));
|
|
}
|
|
|
|
if (actuation != nullptr)
|
|
actuation->Update(currentTimeMs);
|
|
|
|
if (networkSync != nullptr)
|
|
networkSync->NetworkUpdate(this);
|
|
|
|
lastUpdateTimeMs = currentTimeMs;
|
|
}
|
|
|
|
Vector3 Roboid::GetPosition() { return this->worldPosition; }
|
|
Vector2 Roboid::GetPosition2D() {
|
|
return Vector2(this->worldPosition.Right(), this->worldPosition.Forward());
|
|
}
|
|
|
|
Quaternion Roboid::GetOrientation() {
|
|
Vector3 axis = this->worldAngleAxis.axis.ToVector3();
|
|
Quaternion q = Quaternion::AngleAxis(this->worldAngleAxis.angle, axis);
|
|
return q;
|
|
}
|
|
|
|
float Roboid::GetOrientation2D() {
|
|
float maxAngle = 90 - Float::epsilon; // note: range vertical angle = -90..90
|
|
|
|
// rotation axis is vertical, so we have a simple 2D orientation
|
|
if (this->worldAngleAxis.axis.verticalAngle > maxAngle)
|
|
return this->worldAngleAxis.angle;
|
|
if (this->worldAngleAxis.axis.verticalAngle < -maxAngle)
|
|
return -this->worldAngleAxis.angle;
|
|
|
|
Quaternion q = GetOrientation();
|
|
return Quaternion::GetAngleAround(Vector3::up, q);
|
|
}
|
|
|
|
void Roboid::SetPosition(Vector3 newWorldPosition) {
|
|
Quaternion roboidOrientation = this->GetOrientation();
|
|
Vector3 translation = newWorldPosition - this->worldPosition;
|
|
float distance = translation.magnitude();
|
|
float angle = Vector3::SignedAngle(roboidOrientation * Vector3::forward,
|
|
translation, Vector3::up);
|
|
Polar polarTranslation = Polar(angle, distance);
|
|
perception->UpdatePose(polarTranslation);
|
|
this->worldPosition = newWorldPosition;
|
|
|
|
if (networkSync != nullptr)
|
|
// networkSync->SendPosition(this->worldPosition);
|
|
networkSync->SendPose(this->worldPosition, roboidOrientation);
|
|
}
|
|
|
|
#include <math.h>
|
|
void Roboid::SetOrientation(Quaternion worldOrientation) {
|
|
float angle;
|
|
Vector3 axis;
|
|
worldOrientation.ToAngleAxis(&angle, &axis);
|
|
|
|
Quaternion delta = Quaternion::Inverse(GetOrientation()) * worldOrientation;
|
|
perception->UpdatePose(delta);
|
|
|
|
AngleAxis angleAxis = AngleAxis(angle, Axis(axis));
|
|
this->worldAngleAxis = angleAxis;
|
|
}
|
|
|
|
void Roboid::SetOrientation2D(float angle) {
|
|
this->worldAngleAxis = AngleAxis(angle, Axis::up);
|
|
}
|
|
|
|
void Roboid::SetModel(const char *url, Vector3 position, Quaternion orientation,
|
|
float scale) {
|
|
this->modelUrl = url;
|
|
this->modelPosition = position;
|
|
this->modelOrientation = orientation;
|
|
this->modelScale = scale;
|
|
}
|