Skip to content
Snippets Groups Projects
Commit 8c50b53a authored by Jan Hausberg's avatar Jan Hausberg
Browse files

Publish intermediate PWM values and timestamps for debugging

parent bfc97dec
No related branches found
No related tags found
1 merge request!6Draft: Resolve "Fix velocity jumps in head_board::armar7de"
......@@ -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);
}
......
......@@ -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
......
......@@ -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
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment