MotionPlanner: reorder some code to make it easier to read
MotionPlanner: reorder some code to make it easier to read

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