Added navigation support to redirect the robot to a pre-coded waypoint bearing according to the gps's and IMU