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

Fix not all servos in tree moving

Browse Source
This commit is contained in:
Pascal Serrarens 2024-01-31 12:13:13 +01:00
parent 8672e1519a
commit 4776907262
2 changed files with 12 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
ServoMotor.cpp
View File

@ -42,9 +42,15 @@ void ServoMotor::SetTargetVelocity(float targetVelocity) {
float ServoMotor::GetTargetVelocity() { return this->targetVelocity; } float ServoMotor::GetTargetVelocity() { return this->targetVelocity; }
#include <Arduino.h>
void ServoMotor::Update(float currentTimeMs) { void ServoMotor::Update(float currentTimeMs) {
for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
Thing *child = this->GetChild(childIx);
if (child != nullptr && child->type == Thing::ServoType) {
ServoMotor *servo = (ServoMotor *)child;
servo->Update(currentTimeMs);
}
}
if (this->lastUpdateTimeMs == 0 || currentTimeMs < this->lastUpdateTimeMs) { if (this->lastUpdateTimeMs == 0 || currentTimeMs < this->lastUpdateTimeMs) {
this->lastUpdateTimeMs = currentTimeMs; this->lastUpdateTimeMs = currentTimeMs;
return; return;
@ -52,6 +58,7 @@ void ServoMotor::Update(float currentTimeMs) {
float deltaTime = (currentTimeMs - this->lastUpdateTimeMs) / 1000.0F; float deltaTime = (currentTimeMs - this->lastUpdateTimeMs) / 1000.0F;
// Position control
if (controlMode == ControlMode::Position) { if (controlMode == ControlMode::Position) {
if (maxVelocity == 0.0F || hasTargetAngle == false) { if (maxVelocity == 0.0F || hasTargetAngle == false) {
this->lastUpdateTimeMs = currentTimeMs; this->lastUpdateTimeMs = currentTimeMs;
@ -77,7 +84,8 @@ void ServoMotor::Update(float currentTimeMs) {
return; return;
} }
} else { // Velocity Control // Velocity control
} else {
float newAngle = this->targetAngle + targetVelocity * deltaTime; float newAngle = this->targetAngle + targetVelocity * deltaTime;
newAngle = Float::Clamp(newAngle, minAngle, maxAngle); newAngle = Float::Clamp(newAngle, minAngle, maxAngle);
@ -86,13 +94,4 @@ void ServoMotor::Update(float currentTimeMs) {
this->lastUpdateTimeMs = currentTimeMs; this->lastUpdateTimeMs = currentTimeMs;
} }
Serial.println(this->childCount);
for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
Thing *child = this->GetChild(childIx);
if (child->type == Thing::ServoType) {
ServoMotor *servo = (ServoMotor *)child;
servo->Update(currentTimeMs);
}
}
} }

2
VectorAlgebra

@ -1 +1 @@
Subproject commit 78106912a9e80c1ace5533615cab0bd4a00657a0 Subproject commit 596e85d3a41f5f25b6185f2ca4fb18a2140afca1