MotionPlanner: Fix horrible bug.
MotionPlanner: Fix horrible bug.

johnkingsley/aprinter,johnkingsley/aprinter,fixator/aprinter,johnkingsley/aprinter,ambrop72/aprinter,ambrop72/aprinter,fixator/aprinter,ambrop72/aprinter,johnkingsley/aprinter,ambrop72/aprinter,fixator/aprinter,johnkingsley/aprinter,fixator/aprinter,ambrop72/aprinter,fixator/aprinter,ambrop72/aprinter,fixator/aprinter,johnkingsley/aprinter,ambrop72/aprinter,johnkingsley/aprinter