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-26 14:23:37 +02:00
parent 9972ad7e86
commit f28b1f6e1b
1 changed files with 5 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
drive_to_aruco.py
View File

@ -7,8 +7,8 @@ from robot import Robot
POWER = 70 POWER = 70
TURN_T = 0.079 # 10 degrees TURN_T = 7.9 # 1 degree
DRIVE_T = 0.22 # 10 centimeter DRIVE_T = 22 # 1 centimeter
RIGHT_WHEEL_OFFSET = 4 RIGHT_WHEEL_OFFSET = 4
@ -43,18 +43,18 @@ def main():
if arucos != []: if arucos != []:
break break
arlo.go_diff(POWER, POWER, 1, 0) arlo.go_diff(POWER, POWER, 1, 0)
sleep(3 * TURN_T) sleep((30 * TURN_T)/1000)
arlo.stop() arlo.stop()
position = cv2.aruco.estimatePoseSingleMarkers( position = cv2.aruco.estimatePoseSingleMarkers(
np.array([arucos[0][0]]), 14.5, CAMERA_MATRIX, DIST_COEF np.array([arucos[0][0]]), 14.5, CAMERA_MATRIX, DIST_COEF
)[1][0][0] )[1][0][0]
print(position)
angle = np.rad2deg(np.arctan(position[0]/position[2])) angle = np.rad2deg(np.arctan(position[0]/position[2]))
print(angle) 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__": if __name__ == "__main__":