![[Home]](/chips.gif)
History of Simple Tp Notes|
See also blog post: http://www.anderswallin.net/2011/05/emc2-tpruncycle-revisited/ |
 T = target, the position we want to reach P = progress, our current position v_s = the new suggested velocity (see pic) t_m = the total time of the move (see pic) a_m = the maximum allowed acceleration/decceleration v_c = the current velocity t_s = the sampling/cycle time The reasoning/derivation is exactly the same as above. We want the total travel to equal (T-P) and we calculate the total travel as the area under the velocity-curve. The area now splits into the green and red triangles above (math notation is Latex-ish): T-P = {1 \over 2} t_s v_c + {1 \over 2} t_m v_s on the other hand we know that v_s = a_m (t_m - t_s) which we can insert above to get: T-P = {1 \over 2} t_s v_c + {1 \over 2} t_m a_m (t_m - t_s) This is now a quadratic equation in t_m and we can solve for t_m using the quadratic formula to get: t_m = {1 \over 2}t_s + \sqrt{ {t_s^2 \over 4} - {t_s v_c - 2(T-P) \over a_m} } which we insert into the second formula to get our final answer for v_s: v_s = a_m (t_m - t_s) = -{1 \over 2 }a_m t_s + a_m \sqrt{ {t_s^2 \over 4} - {t_s v_c - 2(T-P) \over a_m} } This can be seen to be equivalent to the following lines of C-code (from void tcRunCycle?(TP_STRUCT *tp, TC_STRUCT *tc, double *v, int *on_final_decel)): discr = 0.5 * tc->cycle_time * tc->currentvel - (tc->target - tc->progress); discr = 0.25 * pmSq(tc->cycle_time) - 2.0 / tc->maxaccel * discr; newvel = maxnewvel = -0.5 * tc->maxaccel * tc->cycle_time +tc->maxaccel * pmSqrt(discr); |