From 8c50b53af86a871c3d29b97c5cd039842f7774f8 Mon Sep 17 00:00:00 2001
From: Jan Hausberg <jan.hausberg@kit.edu>
Date: Tue, 18 Jan 2022 13:49:50 +0100
Subject: [PATCH] Publish intermediate PWM values and timestamps for debugging
---
.../ethercat/head_board/armar7de/Data.cpp | 5 ++++
.../armar7de/joint_controller/PWMPosition.cpp | 23 ++++++++++++++++++-
.../armar7de/joint_controller/PWMPosition.h | 3 +++
3 files changed, 30 insertions(+), 1 deletion(-)
diff --git a/source/devices/ethercat/head_board/armar7de/Data.cpp b/source/devices/ethercat/head_board/armar7de/Data.cpp
index f53be491..62b170b0 100644
--- a/source/devices/ethercat/head_board/armar7de/Data.cpp
+++ b/source/devices/ethercat/head_board/armar7de/Data.cpp
@@ -75,6 +75,11 @@ namespace devices::ethercat::head_board::armar7de
velocity.value = velocityFilter.update(velocity.value);
// ARMARX_INFO << VAROUT(velocity.value);
acceleration.read(getVelocity(), timeSinceLastIteration);
+// if (std::abs(acceleration.value) > 100)
+// {
+// ARMARX_INFO << VAROUT(relativeEncoder.getRaw());
+// ARMARX_INFO << VAROUT(getMotorPwmValue());
+// }
// acceleration.value = (static_cast<float>(velocity.value) - oldVelocity) / dt;
filteredAcceleration = accelerationFilter.update(acceleration.value);
}
diff --git a/source/devices/ethercat/head_board/armar7de/joint_controller/PWMPosition.cpp b/source/devices/ethercat/head_board/armar7de/joint_controller/PWMPosition.cpp
index 9a6164fb..1f34f1b7 100644
--- a/source/devices/ethercat/head_board/armar7de/joint_controller/PWMPosition.cpp
+++ b/source/devices/ethercat/head_board/armar7de/joint_controller/PWMPosition.cpp
@@ -71,6 +71,8 @@ namespace devices::ethercat::head_board::armar7de::joint_controller
{
// ARMARX_INFO << "PWMPositionController";
+ this->timeSinceLastIteration = timeSinceLastIteration;
+
if (target.isValid())
{
auto currentPosition = dataPtr->getAbsoluteEncoderAngleInRad();
@@ -105,11 +107,14 @@ namespace devices::ethercat::head_board::armar7de::joint_controller
r.position);
auto targetPWM = pidPosController->getControlValue();
// auto targetPWM = static_cast<int>(controller.run(timeSinceLastIteration, dataPtr->getVelocity(), newVel));
+ targetPWMPid = targetPWM;
newVel = std::clamp(newVel, -posController.maxV, posController.maxV);
float torqueFF = config->feedforwardTorqueToPWMFactor * -sensorValue->gravityTorque;
targetPWM += torqueFF;
+ targetPWMTorqueFF = torqueFF;
targetPWM += newVel * config->feedforwardVelocityToPWMFactor;
+ targetPWMNewVel = newVel * config->feedforwardVelocityToPWMFactor;
// ARMARX_INFO << deactivateSpam(0.1) << VAROUT(pidPosController->previousError) << VAROUT(r.acceleration)
// << VAROUT(target.position) << VAROUT(targetPWM) << VAROUT(pidPosController->Kp) << VAROUT(pidPosController->Ki)
@@ -124,6 +129,14 @@ namespace devices::ethercat::head_board::armar7de::joint_controller
float updateRatio = 0.3;
this->targetPWM = (1.0 - updateRatio) * this->targetPWM + updateRatio * targetPWM;
float pwmDiff = std::abs(dataPtr->getMotorPwmValue() - targetPWM);
+// if (pwmDiff > 100)
+// {
+// ARMARX_ERROR << VAROUT(currentPosition);
+// }
+// else
+// {
+// ARMARX_INFO << VAROUT(currentPosition);
+// }
// ARMARX_INFO << deactivateSpam(0.3) << VAROUT(pwmDiff) << VAROUT(dataPtr->getTargetPWM()) << VAROUT(targetPWM) << VAROUT(dataPtr->getVelocity());
if (pwmDiff > 5 || dataPtr->getVelocity() > 0.01) // avoid jittering when standing still
{
@@ -315,7 +328,15 @@ namespace devices::ethercat::head_board::armar7de::joint_controller
// {"pospidPropCV", new Variant(posController.pid->previousError * posController.pid->Kp)},
// {"pospidDiffCV", new Variant(posController.pid->derivative * posController.pid->Kd)},
// {"pidUsed", new Variant(posController.getCurrentlyPIDActive())},
- {"desiredPWM", new armarx::Variant(targetPWM.load())}};
+ {"desiredPWM", new armarx::Variant(targetPWM.load())},
+
+ {"targetPWMPid", new armarx::Variant(targetPWMPid.load())},
+ {"targetPWMTorqueFF", new armarx::Variant(targetPWMTorqueFF.load())},
+ {"targetPWMNewVel", new armarx::Variant(targetPWMNewVel.load())},
+ {"posControllerCurrentAcc", new armarx::Variant(posController.currentAcc)},
+ {"posControllerDt", new armarx::Variant(posController.dt)},
+ {"timeSinceLastIterationMicrosSeconds", new armarx::Variant(timeSinceLastIteration.toMicroSeconds())}
+ };
}
PWMPositionControllerConfigurationCPtr
diff --git a/source/devices/ethercat/head_board/armar7de/joint_controller/PWMPosition.h b/source/devices/ethercat/head_board/armar7de/joint_controller/PWMPosition.h
index b0bd6df7..411504d7 100644
--- a/source/devices/ethercat/head_board/armar7de/joint_controller/PWMPosition.h
+++ b/source/devices/ethercat/head_board/armar7de/joint_controller/PWMPosition.h
@@ -101,6 +101,9 @@ namespace devices::ethercat::head_board::armar7de::joint_controller
bool stopRequested = false;
mutable armarx::ThreadPool::Handle threadHandle;
const armarx::SensorValue1DoFActuator* sensorValue;
+
+ std::atomic<double> targetPWMPid, targetPWMTorqueFF, targetPWMNewVel, minJerkControllerOutputPosition, minJerkControllerOutputVelocity, minJerkControllerOutputAcceleration;
+ IceUtil::Time timeSinceLastIteration;
};
} // namespace devices::ethercat::head_board::armar7de::joint_controller
--
GitLab