kitgripper: slowly reduce pwm in emergency stop controller

source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperEmergencyStopController.cpp

@@ -6,10 +6,21 @@ namespace armarx
     JointKITGripperEmergencyStopController::JointKITGripperEmergencyStopController(ActorDataPtr dataPtr) :
         dataPtr(dataPtr)
     {
-
+        pid.reset(new PIDController(0, 5000, 0));
+        pid->maxIntegral = 0.1;
     }
     void JointKITGripperEmergencyStopController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
     {
+        pid->update(timeSinceLastIteration.toSecondsDouble(), dataPtr->getVelocity(), 0.0);
+        float targetPwm = pid->getControlValue();
+        if (std::isnan(targetPwm))
+        {
+            targetPwm = 0.0f;
+        }
+        //        dataPtr->setTargetPWM(targetPwm);
+        lastPWM *= 0.9997;
+        //        ARMARX_RT_LOGF_INFO("old pwm %d, new pwm: %.2f", dataPtr->getTargetPWM(), lastPWM);
+        dataPtr->setTargetPWM(lastPWM);
 
     }
 
@@ -21,7 +32,8 @@ namespace armarx
     void JointKITGripperEmergencyStopController::rtPreActivateController()
     {
         //        ARMARX_INFO << "Stopping gripper!";
-        dataPtr->setTargetPWM(0);
+        //        dataPtr->setTargetPWM(0);
+        lastPWM = math::MathUtils::LimitTo(dataPtr->getTargetPWM(), 500);
     }
 
 

source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperEmergencyStopController.h

 #pragma once
 
 #include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
+#include <RobotAPI/libraries/core/PIDController.h>
 #include "../KITGripperBasisBoardData.h"
 
 
@@ -28,6 +29,8 @@ namespace armarx
     private:
         DummyControlTargetEmergencyStop target;
         ActorDataPtr dataPtr;
+        PIDControllerPtr pid;
+        float lastPWM = 0.0f;
     };