75 lines
2.4 KiB
C++
75 lines
2.4 KiB
C++
#include <ControlledMotor.h>
|
|
#include <Propulsion.h>
|
|
|
|
#include <FloatSingle.h>
|
|
|
|
Propulsion::Propulsion() {
|
|
this->motors = nullptr;
|
|
this->motorCount = 0;
|
|
}
|
|
|
|
void Propulsion::AddMotors(MotorPlacement* motors, unsigned int motorCount) {
|
|
this->motors = motors;
|
|
this->motorCount = motorCount;
|
|
}
|
|
|
|
void Propulsion::AddMotors(Placement* motors, unsigned int motorCount) {
|
|
this->placement = motors;
|
|
this->motorCount = motorCount;
|
|
}
|
|
|
|
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++) {
|
|
MotorPlacement placement = motors[motorIx];
|
|
// placement.controlledMotor->Update(timeStep);
|
|
}
|
|
}
|
|
|
|
void Propulsion::SetDiffDriveSpeed(float leftSpeed, float rightSpeed) {
|
|
for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
|
|
Motor* motor = motors[motorIx].motor;
|
|
if (motor == nullptr)
|
|
continue;
|
|
|
|
float xPosition = motors[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++) {
|
|
MotorPlacement placement = motors[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);
|
|
}
|
|
|
|
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);
|
|
}
|