RoboidControl/RoboidControl-cpp
RoboidControl/RoboidControl-cpp
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fixed issues

Browse Source
This commit is contained in:
Pascal Serrarens 2024-01-29 11:58:23 +01:00
parent e995d4cec9
commit af3d2531b2
1 changed files with 29 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
ServoMotor.cpp
View File

@ -2,20 +2,26 @@
#include "VectorAlgebra/FloatSingle.h" #include "VectorAlgebra/FloatSingle.h"
ServoMotor::ServoMotor() { this->controlMode = ControlMode::Position; } ServoMotor::ServoMotor() {
this->controlMode = ControlMode::Position;
this->targetAngle = 0;
this->hasTargetAngle = false;
}
void ServoMotor::SetTargetAngle(float targetAngle) { void ServoMotor::SetTargetAngle(float angle) {
targetAngle = Float::Clamp(targetAngle, minAngle, maxAngle); angle = Float::Clamp(angle, minAngle, maxAngle);
if (maxVelocity == 0.0F) { if (maxVelocity == 0.0F || hasTargetAngle == false) {
SetAngle(targetAngle); SetAngle(angle);
} else if (targetAngle != this->targetAngle) { this->limitedTargetAngle = angle;
} else if (angle != this->targetAngle) {
// if the new target is the same, the limited angle is not overwritten // if the new target is the same, the limited angle is not overwritten
this->limitedTargetAngle = this->targetAngle; this->limitedTargetAngle = this->targetAngle;
} }
this->controlMode = ControlMode::Position; this->controlMode = ControlMode::Position;
this->targetAngle = targetAngle; this->targetAngle = angle;
this->hasTargetAngle = true;
} }
float ServoMotor::GetTargetAngle() { return this->targetAngle; } float ServoMotor::GetTargetAngle() { return this->targetAngle; }
@ -24,7 +30,7 @@ void ServoMotor::SetMaximumVelocity(float maxVelocity) {
this->maxVelocity = maxVelocity; this->maxVelocity = maxVelocity;
} }
void ServoMotor::SetTargetVelocity(float velocity) { void ServoMotor::SetTargetVelocity(float targetVelocity) {
targetVelocity = targetVelocity =
Float::Clamp(targetVelocity, -this->maxVelocity, maxVelocity); Float::Clamp(targetVelocity, -this->maxVelocity, maxVelocity);
@ -51,13 +57,15 @@ void ServoMotor::Update(float currentTimeMs) {
} else { } else {
float angleStep = maxVelocity * deltaTime; float angleStep = maxVelocity * deltaTime;
float deltaAngle = this->targetAngle - this->limitedTargetAngle; float deltaAngle = this->targetAngle - this->limitedTargetAngle;
if (deltaAngle < 0) float absDeltaAngle = (deltaAngle < 0) ? -deltaAngle : deltaAngle;
deltaAngle = -deltaAngle;
if (deltaAngle < angleStep) { if (absDeltaAngle < angleStep) {
this->limitedTargetAngle = this->targetAngle; this->limitedTargetAngle = this->targetAngle;
SetAngle(targetAngle); SetAngle(targetAngle);
} else { } else {
if (deltaAngle < 0)
limitedTargetAngle -= angleStep;
else
limitedTargetAngle += angleStep; limitedTargetAngle += angleStep;
} }
SetAngle(limitedTargetAngle); SetAngle(limitedTargetAngle);
@ -65,8 +73,8 @@ void ServoMotor::Update(float currentTimeMs) {
this->lastUpdateTimeMs = currentTimeMs; this->lastUpdateTimeMs = currentTimeMs;
return; return;
} }
}
} else { // Velocity Control
float newAngle = this->targetAngle + targetVelocity * deltaTime; float newAngle = this->targetAngle + targetVelocity * deltaTime;
newAngle = Float::Clamp(newAngle, minAngle, maxAngle); newAngle = Float::Clamp(newAngle, minAngle, maxAngle);
@ -74,4 +82,5 @@ void ServoMotor::Update(float currentTimeMs) {
SetAngle(newAngle); SetAngle(newAngle);
this->lastUpdateTimeMs = currentTimeMs; this->lastUpdateTimeMs = currentTimeMs;
}
} }