80 lines
1.9 KiB
Python
80 lines
1.9 KiB
Python
from time import sleep
|
|
|
|
import numpy as np
|
|
import cv2
|
|
|
|
import robot
|
|
|
|
LANDMARKS = [11,6,10] # SET ARUCO NUMBERS
|
|
|
|
POWER = 70
|
|
|
|
TURN_T = 7.9 # 1 degree
|
|
DRIVE_T = 22 # 1 centimeter
|
|
|
|
RIGHT_WHEEL_OFFSET = 4
|
|
|
|
CLOCKWISE_OFFSET = 0.96
|
|
|
|
FOCAL_LENGTH = 1691
|
|
|
|
CAMERA_MATRIX = np.array(
|
|
[[FOCAL_LENGTH,0,512],[0,FOCAL_LENGTH,360],[0,0,1]],
|
|
dtype=np.float32
|
|
)
|
|
DIST_COEF = np.array([0,0,0,0,0], dtype=np.float32)
|
|
|
|
def find_aruco(image):
|
|
aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)
|
|
aruco_params = cv2.aruco.DetectorParameters_create()
|
|
corners, ids, _ = cv2.aruco.detectMarkers(
|
|
image,
|
|
aruco_dict,
|
|
parameters=aruco_params
|
|
)
|
|
|
|
if corners is None:
|
|
return []
|
|
|
|
return {ids[i]: box[0] for i, box in enumerate(corners)}
|
|
|
|
def main():
|
|
landmark_order = LANDMARKS + [
|
|
LANDMARKS[0]
|
|
]
|
|
|
|
noah = robot.Robot()
|
|
|
|
while landmark_order != []:
|
|
landmark = landmark_order.pop(0)
|
|
|
|
while True:
|
|
arucos = find_aruco(noah.take_photo())
|
|
if landmark in arucos:
|
|
break
|
|
noah.go_diff(40, 40, 1, 0)
|
|
sleep(0.3)
|
|
noah.stop()
|
|
|
|
position = cv2.aruco.estimatePoseSingleMarkers(
|
|
np.array([arucos[landmark][0]]), 14.5, CAMERA_MATRIX, DIST_COEF
|
|
)[1][0][0]
|
|
|
|
angle = np.rad2deg(np.arctan(position[0]/position[2]))
|
|
drive_distance = np.sqrt(position[0]**2 + position[2]**2)
|
|
if angle < 0:
|
|
noah.go_diff(POWER, POWER, 0, 1)
|
|
sleep((abs(angle) * TURN_T)/1000)
|
|
noah.stop()
|
|
else:
|
|
noah.go_diff(POWER, POWER, 1, 0)
|
|
sleep((abs(angle) * TURN_T * CLOCKWISE_OFFSET)/1000)
|
|
noah.stop()
|
|
|
|
noah.go_diff(POWER, POWER + RIGHT_WHEEL_OFFSET, 1, 1)
|
|
sleep((drive_distance * DRIVE_T)/1000)
|
|
noah.stop()
|
|
|
|
if __name__ == "__main__":
|
|
main()
|