fafo/OpenMicroManipulator
17
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Version v1.0.1:

Browse source 
* 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)
This commit is contained in:
0x23 2025-09-19 09:24:56 +02:00
parent 2cf353e7fc
commit d9888ef369
27 changed files with 1723 additions and 784 deletions
Download patch file Download diff file Expand all files Collapse all files

56
firmware/MotionControllerRP/src/servo_control/pid.cpp
View file

@ -20,37 +20,37 @@ void PIDController::set_parameter(float kP, float kI, float kD, float output_lim
// PID controller function
float PIDController::compute(float error, float dt, float one_over_dt) {
// Proportional component
float proportional = kP * error;
float output = proportional;
// 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;
}
// 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;
}
// 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;
// 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;
integral_prev = 0.0f;
error_prev = 0.0f;
}
//--- LowpassFilter -----------------------------------------------------------
@ -67,4 +67,8 @@ float LowpassFilter::update(float value, float dt) {
float v = value_prev*alpha + (1.0f - alpha)*value;
value_prev = v;
return v;
}
}
void LowpassFilter::reset(float value) {
value_prev = value;
}