limit3.comp
1 component limit3 """Follow input signal while obeying limits 2 Limit the output signal to fall between min and max, limit its slew 3 rate to less than maxv per second, and limit its second derivative to 4 less than maxa per second squared. When the signal is a position, 5 this means that the position, velocity, and acceleration are limited."""; 6 pin in float in; 7 pin in bit enable = 1 "1: out follows in, 0: out returns to 0 (always per constraints)"; 8 pin out float out; 9 pin in bit load=0 10 """When TRUE, immediately set \\fBout\\fB to \\fBin\\fR, ignoring maxv 11 and maxa"""; 12 pin in float min_=-1e20; 13 pin in float max_=1e20; 14 pin in float maxv=1e20; 15 pin in float maxa=1e20; 16 pin in u32 smooth_steps=2 17 """Smooth out acceleration this many periods before reaching input or 18 max/min limit. Higher values avoid oscillation, but will accelerate 19 slightly more slowly."""; 20 variable double in_pos_old; 21 variable double out_old; 22 function _; 23 license "GPL"; 24 ;; 25 26 #include "rtapi_math.h" 27 28 #define SET_NEXT_STATE(_out, _in) \ 29 do { \ 30 out_old = out; \ 31 out = _out; \ 32 in_pos_old = _in; \ 33 return; \ 34 } while (0) 35 36 #define VALID_NEXT(pos) ((pos) <= max_pos && (pos) >= min_pos) 37 38 // Distance = avg. velocity * time 39 #define S_GIVEN_VI_VF_T(vi,vf,t) (((vf) + (vi))/2 * (t)) 40 // Time = chg. velocity / acceleration 41 #define T_GIVEN_VI_VF_A(vi,vf,a) (((vf) - (vi)) / (a)) 42 // Final velocity = initial velocity + acceleration * time 43 #define VF_GIVEN_VI_A_T(vi,a,t) ((vi) + (a)*(t)) 44 // A fudge amount for division errors 45 #define EPSILON 1e-9 46 47 FUNCTION(_) { 48 double invalue; 49 double in_pos_lim, in_vel; 50 double min_vel, max_vel, min_pos, max_pos; 51 double stop_pos_max, stop_pos_min; 52 double stop_time_max, stop_time_min; 53 double in_vel_time_max, in_vel_time_min; 54 double out_pos_max, out_pos_min, in_pos_max, in_pos_min; 55 double ach_pos_min, ach_pos_max; 56 57 double out_vel = (out-out_old)/fperiod; 58 double goal_pos_min, goal_pos_max, goal_pos_cur; 59 double pos_diff, vel_diff, goal_pos_prev; 60 double t, ti, a, v, s; 61 62 if (enable) { 63 invalue = in; // out pin follows in pin per limits 64 } else { 65 invalue = 0; // out pin returns to 0 per limits 66 // so steady-state out==0 67 } 68 69 if (load) { 70 // Apply first order limit 71 in_pos_lim = fmin(max_, fmax(min_, invalue)); 72 SET_NEXT_STATE(in_pos_lim, in_pos_lim); 73 return; 74 } 75 76 // Principal of operation: 77 // 78 // 1. Calculate shortest distance (at max acceleration) to 79 // stop (i.e. reach vel=0) and to match the input velocity 80 // 2. Compare our projected positions and choose whether to worry 81 // about the max/min limits or to follow the input signal 82 // 3. Adjust acceleration according to decision and return 83 84 // 1. Calculate distances and times to stop and match input velocity 85 // 86 // Input and output velocity 87 in_vel = (invalue - in_pos_old) / fperiod; 88 out_vel = (out - out_old) / fperiod; 89 // 90 // Most negative/positive velocity reachable in one period 91 min_vel = fmax(VF_GIVEN_VI_A_T(out_vel, -maxa, fperiod), -maxv); 92 max_vel = fmin(VF_GIVEN_VI_A_T(out_vel, maxa, fperiod), maxv); 93 // Most negative/positive position reachable in one period 94 // - cur. pos + (distance to reach min/max vel in one period) 95 min_pos = out + min_vel * fperiod; 96 max_pos = out + max_vel * fperiod; 97 // 98 // Shortest possible distance to stop 99 // - time to decel to 0; start from previous period 100 stop_time_max = fabs(T_GIVEN_VI_VF_A(max_vel, 0.0, maxa)) + fperiod; 101 stop_time_min = fabs(T_GIVEN_VI_VF_A(min_vel, 0.0, maxa)) + fperiod; 102 // - distance to stop from max_pos/min_pos 103 stop_pos_max = out + S_GIVEN_VI_VF_T(max_vel, 0.0, stop_time_max); 104 stop_pos_min = out + S_GIVEN_VI_VF_T(min_vel, 0.0, stop_time_min); 105 // 106 // Shortest possible distance to match input velocity 107 // - time to match input velocity from this period; out runs 1 period behind 108 in_vel_time_max = fabs(T_GIVEN_VI_VF_A(max_vel, in_vel, maxa)) - fperiod; 109 in_vel_time_min = fabs(T_GIVEN_VI_VF_A(min_vel, in_vel, maxa)) - fperiod; 110 // - output position after velocity match 111 out_pos_max = max_pos + S_GIVEN_VI_VF_T(max_vel, in_vel, in_vel_time_max); 112 out_pos_min = min_pos + S_GIVEN_VI_VF_T(min_vel, in_vel, in_vel_time_min); 113 // - input position after velocity match 114 in_pos_max = invalue + in_vel * in_vel_time_max; 115 in_pos_min = invalue + in_vel * in_vel_time_min; 116 117 // 2. Choose the current goal: input signal, max limit or min limit 118 // 119 // Min/Max limits: 120 // - assume we're stopping at a limit by default 121 vel_diff = -out_vel; 122 ach_pos_min = stop_pos_min; 123 ach_pos_max = stop_pos_max; 124 // - are we headed to crash into a min/max limit? 125 if (stop_pos_max > max_ + EPSILON && !VALID_NEXT(max_)) 126 goal_pos_min = goal_pos_max = goal_pos_cur = goal_pos_prev = max_; 127 else if (stop_pos_min < min_ - EPSILON && !VALID_NEXT(min_)) 128 goal_pos_min = goal_pos_max = goal_pos_cur = goal_pos_prev = min_; 129 // - if input is outside min/max limit but heading back in, is 130 // there time to keep heading toward the limit before we need to 131 // start running to meet the input signal? 132 else if (invalue >= max_ && in_pos_max > out_pos_max) 133 goal_pos_min = goal_pos_max = goal_pos_cur = goal_pos_prev = max_; 134 else if (invalue <= min_ && in_pos_min < out_pos_min) 135 goal_pos_min = goal_pos_max = goal_pos_cur = goal_pos_prev = min_; 136 // 137 // Input signal: 138 // - no min/max constraints; chase the input signal 139 else { 140 goal_pos_min = in_pos_min; 141 goal_pos_max = in_pos_max; 142 goal_pos_cur = invalue; 143 goal_pos_prev = in_pos_old; 144 vel_diff = out_vel - in_vel; 145 ach_pos_min = out_pos_min; 146 ach_pos_max = out_pos_max; 147 } 148 149 // 3. Adjust acceleration 150 // 151 // - Difference in position, last cycle 152 pos_diff = out - goal_pos_prev; 153 // - Time to reach goal position and velocity with uniform acceleration 154 if (fabs(vel_diff) < EPSILON) 155 t = 0; 156 else 157 t = pos_diff / ((vel_diff + 0) / 2); // t = dp / (avg dv) 158 159 // - If current position and velocity are close enough to reach 160 // goal position in this period, and maintaining goal velocity 161 // in the next period doesn't violate acceleration constraints, 162 // pass the input straight to the output 163 if (VALID_NEXT(goal_pos_cur) && fabs(t) <= fperiod) 164 SET_NEXT_STATE(goal_pos_cur, invalue); 165 166 // - If no danger of overshoot, accel at max in direction of goal 167 if (ach_pos_max < goal_pos_max + EPSILON) 168 // Max pos. accel toward goal will still fall short 169 SET_NEXT_STATE(max_pos, invalue); 170 if (ach_pos_min > goal_pos_min - EPSILON) 171 // Max neg. accel toward goal will still fall short 172 SET_NEXT_STATE(min_pos, invalue); 173 174 // - If close to reaching goal, try to grease a landing; always 175 // using max acceleration can result in oscillating around the 176 // goal but never quite getting things right to 'lock' onto it 177 if (fabs(t) < fperiod * smooth_steps) { 178 // - Round up the magnitude of time to an integral number of periods 179 # define SIGN(n) (((n)>=0) ? 1 : -1) 180 ti = (int)((t - EPSILON*SIGN(t)) / fperiod + SIGN(t)) * fperiod; 181 // - Uniform acceleration to reach goal in time `ti` 182 a = (vel_diff - 0) / ti; 183 v = out_vel + a * fperiod; 184 s = v * fperiod; 185 // - Effect new position, within limits 186 SET_NEXT_STATE(fmin(max_pos, fmax(min_pos, out + s)), invalue); 187 } 188 189 // - If moving toward goal and in danger of overshoot, accelerate 190 // at max in opposite direction of goal 191 if (goal_pos_max + EPSILON < ach_pos_max && goal_pos_prev > out) 192 // Heading up from below 193 SET_NEXT_STATE(min_pos, invalue); 194 if (goal_pos_min - EPSILON > ach_pos_min && goal_pos_prev < out) 195 // Heading down from above 196 SET_NEXT_STATE(max_pos, invalue); 197 198 // - Shouldn't get here; coast 199 SET_NEXT_STATE((min_pos+max_pos)/2, invalue); 200 }