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:28:27 +02:00
parent f28b1f6e1b
commit c1a6ddeda4
1 changed files with 13 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
drive_to_aruco.py
View File

@ -43,7 +43,7 @@ def main():
if arucos != []:
break
arlo.go_diff(POWER, POWER, 1, 0)
sleep((30 * TURN_T)/1000)
sleep((30 * TURN_T * CLOCKWISE_OFFSET)/1000)
arlo.stop()
position = cv2.aruco.estimatePoseSingleMarkers(
@ -51,10 +51,19 @@ def main():
)[1][0][0]
angle = np.rad2deg(np.arctan(position[0]/position[2]))
print(angle)
drive_distance = np.sqrt(position[0]**2 + position[2]**2)
print(drive_distance)
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()
if __name__ == "__main__":