69 lines
1.9 KiB
C++
69 lines
1.9 KiB
C++
#include "Propulsion.h"
|
|
#include "Roboid.h"
|
|
|
|
#include "FloatSingle.h"
|
|
|
|
#include <Arduino.h>
|
|
|
|
Propulsion::Propulsion() {
|
|
this->placement = nullptr;
|
|
this->motorCount = 0;
|
|
}
|
|
|
|
unsigned int Propulsion::GetMotorCount() { return this->motorCount; }
|
|
|
|
Motor *Propulsion::GetMotor(unsigned int motorId) {
|
|
if (motorId >= this->motorCount)
|
|
return nullptr;
|
|
|
|
Thing *thing = this->placement[motorId].thing;
|
|
if (thing->IsMotor())
|
|
return (Motor *)thing;
|
|
|
|
return nullptr;
|
|
}
|
|
|
|
Placement *Propulsion::GetMotorPlacement(unsigned int motorId) {
|
|
if (motorId >= this->motorCount)
|
|
return nullptr;
|
|
|
|
Placement *placement = &this->placement[motorId];
|
|
if (placement->thing->IsMotor())
|
|
return placement;
|
|
|
|
return nullptr;
|
|
}
|
|
|
|
void Propulsion::Update(float currentTimeMs) {}
|
|
|
|
void Propulsion::SetTwistSpeed(float forward, float yaw) {}
|
|
|
|
void Propulsion::SetTwistSpeed(Vector2 linear, float yaw) {}
|
|
|
|
void Propulsion::SetTwistSpeed(Vector3 linear, float yaw, float pitch,
|
|
float roll) {}
|
|
|
|
Polar Propulsion::GetVelocity() { return Polar(0, 0); }
|
|
|
|
float Propulsion::GetAngularVelocity() { return 0; }
|
|
|
|
Vector3 Propulsion::GetPosition() { return this->worldPosition; }
|
|
|
|
Quaternion Propulsion::GetOrientation() { return this->worldOrientation; }
|
|
|
|
void Propulsion::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);
|
|
roboid->perception->UpdatePose(polarTranslation);
|
|
this->worldPosition = newWorldPosition;
|
|
}
|
|
|
|
void Propulsion::SetOrientation(Quaternion worldOrientation) {
|
|
Quaternion delta =
|
|
Quaternion::Inverse(this->worldOrientation) * worldOrientation;
|
|
roboid->perception->UpdatePose(delta);
|
|
this->worldOrientation = worldOrientation;
|
|
} |