d9888ef369
* Improved homing (parallel homing support, better repeatability, better geometric reference point) * Improved joint calibration procedure * Calibration data can now be stored persistently on the flash memory (no repeated calibration required) * Improved logging * added PythonAPI to control device easily New G-Code commands: * Enable/Disable motors command, including pose recovery from current position on motor enable * Dedicated joint calibration command with save to flash option * Set pose command to directly set a target pose for the servo loops, bypassing the motion controller (good for real-time control)
74 lines
1.9 KiB
C++
74 lines
1.9 KiB
C++
#include "pid.h"
|
|
#include <algorithm>
|
|
|
|
PIDController::PIDController()
|
|
: kP(0.0f), kI(0.0f), kD(0.0f), kI_half(0.0f)
|
|
, output_limit(0.0f), windup_limit(0.0f)
|
|
, error_prev(0.0f), integral_prev(0.0f)
|
|
{
|
|
}
|
|
|
|
void PIDController::set_parameter(float kP, float kI, float kD, float output_limit, float windup_limit) {
|
|
PIDController::kP = kP;
|
|
PIDController::kI = kI;
|
|
PIDController::kD = kD;
|
|
PIDController::output_limit = output_limit;
|
|
PIDController::windup_limit = windup_limit;
|
|
|
|
PIDController::kI_half = kI*0.5f;
|
|
}
|
|
|
|
// PID controller function
|
|
float PIDController::compute(float error, float dt, float one_over_dt) {
|
|
// Proportional component
|
|
float proportional = kP * error;
|
|
float output = proportional;
|
|
|
|
// Integral component
|
|
if(kI != 0.0f) {
|
|
// Tustin transform of the integral part
|
|
// u_ik = u_ik_1 + I*Ts/2*(ek + ek_1)
|
|
float integral = integral_prev + kI_half*dt*(error + error_prev);
|
|
integral = std::clamp(integral, -windup_limit, windup_limit);
|
|
output += integral;
|
|
integral_prev = integral;
|
|
}
|
|
|
|
// Derivative component
|
|
if(kD != 0.0f) {
|
|
// u_dk = D(ek - ek_1)/Ts
|
|
float derivative = kD*(error - error_prev)*one_over_dt;
|
|
output += derivative;
|
|
}
|
|
|
|
// clamp output and store error
|
|
output = std::clamp(output, -output_limit, output_limit);
|
|
error_prev = error;
|
|
|
|
return output;
|
|
}
|
|
|
|
void PIDController::reset(){
|
|
integral_prev = 0.0f;
|
|
error_prev = 0.0f;
|
|
}
|
|
|
|
//--- LowpassFilter -----------------------------------------------------------
|
|
|
|
LowpassFilter::LowpassFilter(): value_prev(0.0f), time_constant(1.0f) {
|
|
}
|
|
|
|
void LowpassFilter::set_time_constant(float time_constant) {
|
|
LowpassFilter::time_constant = time_constant;
|
|
}
|
|
|
|
float LowpassFilter::update(float value, float dt) {
|
|
float alpha = time_constant/(time_constant + dt);
|
|
float v = value_prev*alpha + (1.0f - alpha)*value;
|
|
value_prev = v;
|
|
return v;
|
|
}
|
|
|
|
void LowpassFilter::reset(float value) {
|
|
value_prev = value;
|
|
}
|