BUGFIX S-Curve Deceleration Algorithm