77 lines
2.2 KiB
C++
77 lines
2.2 KiB
C++
#include "ServoMotor.h"
|
|
|
|
#include "VectorAlgebra/FloatSingle.h"
|
|
|
|
ServoMotor::ServoMotor() { this->controlMode = ControlMode::Position; }
|
|
|
|
void ServoMotor::SetTargetAngle(float targetAngle) {
|
|
targetAngle = Float::Clamp(targetAngle, minAngle, maxAngle);
|
|
|
|
if (maxVelocity == 0.0F) {
|
|
SetAngle(targetAngle);
|
|
} else if (targetAngle != this->targetAngle) {
|
|
// if the new target is the same, the limited angle is not overwritten
|
|
this->limitedTargetAngle = this->targetAngle;
|
|
}
|
|
|
|
this->controlMode = ControlMode::Position;
|
|
this->targetAngle = targetAngle;
|
|
}
|
|
|
|
float ServoMotor::GetTargetAngle() { return this->targetAngle; }
|
|
|
|
void ServoMotor::SetMaximumVelocity(float maxVelocity) {
|
|
this->maxVelocity = maxVelocity;
|
|
}
|
|
|
|
void ServoMotor::SetTargetVelocity(float velocity) {
|
|
targetVelocity =
|
|
Float::Clamp(targetVelocity, -this->maxVelocity, maxVelocity);
|
|
|
|
this->controlMode = ControlMode::Velocity;
|
|
this->targetVelocity = targetVelocity;
|
|
this->hasTargetAngle = false; // can't we use the controlMode for this?
|
|
}
|
|
|
|
float ServoMotor::GetTargetVelocity() { return this->targetVelocity; }
|
|
|
|
void ServoMotor::Update(float currentTimeMs) {
|
|
if (this->lastUpdateTimeMs == 0 || currentTimeMs < this->lastUpdateTimeMs) {
|
|
this->lastUpdateTimeMs = currentTimeMs;
|
|
return;
|
|
}
|
|
|
|
float deltaTime = (currentTimeMs - this->lastUpdateTimeMs) / 1000.0F;
|
|
|
|
if (controlMode == ControlMode::Position) {
|
|
if (maxVelocity == 0.0F || hasTargetAngle == false) {
|
|
this->lastUpdateTimeMs = currentTimeMs;
|
|
return;
|
|
|
|
} else {
|
|
float angleStep = maxVelocity * deltaTime;
|
|
float deltaAngle = this->targetAngle - this->limitedTargetAngle;
|
|
if (deltaAngle < 0)
|
|
deltaAngle = -deltaAngle;
|
|
|
|
if (deltaAngle < angleStep) {
|
|
this->limitedTargetAngle = this->targetAngle;
|
|
SetAngle(targetAngle);
|
|
} else {
|
|
limitedTargetAngle += angleStep;
|
|
}
|
|
SetAngle(limitedTargetAngle);
|
|
|
|
this->lastUpdateTimeMs = currentTimeMs;
|
|
return;
|
|
}
|
|
}
|
|
|
|
float newAngle = this->targetAngle + targetVelocity * deltaTime;
|
|
newAngle = Float::Clamp(newAngle, minAngle, maxAngle);
|
|
|
|
ServoMotor::SetTargetAngle(newAngle);
|
|
SetAngle(newAngle);
|
|
|
|
this->lastUpdateTimeMs = currentTimeMs;
|
|
} |