diff --git a/drive_to_aruco.py b/drive_to_aruco.py index 1e2ec17..9556041 100644 --- a/drive_to_aruco.py +++ b/drive_to_aruco.py @@ -52,6 +52,7 @@ def main(): angle = np.rad2deg(np.arctan(position[0]/position[2])) drive_distance = np.sqrt(position[0]**2 + position[2]**2) + print(drive_distance) if angle < 0: arlo.go_diff(POWER, POWER, 0, 1) sleep((abs(angle) * TURN_T)/1000) @@ -61,9 +62,9 @@ def main(): sleep((abs(angle) * TURN_T * CLOCKWISE_OFFSET)/1000) arlo.stop() - arlo.go_diff(POWER, POWER + RIGHT_WHEEL_OFFSET, 1, 1) - sleep((drive_distance * DRIVE_T)/1000) - arlo.stop() + # arlo.go_diff(POWER, POWER + RIGHT_WHEEL_OFFSET, 1, 1) + # sleep((drive_distance * DRIVE_T)/1000) + # arlo.stop() if __name__ == "__main__":