149 lines
4.5 KiB
C++
149 lines
4.5 KiB
C++
#include "Propulsion.h"
|
|
#include "ControlledMotor.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(Quadcopter* quadcopter) {
|
|
this->quadcopter = 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::SetTwistSpeed(Vector3 linear, float yaw) {
|
|
if (quadcopter != nullptr)
|
|
quadcopter->SetTwistSpeed(linear, yaw);
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
// bool Propulsion::Drive(Roboid* roboid, float forwardDistance) {
|
|
// bool finished = true;
|
|
// for (unsigned int motorIx = 0; motorIx < roboid->propulsion->motorCount;
|
|
// motorIx++) {
|
|
// Motor* motor = (Motor*)roboid->placement[motorIx].thing;
|
|
// if (motor == nullptr)
|
|
// continue;
|
|
// if (motor->isControlledMotor == false)
|
|
// continue;
|
|
|
|
// ControlledMotor* controlledMotor = (ControlledMotor*)motor;
|
|
// bool motorFinished = controlledMotor->Drive(forwardDistance);
|
|
// if (motorFinished == false)
|
|
// finished = false;
|
|
// }
|
|
// return finished;
|
|
// }
|