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.
- 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_gtherefore limits how much of the velocity range the position loop may use on its own.- While this clamp is active
vel_satcounts up byperiod, otherwise it counts down to 0. - With
pos_en <= 0,pos_error, the P velocity andvel_satare 0 (pure velocity mode).
- 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 atmax_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_dacts like a damping factor: larger values give a slower integrator.- With
vel_en <= 0the velocity error,acc_cmdand the integrator are 0.
- Acceleration to torque:
acc_cmdis split by a first order low pass with cutoffj_lpf(Hz):acc_cmd_lpand the high pass restacc_cmd_hp. Withj_lpf <= 0the filter passes everything, so all ofacc_cmdis inacc_cmd_lp.fb_torque_cmd = acc_cmd_hp * j_mot + acc_cmd_lp * (j_mot + j_sys), so the load inertiaj_sysis only used for the low frequency part.fb_torque_cmdis 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_satcounts up byperiodwhilefb_torque_cmdis beyond 99 % of that limit, otherwise down to 0.
- Feedforward:
ff_torque_cmd = acc_ext_cmd * (j_mot + j_sys) + d * vel_cmd + f * SIGN2(vel_cmd, max_vel * 0.001) + o;
fis Coulomb friction, applied with a linear ramp for speeds below 0.1 % ofmax_vel;dis viscous damping;ois a constant offset (e.g. gravity).
- 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 tofault0.sat, which trips a saturation error when it exceedsfault0.max_sat.
- Regenerative stop (
stop):
- While
en > 0andstop > 0(linked tofault0.rstop) the position loop is off (pos_error, the P velocity andvel_satare 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. Thed,fandofeedforward terms still act on the (zero based)vel_cmd. - Because
pos_erroris held at 0,fault0cannot trip on a position error during the stop.
- 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_veland the external acceleration command below 1 rad/s^2 forhold_tseconds (not exactly 0: differentiating the in-drive stp position leaves float noise of about 1e-5 rad/s^2 at rest), andabs(pos_error) < hold_arm(or< hold_winonce the error has not shrunk forhold_stk: the axis is stuck).holdis 1 then and stays 1 whileabs(pos_error) < hold_win. Until then the loop runs normally and pulls the error in. Starting onhold_winfroze 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 abouthold_arm(harness Z with dob: 1.83 mrad onhold_win, 0.48 withhold_arm0.5 mrad, 0.11 with 0.1 mrad, the hold 0.07 s later, same 10.8 A).hold_stkmust 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
holdis 1 the integrator is frozen and slewed so thattorque_cmdmoves tohold_oathold_rateNm/s.hold_ois the moving offsetofromid_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 leaveshold_win,holdends 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_en2 is for use with the disturbance observer (dob0.k> 0):holdand the frozen integrator only, anddob0.hold = pid0.holddoes the slew in dob, which would otherwise fight it (see dob). With dob,hold_en1 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 withhold_en1 (the creep back tohold_armtakes about 0.9 s); with dob (f = d = 0,dob0.max8) 17.6 / 3.3 A becomes 10.8 / 10.8 A withhold_en2, started within 0.5 s.
- 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 fromconf0, with the negative limits set to the same value as the positive ones).max_velmust also be non zero because the friction feedforward divides by it.
- All limits (
Pins #
| Name | Description |
|---|---|
| pos_ext_cmd | input, Position command (rad) |
| pos_fb | input, Position feedback (rad) |
| pos_error | output, Position error, wrapped to +-pi (rad) |
| vel_ext_cmd | input, Velocity feedforward command (rad/s) |
| vel_fb | input, Velocity feedback (rad/s) |
| vel_cmd | output, Clamped velocity command of the velocity loop (rad/s) |
| vel_error | output, Velocity error (rad/s) |
| acc_ext_cmd | input, Acceleration feedforward command (rad/s^2) |
| acc_cmd | output, Acceleration command of the velocity loop (rad/s^2) |
| acc_cmd_lp | output, Low pass part of acc_cmd, scaled with j_mot + j_sys (rad/s^2) |
| acc_cmd_hp | output, High pass part of acc_cmd, scaled with j_mot only (rad/s^2) |
| torque_ext_cmd | input, Additional torque command (Nm) |
| fb_torque_cmd | output, Feedback torque incl. integrator (Nm) |
| ff_torque_cmd | output, Feedforward torque (Nm) |
| torque_cmd | output, Total clamped torque command (Nm) |
| torque_sum | output, Velocity loop integrator (Nm) |
| en | input, Enable, 0 resets all outputs and the integrator |
| stop | input, > 0 = regenerative stop: position loop off, zero speed, no external feedforward (fault0.rstop) |
| pos_en | parameter, Enable position loop (default 1) |
| vel_en | parameter, Enable velocity loop (default 1) |
| pos_bw | parameter, Position loop bandwidth / P gain (1/s, default 100) |
| vel_bw | parameter, Velocity loop bandwidth / P gain (1/s, default 2000) |
| vel_d | parameter, Velocity loop damping, integrator gain is vel_bw^2 / vel_d (default 2, min 0.1) |
| vel_g | parameter, Fraction of max_vel the position loop may command (default 0.1) |
| torque_g | parameter, Fraction of max_torque the feedback path may command (default 1) |
| scale | input, Gain scale for pos_bw and vel_bw (default 1) |
| j_lpf | parameter, Cutoff of the j_sys low pass (Hz), 0 = no filter |
| acc_g | parameter, Not used by the code |
| j_mot | parameter, Motor inertia (kgm^2) |
| f | parameter, Coulomb friction feedforward (Nm) |
| d | parameter, Viscous damping feedforward (Nm/(rad/s)) |
| j_sys | parameter, Load inertia (kgm^2) |
| o | parameter, Constant torque offset feedforward (Nm) |
| max_vel | parameter, Maximum positive velocity (rad/s) |
| neg_min_vel | parameter, Maximum negative velocity as positive number (rad/s) |
| max_acc | parameter, Not used by the code, acc_cmd is not clamped (rad/s^2) |
| max_torque | parameter, Maximum positive torque (Nm) |
| neg_min_torque | parameter, Maximum negative torque as positive number (Nm) |
| vel_sat | output, Time the position loop output is saturated (s) |
| torque_sat | output, Time the feedback torque is saturated (s) |
| sat | output, max(vel_sat, torque_sat), to fault0.sat (s) |
| hold_en | parameter, 1 = standstill hold, 2 = hold flag and frozen integrator only (dob0 does the slew), default 0 |
| hold_o | parameter, Torque the hold moves to at rest (Nm), conf0.o |
| hold_win | parameter, Position error window for the hold (rad), default 0.002 |
| hold_arm | parameter, Position error the hold starts inside (rad), default 0.0001 |
| hold_stk | parameter, Time the error has not shrunk before the hold starts anywhere inside hold_win (s), default 0.3 |
| hold_t | parameter, Time at rest before the hold starts (s), default 0.1 |
| hold_rate | parameter, Slew rate of the torque to hold_o (Nm/s), default 20 |
| hold | output, 1 while the hold is active |
References #
Source: .c File