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
)[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]))
# drive_distance = np.sqrt(position[0]**2 + position[2]**2)
# if angle < 0:
# arlo.go_diff(POWER, POWER, 0, 1)
# sleep((abs(angle) * TURN_T)/1000)
# arlo.stop()
# else:
# arlo.go_diff(POWER, POWER, 1, 0)
# sleep((abs(angle) * TURN_T * CLOCKWISE_OFFSET)/1000)
# arlo.stop()
angle = np.rad2deg(np.arctan(position[0]/position[2]))
drive_distance = np.sqrt(position[0]**2 + position[2]**2)
if angle < 0:
arlo.go_diff(POWER, POWER, 0, 1)
sleep((abs(angle) * TURN_T)/1000)
arlo.stop()
else:
arlo.go_diff(POWER, POWER, 1, 0)
sleep((abs(angle) * TURN_T * CLOCKWISE_OFFSET)/1000)
arlo.stop()
# arlo.go_diff(POWER, POWER + RIGHT_WHEEL_OFFSET, 1, 1)
# sleep((drive_distance * DRIVE_T)/1000)
# arlo.stop()
arlo.go_diff(POWER, POWER + RIGHT_WHEEL_OFFSET, 1, 1)
sleep((drive_distance * DRIVE_T)/1000)
arlo.stop()
if __name__ == "__main__":