Skip to content

Commit 1db7013

Browse files
mikeshubthinkyhead
mikeshub
authored and
thinkyhead
committed
Fix and improve PID loops (#14373)
- Windup guarding was missing. The kludge in place of windup guard is removed. D term filter calculations are simplified to require fewer `float` calculations. Sign change for D term output to make debugging output clearer. - Use "no overshoot" for bed PID tuning.
1 parent 17778d1 commit 1db7013

File tree

1 file changed

+39
-44
lines changed

1 file changed

+39
-44
lines changed

‎Marlin/src/module/temperature.cpp

Lines changed: 39 additions & 44 deletions
Original file line numberDiff line numberDiff line change
@@ -350,11 +350,13 @@ temp_range_t Temperature::temp_range[HOTENDS] = ARRAY_BY_HOTENDS(sensor_heater_0
350350
PID_t tune_pid = { 0, 0, 0 };
351351
float max = 0, min = 10000;
352352

353+
const bool isbed = (heater < 0);
354+
353355
#if HAS_PID_FOR_BOTH
354-
#define GHV(B,H) (heater < 0 ? (B) : (H))
355-
#define SHV(B,H) do{ if (heater < 0) temp_bed.soft_pwm_amount = B; else temp_hotend[heater].soft_pwm_amount = H; }while(0)
356-
#define ONHEATINGSTART() (heater < 0 ? printerEventLEDs.onBedHeatingStart() : printerEventLEDs.onHotendHeatingStart())
357-
#define ONHEATING(S,C,T) do{ if (heater < 0) printerEventLEDs.onBedHeating(S,C,T); else printerEventLEDs.onHotendHeating(S,C,T); }while(0)
356+
#define GHV(B,H) (isbed ? (B) : (H))
357+
#define SHV(B,H) do{ if (isbed) temp_bed.soft_pwm_amount = B; else temp_hotend[heater].soft_pwm_amount = H; }while(0)
358+
#define ONHEATINGSTART() (isbed ? printerEventLEDs.onBedHeatingStart() : printerEventLEDs.onHotendHeatingStart())
359+
#define ONHEATING(S,C,T) (isbed ? printerEventLEDs.onBedHeating(S,C,T) : printerEventLEDs.onHotendHeating(S,C,T))
358360
#elif ENABLED(PIDTEMPBED)
359361
#define GHV(B,H) B
360362
#define SHV(B,H) (temp_bed.soft_pwm_amount = B)
@@ -370,7 +372,7 @@ temp_range_t Temperature::temp_range[HOTENDS] = ARRAY_BY_HOTENDS(sensor_heater_0
370372
#if WATCH_BED || WATCH_HOTENDS
371373
#define HAS_TP_BED BOTH(THERMAL_PROTECTION_BED, PIDTEMPBED)
372374
#if HAS_TP_BED && BOTH(THERMAL_PROTECTION_HOTENDS, PIDTEMP)
373-
#define GTV(B,H) (heater < 0 ? (B) : (H))
375+
#define GTV(B,H) (isbed ? (B) : (H))
374376
#elif HAS_TP_BED
375377
#define GTV(B,H) (B)
376378
#else
@@ -456,23 +458,25 @@ temp_range_t Temperature::temp_range[HOTENDS] = ARRAY_BY_HOTENDS(sensor_heater_0
456458

457459
SERIAL_ECHOPAIR(MSG_BIAS, bias, MSG_D, d, MSG_T_MIN, min, MSG_T_MAX, max);
458460
if (cycles > 2) {
459-
float Ku = (4.0f * d) / (float(M_PI) * (max - min) * 0.5f),
460-
Tu = ((float)(t_low + t_high) * 0.001f);
461-
tune_pid.Kp = 0.6f * Ku;
461+
const float Ku = (4.0f * d) / (float(M_PI) * (max - min) * 0.5f),
462+
Tu = float(t_low + t_high) * 0.001f,
463+
pf = isbed ? 0.2f : 0.6f,
464+
df = isbed ? 1.0f / 3.0f : 1.0f / 8.0f;
465+
tune_pid.Kp = Ku * pf;
466+
tune_pid.Kd = tune_pid.Kp * Tu * df;
462467
tune_pid.Ki = 2 * tune_pid.Kp / Tu;
463-
tune_pid.Kd = tune_pid.Kp * Tu * 0.125f;
464468
SERIAL_ECHOPAIR(MSG_KU, Ku, MSG_TU, Tu);
465469
SERIAL_ECHOLNPGM("\n" MSG_CLASSIC_PID);
466470
SERIAL_ECHOLNPAIR(MSG_KP, tune_pid.Kp, MSG_KI, tune_pid.Ki, MSG_KD, tune_pid.Kd);
467471
/**
468-
tune_pid.Kp = 0.33*Ku;
469-
tune_pid.Ki = tune_pid.Kp/Tu;
470-
tune_pid.Kd = tune_pid.Kp*Tu/3;
472+
tune_pid.Kp = 0.33 * Ku;
473+
tune_pid.Ki = tune_pid.Kp / Tu;
474+
tune_pid.Kd = tune_pid.Kp * Tu / 3;
471475
SERIAL_ECHOLNPGM(" Some overshoot");
472476
SERIAL_ECHOLNPAIR(" Kp: ", tune_pid.Kp, " Ki: ", tune_pid.Ki, " Kd: ", tune_pid.Kd, " No overshoot");
473-
tune_pid.Kp = 0.2*Ku;
474-
tune_pid.Ki = 2*tune_pid.Kp/Tu;
475-
tune_pid.Kd = tune_pid.Kp*Tu/3;
477+
tune_pid.Kp = 0.2 * Ku;
478+
tune_pid.Ki = 2 * tune_pid.Kp / Tu;
479+
tune_pid.Kd = tune_pid.Kp * Tu / 3;
476480
SERIAL_ECHOPAIR(" Kp: ", tune_pid.Kp, " Ki: ", tune_pid.Ki, " Kd: ", tune_pid.Kd);
477481
*/
478482
}
@@ -496,7 +500,7 @@ temp_range_t Temperature::temp_range[HOTENDS] = ARRAY_BY_HOTENDS(sensor_heater_0
496500
// Report heater states every 2 seconds
497501
if (ELAPSED(ms, next_temp_ms)) {
498502
#if HAS_TEMP_SENSOR
499-
print_heater_states(heater >= 0 ? heater : active_extruder);
503+
print_heater_states(isbed ? active_extruder : heater);
500504
SERIAL_EOL();
501505
#endif
502506
next_temp_ms = ms + 2000UL;
@@ -507,9 +511,9 @@ temp_range_t Temperature::temp_range[HOTENDS] = ARRAY_BY_HOTENDS(sensor_heater_0
507511
#if WATCH_BED && WATCH_HOTENDS
508512
true
509513
#elif WATCH_HOTENDS
510-
heater >= 0
514+
!isbed
511515
#else
512-
heater < 0
516+
isbed
513517
#endif
514518
) {
515519
if (!heated) { // If not yet reached target...
@@ -569,7 +573,7 @@ temp_range_t Temperature::temp_range[HOTENDS] = ARRAY_BY_HOTENDS(sensor_heater_0
569573
// Use the result? (As with "M303 U1")
570574
if (set_result) {
571575
#if HAS_PID_FOR_BOTH
572-
if (heater < 0) _SET_BED_PID(); else _SET_EXTRUDER_PID();
576+
if (isbed) _SET_BED_PID(); else _SET_EXTRUDER_PID();
573577
#elif ENABLED(PIDTEMP)
574578
_SET_EXTRUDER_PID();
575579
#else
@@ -805,9 +809,7 @@ float Temperature::get_pid_output(const int8_t e) {
805809
static float temp_iState[HOTENDS] = { 0 },
806810
temp_dState[HOTENDS] = { 0 };
807811
static bool pid_reset[HOTENDS] = { false };
808-
float pid_error = temp_hotend[HOTEND_INDEX].target - temp_hotend[HOTEND_INDEX].current;
809-
work_pid[HOTEND_INDEX].Kd = PID_K2 * PID_PARAM(Kd, HOTEND_INDEX) * (temp_hotend[HOTEND_INDEX].current - temp_dState[HOTEND_INDEX]) + float(PID_K1) * work_pid[HOTEND_INDEX].Kd;
810-
temp_dState[HOTEND_INDEX] = temp_hotend[HOTEND_INDEX].current;
812+
const float pid_error = temp_hotend[HOTEND_INDEX].target - temp_hotend[HOTEND_INDEX].current;
811813

812814
if (temp_hotend[HOTEND_INDEX].target == 0
813815
|| pid_error < -(PID_FUNCTIONAL_RANGE)
@@ -825,13 +827,17 @@ float Temperature::get_pid_output(const int8_t e) {
825827
else {
826828
if (pid_reset[HOTEND_INDEX]) {
827829
temp_iState[HOTEND_INDEX] = 0.0;
830+
work_pid[HOTEND_INDEX].Kd = 0.0;
828831
pid_reset[HOTEND_INDEX] = false;
829832
}
830-
temp_iState[HOTEND_INDEX] += pid_error;
833+
834+
work_pid[HOTEND_INDEX].Kd = work_pid[HOTEND_INDEX].Kd + PID_K2 * (PID_PARAM(Kd, HOTEND_INDEX) * (temp_dState[HOTEND_INDEX] - temp_hotend[HOTEND_INDEX].current) - work_pid[HOTEND_INDEX].Kd);
835+
const float max_power_over_i_gain = (float)PID_MAX / PID_PARAM(Ki, HOTEND_INDEX);
836+
temp_iState[HOTEND_INDEX] = constrain(temp_iState[HOTEND_INDEX] + pid_error, 0, max_power_over_i_gain);
831837
work_pid[HOTEND_INDEX].Kp = PID_PARAM(Kp, HOTEND_INDEX) * pid_error;
832838
work_pid[HOTEND_INDEX].Ki = PID_PARAM(Ki, HOTEND_INDEX) * temp_iState[HOTEND_INDEX];
833839

834-
pid_output = work_pid[HOTEND_INDEX].Kp + work_pid[HOTEND_INDEX].Ki - work_pid[HOTEND_INDEX].Kd;
840+
pid_output = work_pid[HOTEND_INDEX].Kp + work_pid[HOTEND_INDEX].Ki + work_pid[HOTEND_INDEX].Kd;
835841

836842
#if ENABLED(PID_EXTRUSION_SCALING)
837843
work_pid[HOTEND_INDEX].Kc = 0;
@@ -850,15 +856,9 @@ float Temperature::get_pid_output(const int8_t e) {
850856
}
851857
#endif // PID_EXTRUSION_SCALING
852858

853-
if (pid_output > PID_MAX) {
854-
if (pid_error > 0) temp_iState[HOTEND_INDEX] -= pid_error; // conditional un-integration
855-
pid_output = PID_MAX;
856-
}
857-
else if (pid_output < 0) {
858-
if (pid_error < 0) temp_iState[HOTEND_INDEX] -= pid_error; // conditional un-integration
859-
pid_output = 0;
860-
}
859+
pid_output = constrain(pid_output, 0, PID_MAX);
861860
}
861+
temp_dState[HOTEND_INDEX] = temp_hotend[HOTEND_INDEX].current;
862862

863863
#else // PID_OPENLOOP
864864

@@ -908,23 +908,18 @@ float Temperature::get_pid_output(const int8_t e) {
908908
static PID_t work_pid = { 0 };
909909
static float temp_iState = 0, temp_dState = 0;
910910

911-
float pid_error = temp_bed.target - temp_bed.current;
912-
temp_iState += pid_error;
911+
const float max_power_over_i_gain = (float)MAX_BED_POWER / temp_bed.pid.Ki,
912+
pid_error = temp_bed.target - temp_bed.current;
913+
914+
temp_iState = constrain(temp_iState + pid_error, 0, max_power_over_i_gain);
915+
913916
work_pid.Kp = temp_bed.pid.Kp * pid_error;
914917
work_pid.Ki = temp_bed.pid.Ki * temp_iState;
915-
work_pid.Kd = PID_K2 * temp_bed.pid.Kd * (temp_bed.current - temp_dState) + PID_K1 * work_pid.Kd;
918+
work_pid.Kd = work_pid.Kd + PID_K2 * (temp_bed.pid.Kd * (temp_dState - temp_bed.current) - work_pid.Kd);
916919

917920
temp_dState = temp_bed.current;
918921

919-
float pid_output = work_pid.Kp + work_pid.Ki - work_pid.Kd;
920-
if (pid_output > MAX_BED_POWER) {
921-
if (pid_error > 0) temp_iState -= pid_error; // conditional un-integration
922-
pid_output = MAX_BED_POWER;
923-
}
924-
else if (pid_output < 0) {
925-
if (pid_error < 0) temp_iState -= pid_error; // conditional un-integration
926-
pid_output = 0;
927-
}
922+
const float pid_output = constrain(work_pid.Kp + work_pid.Ki + work_pid.Kd, 0, MAX_BED_POWER);
928923

929924
#else // PID_OPENLOOP
930925

0 commit comments

Comments
 (0)