Pid

PID #

Brief #

pid is the standard cascaded position / velocity controller of the F4 board. It turns a position command into a torque command (Nm) using a proportional position loop, a PI velocity loop with inertia scaling, and friction / damping / inertia feedforward. It is loaded by conf/template/pid.txt (and most machine configs), where pid0.pos_ext_cmd = reslimit0.pos_out, pid0.vel_ext_cmd = vel0.vel, pid0.pos_fb = fb_switch0.pos_fb, pid0.vel_fb = vel1.vel, pid0.en = fault0.en_pid, pid0.stop = fault0.rstop, and pid0.torque_cmd feeds the torque-to-current component (pmsm_ttc, dc_ttc, …). Gains and limits are linked from conf0 (pos_bw, vel_bw, vel_d, j, max_vel, max_force, …).

Component Explanation #

All work is done in rt. When en is 0 every output, the integrator and the saturation timers are reset to 0.

  1. Position loop (pos -> vel), active when pos_en > 0:
  • pos_error = minus(pos_ext_cmd, pos_fb), the shortest angular difference.
  • vel_cmd = pos_error * pos_bw * scale, clamped to [-neg_min_vel * vel_g, max_vel * vel_g]. vel_g therefore limits how much of the velocity range the position loop may use on its own.
  • While this clamp is active vel_sat counts up by period, otherwise it counts down to 0.
  • With pos_en <= 0, pos_error, the P velocity and vel_sat are 0 (pure velocity mode).
  1. Velocity loop (vel -> acc), active when vel_en > 0:
  • vel_cmd += vel_ext_cmd (velocity feedforward), then clamped to [-neg_min_vel, max_vel].
  • vel_error = vel_cmd - vel_fb, acc_cmd = vel_error * vel_bw * scale. The P term is not clamped at max_acc: a clamped P term leaves the integrator to run the loop after a large error, which latches a torque-saturated limit cycle; the torque clamp is the limit.
  • The integral part is accumulated directly as torque:
torque_sum += vel_error * vel_bw^2 * scale^2 / MAX(vel_d, 0.1) * (j_mot + j_sys) * period;
  • vel_d acts like a damping factor: larger values give a slower integrator.
  • With vel_en <= 0 the velocity error, acc_cmd and the integrator are 0.
  1. Acceleration to torque:
  • acc_cmd is split by a first order low pass with cutoff j_lpf (Hz): acc_cmd_lp and the high pass rest acc_cmd_hp. With j_lpf <= 0 the filter passes everything, so all of acc_cmd is in acc_cmd_lp.
  • fb_torque_cmd = acc_cmd_hp * j_mot + acc_cmd_lp * (j_mot + j_sys), so the load inertia j_sys is only used for the low frequency part.
  • fb_torque_cmd is clamped to [-neg_min_torque * torque_g, max_torque * torque_g], then the integrator is clamped dynamically to the remaining headroom (anti windup) and added.
  • torque_sat counts up by period while fb_torque_cmd is beyond 99 % of that limit, otherwise down to 0.
  1. Feedforward:
ff_torque_cmd = acc_ext_cmd * (j_mot + j_sys) + d * vel_cmd + f * SIGN2(vel_cmd, max_vel * 0.001) + o;
  • f is Coulomb friction, applied with a linear ramp for speeds below 0.1 % of max_vel; d is viscous damping; o is a constant offset (e.g. gravity).
  1. Output:
  • torque_cmd = CLAMP(torque_ext_cmd + ff_torque_cmd + fb_torque_cmd, -neg_min_torque, max_torque).
  • sat = MAX(vel_sat, torque_sat) (s). It is usually linked to fault0.sat, which trips a saturation error when it exceeds fault0.max_sat.
  1. Regenerative stop (stop):
  • While en > 0 and stop > 0 (linked to fault0.rstop) the position loop is off (pos_error, the P velocity and vel_sat are 0) and the velocity, acceleration and torque feedforward inputs (vel_ext_cmd, acc_ext_cmd, torque_ext_cmd) are ignored, so the velocity loop brakes the motor to zero speed and the energy goes back into the DC link. The d, f and o feedforward terms still act on the (zero based) vel_cmd.
  • Because pos_error is held at 0, fault0 cannot trip on a position error during the stop.
  1. Standstill hold (hold_en, off by default):
  • Why: with stiction a stop ends wherever in the friction band the last move left the torque (on a vertical axis: gravity plus the friction after an up move, gravity minus it after a down move), and the integrator keeps winding on the few encoder counts of error stiction leaves. On Z that is 3 to 18 A standing current depending on the direction of the last move.
  • At rest: the external velocity command has been below 0.1 % of max_vel and the external acceleration command below 1 rad/s^2 for hold_t seconds (not exactly 0: differentiating the in-drive stp position leaves float noise of about 1e-5 rad/s^2 at rest), and abs(pos_error) < hold_arm (or < hold_win once the error has not shrunk for hold_stk: the axis is stuck). hold is 1 then and stays 1 while abs(pos_error) < hold_win. Until then the loop runs normally and pulls the error in. Starting on hold_win froze the integrator on the way back from the stop overshoot, so on the Z bench every stop rested at the window edge (+1.99 mrad after up, -1.7 to -1.8 after down, overshoot 2.95 mrad); the rest error is now about hold_arm (harness Z with dob: 1.83 mrad on hold_win, 0.48 with hold_arm 0.5 mrad, 0.11 with 0.1 mrad, the hold 0.07 s later, same 10.8 A). hold_stk must leave the integrator time to wind through the band: on the Y plant 0.1 s started the hold at 0.2 mrad (1.2 without dob) where 0.3 s lets it reach 0.1 mrad.
  • While hold is 1 the integrator is frozen and slewed so that torque_cmd moves to hold_o at hold_rate Nm/s. hold_o is the moving offset o from id_sys (gravity on a vertical axis, about 0 on a horizontal one); no static friction value is needed, because the static band is not a stable number (on the Z bench the breakaway torque depended on the approach direction and scattered by 2 Nm).
  • If the real band does not reach hold_o, the axis creeps: the position error leaves hold_win, hold ends at once and the integrator takes over from where the hold left it. The hold then stays off until the next move (latched), so it cannot cycle at the window edge (hold, creep out, integrator pulls back, hold again). The window bounds the position error the hold can cost.
  • hold_en 2 is for use with the disturbance observer (dob0.k > 0): hold and the frozen integrator only, and dob0.hold = pid0.hold does the slew in dob, which would otherwise fight it (see dob). With dob, hold_en 1 must not be used.
  • Same on every axis. Harness (tests/sim, the Z and Y plants with the friction measured on the bench, zm_* / ym_*): Z no dob 17.9 / 3.3 A after an up / down stop becomes 10.8 / 10.8 A with hold_en 1 (the creep back to hold_arm takes about 0.9 s); with dob (f = d = 0, dob0.max 8) 17.6 / 3.3 A becomes 10.8 / 10.8 A with hold_en 2, started within 0.5 s.
  1. Defaults (nrt_init):
    • All limits (max_vel, neg_min_vel, max_torque, neg_min_torque) default to 0, which gives zero output; they must be linked (the template links them from conf0, with the negative limits set to the same value as the positive ones). max_vel must also be non zero because the friction feedforward divides by it.

Pins #

NameDescription
pos_ext_cmdinput, Position command (rad)
pos_fbinput, Position feedback (rad)
pos_erroroutput, Position error, wrapped to +-pi (rad)
vel_ext_cmdinput, Velocity feedforward command (rad/s)
vel_fbinput, Velocity feedback (rad/s)
vel_cmdoutput, Clamped velocity command of the velocity loop (rad/s)
vel_erroroutput, Velocity error (rad/s)
acc_ext_cmdinput, Acceleration feedforward command (rad/s^2)
acc_cmdoutput, Acceleration command of the velocity loop (rad/s^2)
acc_cmd_lpoutput, Low pass part of acc_cmd, scaled with j_mot + j_sys (rad/s^2)
acc_cmd_hpoutput, High pass part of acc_cmd, scaled with j_mot only (rad/s^2)
torque_ext_cmdinput, Additional torque command (Nm)
fb_torque_cmdoutput, Feedback torque incl. integrator (Nm)
ff_torque_cmdoutput, Feedforward torque (Nm)
torque_cmdoutput, Total clamped torque command (Nm)
torque_sumoutput, Velocity loop integrator (Nm)
eninput, Enable, 0 resets all outputs and the integrator
stopinput, > 0 = regenerative stop: position loop off, zero speed, no external feedforward (fault0.rstop)
pos_enparameter, Enable position loop (default 1)
vel_enparameter, Enable velocity loop (default 1)
pos_bwparameter, Position loop bandwidth / P gain (1/s, default 100)
vel_bwparameter, Velocity loop bandwidth / P gain (1/s, default 2000)
vel_dparameter, Velocity loop damping, integrator gain is vel_bw^2 / vel_d (default 2, min 0.1)
vel_gparameter, Fraction of max_vel the position loop may command (default 0.1)
torque_gparameter, Fraction of max_torque the feedback path may command (default 1)
scaleinput, Gain scale for pos_bw and vel_bw (default 1)
j_lpfparameter, Cutoff of the j_sys low pass (Hz), 0 = no filter
acc_gparameter, Not used by the code
j_motparameter, Motor inertia (kgm^2)
fparameter, Coulomb friction feedforward (Nm)
dparameter, Viscous damping feedforward (Nm/(rad/s))
j_sysparameter, Load inertia (kgm^2)
oparameter, Constant torque offset feedforward (Nm)
max_velparameter, Maximum positive velocity (rad/s)
neg_min_velparameter, Maximum negative velocity as positive number (rad/s)
max_accparameter, Not used by the code, acc_cmd is not clamped (rad/s^2)
max_torqueparameter, Maximum positive torque (Nm)
neg_min_torqueparameter, Maximum negative torque as positive number (Nm)
vel_satoutput, Time the position loop output is saturated (s)
torque_satoutput, Time the feedback torque is saturated (s)
satoutput, max(vel_sat, torque_sat), to fault0.sat (s)
hold_enparameter, 1 = standstill hold, 2 = hold flag and frozen integrator only (dob0 does the slew), default 0
hold_oparameter, Torque the hold moves to at rest (Nm), conf0.o
hold_winparameter, Position error window for the hold (rad), default 0.002
hold_armparameter, Position error the hold starts inside (rad), default 0.0001
hold_stkparameter, Time the error has not shrunk before the hold starts anywhere inside hold_win (s), default 0.3
hold_tparameter, Time at rest before the hold starts (s), default 0.1
hold_rateparameter, Slew rate of the torque to hold_o (Nm/s), default 20
holdoutput, 1 while the hold is active

References #

Source: .c File