From 2f8fd491e0b68fde14d822606e66eefd0835e4f0 Mon Sep 17 00:00:00 2001
From: ArmarX User <armarx@kit.edu>
Date: Fri, 23 Aug 2019 18:13:21 +0200
Subject: [PATCH] work on torque controller
---
.../JointPWMTorqueController.cpp | 73 +++++++++++--------
.../JointPWMTorqueController.h | 3 +
2 files changed, 45 insertions(+), 31 deletions(-)
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMTorqueController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMTorqueController.cpp
index 2cf2bf267..4437bed7d 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMTorqueController.cpp
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMTorqueController.cpp
@@ -28,26 +28,24 @@ void JointPWMTorqueController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
{
if (target.isValid())
{
- const float velocity = dataPtr->getVelocity();
+ const float velocity = dataPtr->getRawVelocity(); // dataPtr->getVelocity();
const float targetTorque = target.torque;
- const int maxIter = 30;
+ const int maxIter = 50;
int counter = 0;
- float pwm_approx_min = -1000.f;
- float pwm_approx_max = 1000.f;
- float pwm_approx_estimate = 0.f;
+ float pwm_approx_min = -3000.f;
+ float pwm_approx_max = 3000.f;
+ float pwm_approx_estimate = (pwm_approx_min + pwm_approx_max) * 0.5;
auto f = [ = ](float pwm)
{
int32_t pwm_integer = (int32_t) pwm;
- //return torqueEstimation->linearModel_dcx22(velocity, pwm_integer) - targetTorque;
torqueEstimation->estimateTorque(velocity, pwm_integer);
return torqueEstimation->getTorqueWithInertia() - targetTorque;
- //return torqueEstimation->linearModel_dcx22(velocity, pwm_integer) - targetTorque;
};
- while (std::fabs(f(pwm_approx_estimate)) > .001f && counter < maxIter)
+ while (std::fabs(f(pwm_approx_estimate)) > .005f && counter < maxIter)
{
if (f(pwm_approx_min) * f(pwm_approx_max) < 0)
{
@@ -70,34 +68,47 @@ void JointPWMTorqueController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
}
/*if (f(pwm_approx1) * f(pwm_approx2) >= 0)
- {
- //ARMARX_INFO << deactivateSpam(1) << "ASSERT";
- dataPtr->setTargetPWM(0);
- return;
- }
+ {
+ //ARMARX_INFO << deactivateSpam(1) << "ASSERT";
+ dataPtr->setTargetPWM(0);
+ return;
+ }
- float pwm_approx3 = pwm_approx1;
+ float pwm_approx3 = pwm_approx1;
- for (; counter < maxIter; counter++)
- {
- pwm_approx3 = (pwm_approx1 * f(pwm_approx2) - pwm_approx2 * f(pwm_approx1)) / (f(pwm_approx2) - f(pwm_approx1));
- if (abs(f(pwm_approx3)) <= .01)
- {
- break;
- }
- else if (f(pwm_approx3) * f(pwm_approx1) < 0)
- {
- pwm_approx2 = pwm_approx3;
- }
- else
- {
- pwm_approx1 = pwm_approx3;
- }
- }*/
+ for (; counter < maxIter; counter++)
+ {
+ pwm_approx3 = (pwm_approx1 * f(pwm_approx2) - pwm_approx2 * f(pwm_approx1)) / (f(pwm_approx2) - f(pwm_approx1));
+ if (abs(f(pwm_approx3)) <= .01)
+ {
+ break;
+ }
+ else if (f(pwm_approx3) * f(pwm_approx1) < 0)
+ {
+ pwm_approx2 = pwm_approx3;
+ }
+ else
+ {
+ pwm_approx1 = pwm_approx3;
+ }
+ }*/
- ARMARX_INFO << deactivateSpam(0.1) << VAROUT(targetTorque) << VAROUT(pwm_approx_estimate) << VAROUT(f(pwm_approx_estimate)) << VAROUT(counter);
+
+
+
+ ARMARX_INFO << deactivateSpam(0.01) << VAROUT(targetTorque) << VAROUT(pwm_approx_estimate) << VAROUT(f(pwm_approx_estimate)) << VAROUT(counter);
dataPtr->setTargetPWM(int(pwm_approx_estimate));
//dataPtr->setTargetPWM(0);
+
+ // torqueEstimation->estimateTorque(dataPtr->getVelocity(), dataPtr->getTargetPWM());
+ // float torque_error = targetTorque - torqueEstimation->getTorqueWithInertia();
+ // float P = 150.0f;
+ // float I = 20.0f;
+ // error_sum += torque_error;
+ // float pwm = P * torque_error + I * error_sum;
+ // pwm = (1 - filter_coeff) * last_pwm + filter_coeff * pwm;
+ // dataPtr->setTargetPWM(int(pwm));
+ // last_pwm = pwm;
}
}
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMTorqueController.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMTorqueController.h
index c6e7f294f..e8eef21fb 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMTorqueController.h
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMTorqueController.h
@@ -30,6 +30,9 @@ namespace armarx
KITGripperBasisBoardPtr board;
const std::string deviceName;
+ float last_pwm = 0.0f;
+ float filter_coeff = 0.8f;
+ float error_sum = 0.0f;
MotorTorqueEstimatorPtr torqueEstimation;
size_t actorIndex = 0;
--
GitLab