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 13:33:13 +02:00
parent ac48afd01a
commit 94c7bf1d35
1 changed files with 17 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

31
drive_between_arucos.py
View File

@ -59,22 +59,25 @@ def main():
np.array([aruco[0]]), 14.5, CAMERA_MATRIX, DIST_COEF np.array([aruco[0]]), 14.5, CAMERA_MATRIX, DIST_COEF
)[1][0][0] for aruco in arucos]) )[1][0][0] for aruco in arucos])
print(aruco_positions) position = [
np.average(aruco_positions[:,0]),
np.average(aruco_positions[:,2])
]
# angle = np.rad2deg(np.arctan(position[0]/position[2])) angle = np.rad2deg(np.arctan(position[0]/position[2]))
# drive_distance = np.sqrt(position[0]**2 + position[2]**2) drive_distance = np.sqrt(position[0]**2 + position[2]**2)
# if angle < 0: if angle < 0:
# arlo.go_diff(POWER, POWER, 0, 1) arlo.go_diff(POWER, POWER, 0, 1)
# sleep((abs(angle) * TURN_T)/1000) sleep((abs(angle) * TURN_T)/1000)
# arlo.stop() arlo.stop()
# else: else:
# arlo.go_diff(POWER, POWER, 1, 0) arlo.go_diff(POWER, POWER, 1, 0)
# sleep((abs(angle) * TURN_T * CLOCKWISE_OFFSET)/1000) sleep((abs(angle) * TURN_T * CLOCKWISE_OFFSET)/1000)
# arlo.stop() arlo.stop()
# arlo.go_diff(POWER, POWER + RIGHT_WHEEL_OFFSET, 1, 1) arlo.go_diff(POWER, POWER + RIGHT_WHEEL_OFFSET, 1, 1)
# sleep((drive_distance * DRIVE_T)/1000) sleep((drive_distance * DRIVE_T)/1000)
# arlo.stop() arlo.stop()
if __name__ == "__main__": if __name__ == "__main__":