Function fwd_dist and bck_dist now use rps values rather than encoder values to maintain a straight path