121 lines
3.7 KiB
C++
121 lines
3.7 KiB
C++
#include "ControlledMotor.h"
|
|
#include "Propulsion.h"
|
|
|
|
#include "FloatSingle.h"
|
|
|
|
Propulsion::Propulsion() {
|
|
this->placement = nullptr;
|
|
this->motorCount = 0;
|
|
}
|
|
|
|
//void Propulsion::AddMotors(MotorPlacement* motors, unsigned int motorCount) {
|
|
// this->palce = motors;
|
|
// this->motorCount = motorCount;
|
|
//}
|
|
|
|
void Propulsion::AddMotors(Placement* things, unsigned int thingCount) {
|
|
//this->placement = motors;
|
|
//this->motorCount = motorCount;
|
|
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];
|
|
}
|
|
|
|
}
|
|
|
|
void Propulsion::AddQuadcopter() {
|
|
this->quadcopter = new Quadcopter();
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
void Propulsion::Update() {
|
|
// Hmmm. Arduino dependent code
|
|
// unsigned long curMillis = millis();
|
|
// float timeStep = (float)(curMillis - lastMillis) / 1000;
|
|
// lastMillis = curMillis;
|
|
|
|
for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
|
|
//Placement placement = placement[motorIx];
|
|
// placement.controlledMotor->Update(timeStep);
|
|
}
|
|
}
|
|
|
|
void Propulsion::SetDiffDriveSpeed(float leftSpeed, float rightSpeed) {
|
|
for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
|
|
Motor* motor = (Motor*) placement[motorIx].thing;
|
|
if (motor == nullptr)
|
|
continue;
|
|
|
|
float xPosition = placement[motorIx].position.x;
|
|
if (xPosition < 0)
|
|
motor->SetSpeed(leftSpeed);
|
|
else if (xPosition > 0)
|
|
motor->SetSpeed(rightSpeed);
|
|
};
|
|
}
|
|
|
|
void Propulsion::SetDiffDriveVelocities(float leftVelocity, float rightVelocity) {
|
|
for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
|
|
//Placement placement = placement[motorIx];
|
|
//if (placement.position.x < 0)
|
|
// placement.controlledMotor->SetTargetVelocity(leftVelocity);
|
|
//else if (placement.position.x > 0)
|
|
// placement.controlledMotor->SetTargetVelocity(rightVelocity);
|
|
};
|
|
}
|
|
|
|
void Propulsion::SetTwistSpeed(float forward, float yaw) {
|
|
// This is configuration dependent, a drone will do something completely different...
|
|
float leftSpeed = Float::Clamp(forward - yaw, -1, 1);
|
|
float rightSpeed = Float::Clamp(forward + yaw, -1, 1);
|
|
SetDiffDriveSpeed(leftSpeed, rightSpeed);
|
|
}
|
|
|
|
void Propulsion::SetTwistSpeed(float forward, float yaw, float pitch) {
|
|
float leftSpeed = Float::Clamp(forward - yaw, -1, 1);
|
|
float rightSpeed = Float::Clamp(forward + yaw, -1, 1);
|
|
SetDiffDriveSpeed(leftSpeed, rightSpeed);
|
|
|
|
if (quadcopter != nullptr) {
|
|
quadcopter->SetTwistSpeed(forward, yaw, pitch);
|
|
}
|
|
}
|
|
|
|
void Propulsion::SetTwistVelocity(float forwardVelocity, float turningVelocity) {
|
|
float leftVelocity = Float::Clamp(forwardVelocity - turningVelocity, -1, 1);
|
|
float rightVelocity = Float::Clamp(forwardVelocity + turningVelocity, -1, 1);
|
|
SetDiffDriveVelocities(leftVelocity, rightVelocity);
|
|
}
|
|
|
|
void Propulsion::SetLinearSpeed(Vector3 velocity, float yawSpeed, float rollSpeed) {
|
|
if (quadcopter != nullptr)
|
|
quadcopter->LinearMotion(velocity, yawSpeed, rollSpeed);
|
|
}
|
|
|
|
Quadcopter* Propulsion::GetQuadcopter() {
|
|
return quadcopter;
|
|
} |