71 lines
1.7 KiB
C++
71 lines
1.7 KiB
C++
#include "Propulsion.h"
|
|
|
|
#include "FloatSingle.h"
|
|
|
|
#include <Arduino.h>
|
|
|
|
Propulsion::Propulsion() {
|
|
this->placement = nullptr;
|
|
this->motorCount = 0;
|
|
}
|
|
|
|
void Propulsion::AddMotors(Placement* things, unsigned int thingCount) {
|
|
this->motorCount = 0;
|
|
for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
|
|
Thing* thing = things[thingIx].thing;
|
|
if (thing->IsMotor())
|
|
motorCount++;
|
|
}
|
|
this->placement = new Placement[motorCount];
|
|
|
|
unsigned int motorIx = 0;
|
|
for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
|
|
Thing* thing = things[thingIx].thing;
|
|
if (thing->IsMotor())
|
|
this->placement[motorIx++] = things[thingIx];
|
|
}
|
|
}
|
|
|
|
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) {
|
|
this->lastUpdateTime = currentTimeMs;
|
|
}
|
|
|
|
void Propulsion::SetMaxSpeed(float maxSpeed) {
|
|
this->maxSpeed = abs(maxSpeed);
|
|
}
|
|
|
|
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) {}
|