(V2.1) Robot won't move to the nearest position when sending it an impossible position

[apockill]

Bug:
When moving to certain positions that are out of reach, the robot will return "F" and not move at all

Expected Result:
The robot should move to the nearest position that is reachable, closest to the requested position.

Steps to reproduce bug:

1. Unplug robot from power and computer.
2. Plug in robot to power, then computer.
3. Send [sMovX150Y150Z1500V100] OR [sPolS1000R50H100V100] via serial