diff --git a/drive_to_aruco.py b/drive_to_aruco.py index 0c72009..f4b9401 100644 --- a/drive_to_aruco.py +++ b/drive_to_aruco.py @@ -7,8 +7,8 @@ from robot import Robot POWER = 70 -TURN_T = 0.079 # 10 degrees -DRIVE_T = 0.22 # 10 centimeter +TURN_T = 7.9 # 1 degree +DRIVE_T = 22 # 1 centimeter RIGHT_WHEEL_OFFSET = 4 @@ -43,18 +43,18 @@ def main(): if arucos != []: break arlo.go_diff(POWER, POWER, 1, 0) - sleep(3 * TURN_T) + sleep((30 * TURN_T)/1000) arlo.stop() position = cv2.aruco.estimatePoseSingleMarkers( np.array([arucos[0][0]]), 14.5, CAMERA_MATRIX, DIST_COEF )[1][0][0] - print(position) angle = np.rad2deg(np.arctan(position[0]/position[2])) print(angle) - # drive_distance = np.sqrt(position[0]**2 + position[2]**2) + drive_distance = np.sqrt(position[0]**2 + position[2]**2) + print(drive_distance) if __name__ == "__main__":