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

StopMovementController as 0-VelocityController in head_board::armar7de

parent d8cb11c2
No related branches found
No related tags found
1 merge request!16Resolve "Fix StopMovement controller of head_board::armar7de"
......@@ -76,6 +76,40 @@ namespace devices::ethercat::head_board::armar7de
return targetPWM;
}
PWMVelocityControllerConfiguration::PWMVelocityControllerConfiguration(
float maxVelocityRad,
float maxDecelerationRad,
float jerk,
float maxDt,
float directSetVLimit,
float p,
float i,
float d,
float maxIntegral,
float feedforwardVelocityToPWMFactor,
float feedforwardTorqueToPWMFactor,
float PWMDeadzone,
float velocityUpdatePercent,
float conditionalIntegralErrorTreshold,
bool feedForwardMode) :
p(p),
i(i),
d(d),
maxIntegral(maxIntegral),
feedforwardVelocityToPWMFactor(feedforwardVelocityToPWMFactor),
feedforwardTorqueToPWMFactor(feedforwardTorqueToPWMFactor),
PWMDeadzone(PWMDeadzone),
velocityUpdatePercent(velocityUpdatePercent),
conditionalIntegralErrorTreshold(conditionalIntegralErrorTreshold),
feedForwardMode(feedForwardMode)
{
this->maxVelocityRad = maxVelocityRad;
this->maxDecelerationRad = maxDecelerationRad;
this->jerk = jerk;
this->maxDt = maxDt;
this->directSetVLimit = directSetVLimit;
}
void
PWMVelocityController::reset(double currentVelocity)
......
......@@ -46,6 +46,22 @@ namespace devices::ethercat::head_board::armar7de
public:
PWMVelocityControllerConfiguration() = default;
PWMVelocityControllerConfiguration(
float maxVelocityRad,
float maxDecelerationRad,
float jerk,
float maxDt,
float directSetVLimit,
float p,
float i,
float d,
float maxIntegral,
float feedforwardVelocityToPWMFactor,
float feedforwardTorqueToPWMFactor,
float PWMDeadzone,
float velocityUpdatePercent,
float conditionalIntegralErrorTreshold,
bool feedForwardMode);
static PWMVelocityControllerConfigurationCPtr
CreatePWMVelocityControllerConfigData(armarx::control::hardware_config::Config& hwConfig);
float p;
......
#include "StopMovement.h"
// armarx
#include <ArmarXCore/core/ManagedIceObject.h>
#include <RobotAPI/libraries/core/PIDController.h>
// robot_devices
#include <devices/ethercat/head_board/armar7de/Data.h>
......@@ -8,31 +12,73 @@
namespace devices::ethercat::head_board::armar7de::joint_controller
{
StopMovementController::StopMovementController(DataPtr dataPtr) : dataPtr(dataPtr)
StopMovementController::StopMovementController(DataPtr dataPtr) :
JointController(),
config(new PWMVelocityControllerConfiguration(6, 2.5, 30, 0.003, 0.005, 300, 7000, 0, 0.1, 600, 0, 60, 0.5, 1000000, false)),
controller(config),
dataPtr(dataPtr)
{
pid.reset(new armarx::PIDController(0, 5000, 0));
pid->maxIntegral = 0.1;
// velController.acceleration = velocityControllerConfigDataPtr->maxAccelerationRad;
velController.deceleration = config->maxDecelerationRad;
velController.jerk = config->jerk;
velController.maxDt = config->maxDt;
velController.maxV = config->maxVelocityRad;
velController.directSetVLimit = config->directSetVLimit;
}
StopMovementController::~StopMovementController() noexcept(true)
{
stopRequested = true;
try
{
threadHandle.join();
}
catch (...)
{
}
}
void
StopMovementController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
{
pid->update(timeSinceLastIteration.toSecondsDouble(), dataPtr->getVelocity(), 0.0);
float targetPwm = pid->getControlValue();
{
auto currentPosition = dataPtr->getAbsoluteEncoderAngleInRad();
if (std::isnan(targetPwm))
velController.currentPosition = currentPosition;
velController.currentV = lastTargetVelocity;
velController.currentAcc = lastTargetAcceleration;
velController.dt = timeSinceLastIteration.toSecondsDouble();
velController.targetV = 0;
auto r = velController.run();
double newVel = r.velocity;
double newAcc = r.acceleration;
// ARMARX_INFO << deactivateSpam(1) << VAROUT(newVel) << VAROUT(target.velocity);
if (std::isnan(newVel))
{
targetPwm = 0.0f;
newVel = 0;
newAcc = 0;
}
// ARMARX_INFO << "StopMovement";
// ARMARX_INFO << VAROUT(dataPtr->getVelocity());
// ARMARX_INFO << VAROUT(targetPwm);
targetPwm = 0.0f;
auto targetPWM = static_cast<int>(controller.run(timeSinceLastIteration,
dataPtr->getVelocity(),
newVel,
0));
// ARMARX_RT_LOGF_INFO("target pwm: %d", targetPWM);
// if (std::abs(targetPWM) > 300)
// targetPWM = 0;
dataPtr->setMotorPwmValue(targetPWM);
lastTargetVelocity = newVel;
lastTargetAcceleration = newAcc;
dataPtr->setMotorPwmValue(targetPwm);
// ARMARX_RT_LOGF_INFO("target velocity: %.3f, current velocity: %.3f, target pwm: %d, kp: %.3f ki: %f, kd: %f, max acc: %.3f",
// target.velocity, dataPtr->getVelocity(), targetPWM, pid->Kp, pid->Ki, pid->Kd, controller.acceleration).deactivateSpam(1);
// ARMARX_RT_LOGF_INFO("target velocity: %.3f, current velocity: %.3f, target pwm: %d, kp: %.3f ki: %f, kd: %f, velController acc: %.3f",
// target.velocity, dataPtr->getVelocity(), targetPWM, controller.pid->Kp, controller.pid->Ki, controller.pid->Kd, velController.currentAcc).deactivateSpam(1);
}
......@@ -46,7 +92,9 @@ namespace devices::ethercat::head_board::armar7de::joint_controller
void
StopMovementController::rtPreActivateController()
{
pid->reset();
lastTargetVelocity = dataPtr->getVelocity();
lastTargetAcceleration = dataPtr->getAcceleration();
controller.reset(static_cast<double>(dataPtr->getVelocity()));
}
} // namespace devices::ethercat::head_board::armar7de::joint_controller
......@@ -2,16 +2,29 @@
// armarx
#include <ArmarXCore/core/services/tasks/ThreadPool.h>
#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
#include <RobotAPI/libraries/core/PIDController.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
// robot_devices
#include <devices/ethercat/head_board/armar7de/PWMVelocity.h>
namespace armarx
{
using PIDControllerPtr = std::shared_ptr<class PIDController>;
}
namespace devices::ethercat::head_board::armar7de
{
using DataPtr = std::shared_ptr<class Data>;
using PWMVelocityControllerConfigurationCPtr =
std::shared_ptr<const class PWMVelocityControllerConfiguration>;
}
namespace devices::ethercat::head_board::armar7de::joint_controller
{
......@@ -23,6 +36,7 @@ namespace devices::ethercat::head_board::armar7de::joint_controller
public:
StopMovementController(DataPtr dataPtr);
~StopMovementController() noexcept(true);
private:
void rtRun(const IceUtil::Time& sensorValuesTimestamp,
......@@ -37,9 +51,16 @@ namespace devices::ethercat::head_board::armar7de::joint_controller
void rtPreActivateController() override;
private:
PWMVelocityControllerConfigurationCPtr config;
devices::ethercat::head_board::armar7de::PWMVelocityController controller;
armarx::VelocityControllerWithRampedAccelerationAndPositionBounds velController;
std::atomic<float> lastTargetVelocity, lastTargetAcceleration;
armarx::DummyControlTargetStopMovement target;
DataPtr dataPtr;
armarx::PIDControllerPtr pid;
bool stopRequested = false;
mutable armarx::ThreadPool::Handle threadHandle;
};
} // 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