NikolajDanger/Roboteksperimentarium
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

✨

Browse Source
This commit is contained in:
NikolajDanger
2022-09-28 14:02:18 +02:00
parent 0cb11b5264
commit 13bde48720
1 changed files with 3 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
drive_between_arucos.py
View File

@ -50,10 +50,9 @@ def find_arucos(arlo):
print(f"found box {id_}") print(f"found box {id_}")
position = cv2.aruco.estimatePoseSingleMarkers( position = cv2.aruco.estimatePoseSingleMarkers(
np.array([aruco]), 14.5, CAMERA_MATRIX, DIST_COEF np.array([aruco]), 14.5, CAMERA_MATRIX, DIST_COEF
) )[1][0][0]
print(position) print(position)
exit() position = np.array([position[1], position[0]])
position = np.array([position[0], position[1]])
position[0] += 22.5 position[0] += 22.5
print(id_, position) print(id_, position)
aruco_dict[id_] = position aruco_dict[id_] = position
@ -77,7 +76,7 @@ def main():
np.average(aruco_positions[:,1]) np.average(aruco_positions[:,1])
] ]
angle = np.rad2deg(np.arctan(position[0]/position[1])) angle = np.rad2deg(np.arctan(position[1]/position[0]))
drive_distance = np.sqrt(position[0]**2 + position[1]**2) drive_distance = np.sqrt(position[0]**2 + position[1]**2)
if angle < 0: if angle < 0:
arlo.go_diff(POWER, POWER, 0, 1) arlo.go_diff(POWER, POWER, 0, 1)