113 lines
3.2 KiB
C++
113 lines
3.2 KiB
C++
#include "ServoMotor.h"
|
|
|
|
#include "LinearAlgebra/FloatSingle.h"
|
|
#include "Participant.h"
|
|
|
|
namespace RoboidControl {
|
|
|
|
ServoMotor::ServoMotor(Participant* participant) : Thing(participant, Thing::Servo, 0) {
|
|
this->controlMode = ControlMode::Position;
|
|
this->targetAngle = Angle16();
|
|
this->hasTargetAngle = false;
|
|
}
|
|
|
|
ServoMotor::ServoMotor(Thing* parent) : ServoMotor(parent->owner) {
|
|
std::cout << "new parent " << parent->name << " owner " << parent->owner << std::endl;
|
|
this->parent = parent;
|
|
}
|
|
|
|
void ServoMotor::SetTargetAngle(Angle16 angle) {
|
|
angle = Angle16::Clamp(angle, minAngle, maxAngle);
|
|
|
|
if (maxSpeed == 0.0F) {
|
|
SetAngle(angle);
|
|
this->limitedTargetAngle = angle;
|
|
}
|
|
|
|
this->controlMode = ControlMode::Position;
|
|
this->targetAngle = angle;
|
|
this->hasTargetAngle = true;
|
|
}
|
|
|
|
Angle16 ServoMotor::GetTargetAngle() {
|
|
return this->targetAngle;
|
|
}
|
|
|
|
void ServoMotor::SetMaximumVelocity(float maxVelocity) {
|
|
this->maxSpeed = maxVelocity;
|
|
}
|
|
|
|
void ServoMotor::SetTargetVelocity(float targetVelocity) {
|
|
targetVelocity = Float::Clamp(targetVelocity, -this->maxSpeed, maxSpeed);
|
|
|
|
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(unsigned long currentTimeMs, bool recurse) {
|
|
for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
|
|
Thing* child = this->GetChild(childIx);
|
|
if (child != nullptr && child->type == Thing::Servo) {
|
|
ServoMotor* servo = (ServoMotor*)child;
|
|
servo->Update(currentTimeMs, recurse);
|
|
}
|
|
}
|
|
|
|
if (this->lastUpdateTimeMs == 0 || currentTimeMs < this->lastUpdateTimeMs) {
|
|
this->lastUpdateTimeMs = currentTimeMs;
|
|
return;
|
|
}
|
|
|
|
float deltaTime = (currentTimeMs - this->lastUpdateTimeMs) / 1000.0F;
|
|
|
|
// Position control
|
|
if (controlMode == ControlMode::Position) {
|
|
if (maxSpeed == 0.0F || hasTargetAngle == false) {
|
|
this->lastUpdateTimeMs = currentTimeMs;
|
|
return;
|
|
|
|
} else {
|
|
float angleStep = maxSpeed * deltaTime;
|
|
float deltaAngle =
|
|
this->targetAngle.InDegrees() - this->limitedTargetAngle.InDegrees();
|
|
float absDeltaAngle = (deltaAngle < 0) ? -deltaAngle : deltaAngle;
|
|
|
|
if (absDeltaAngle < angleStep) {
|
|
this->limitedTargetAngle = this->targetAngle;
|
|
SetAngle(targetAngle);
|
|
} else {
|
|
if (deltaAngle < 0)
|
|
limitedTargetAngle = limitedTargetAngle - Angle16::Degrees(angleStep);
|
|
else
|
|
limitedTargetAngle = limitedTargetAngle + Angle16::Degrees(angleStep);
|
|
}
|
|
SetAngle(limitedTargetAngle);
|
|
|
|
this->lastUpdateTimeMs = currentTimeMs;
|
|
return;
|
|
}
|
|
|
|
// Velocity control
|
|
} else {
|
|
float newAngle = this->targetAngle.InDegrees() + targetVelocity * deltaTime;
|
|
newAngle =
|
|
Float::Clamp(newAngle, minAngle.InDegrees(), maxAngle.InDegrees());
|
|
|
|
Angle16 targetAngle = Angle16::Degrees(newAngle);
|
|
ServoMotor::SetTargetAngle(targetAngle);
|
|
SetAngle(targetAngle);
|
|
|
|
this->lastUpdateTimeMs = currentTimeMs;
|
|
}
|
|
}
|
|
|
|
void ServoMotor::SetAngle(Angle16 angle) {
|
|
this->actualAngle = angle;
|
|
};
|
|
|
|
} // namespace RoboidControl
|