Publish intermediate PWM values and timestamps for debugging

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);
     }

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

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