Softer Commands

Added in softer movement commands for when the robot approaches angle /
distance