diff --git a/Propulsion.cpp b/Propulsion.cpp index 9ef4c4b..775ee88 100644 --- a/Propulsion.cpp +++ b/Propulsion.cpp @@ -1,59 +1,54 @@ -#include "ControlledMotor.h" #include "Propulsion.h" +#include "ControlledMotor.h" #include "FloatSingle.h" -extern "C" { - #include "debug.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(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]; - } + // 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(Quadcopter* quadcopter) { - this->quadcopter = quadcopter; + this->quadcopter = quadcopter; } unsigned int Propulsion::GetMotorCount() { - return this->motorCount; + 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; - + if (motorId >= this->motorCount) return nullptr; + + Thing* thing = this->placement[motorId].thing; + if (thing->isMotor) + return (Motor*)thing; + + return nullptr; } void Propulsion::Update() { @@ -63,14 +58,14 @@ void Propulsion::Update() { // lastMillis = curMillis; for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) { - //Placement placement = placement[motorIx]; - // placement.controlledMotor->Update(timeStep); + // 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; + Motor* motor = (Motor*)placement[motorIx].thing; if (motor == nullptr) continue; @@ -84,11 +79,11 @@ void Propulsion::SetDiffDriveSpeed(float leftSpeed, float 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); + // Placement placement = placement[motorIx]; + // if (placement.position.x < 0) + // placement.controlledMotor->SetTargetVelocity(leftVelocity); + // else if (placement.position.x > 0) + // placement.controlledMotor->SetTargetVelocity(rightVelocity); }; } @@ -105,13 +100,13 @@ void Propulsion::SetTwistSpeed(float forward, float yaw, float pitch) { SetDiffDriveSpeed(leftSpeed, rightSpeed); if (quadcopter != nullptr) { - quadcopter->SetTwistSpeed(forward, yaw, pitch); + quadcopter->SetTwistSpeed(forward, yaw, pitch); } } void Propulsion::SetTwistSpeed(Vector3 linear, float yaw) { - if (quadcopter != nullptr) - quadcopter->SetTwistSpeed(linear, yaw); + if (quadcopter != nullptr) + quadcopter->SetTwistSpeed(linear, yaw); } void Propulsion::SetTwistVelocity(float forwardVelocity, float turningVelocity) { @@ -121,10 +116,10 @@ void Propulsion::SetTwistVelocity(float forwardVelocity, float turningVelocity) } void Propulsion::SetLinearSpeed(Vector3 velocity, float yawSpeed, float rollSpeed) { - if (quadcopter != nullptr) - quadcopter->LinearMotion(velocity, yawSpeed, rollSpeed); + if (quadcopter != nullptr) + quadcopter->LinearMotion(velocity, yawSpeed, rollSpeed); } Quadcopter* Propulsion::GetQuadcopter() { - return quadcopter; + return quadcopter; } \ No newline at end of file diff --git a/component.mk b/component.mk new file mode 100644 index 0000000..f3b1528 --- /dev/null +++ b/component.mk @@ -0,0 +1 @@ +COMPONENT_ADD_INCLUDEDIRS = ./VectorAlgebra/include \ No newline at end of file