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

✨

Browse Source
This commit is contained in:
NikolajDanger
2022-10-24 14:41:36 +02:00
parent 4e41d872ce
commit edec956494
1 changed files with 28 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
rally.py
View File

@@ -71,6 +71,7 @@ def main():
while landmark_order != []: while landmark_order != []:
landmark = landmark_order.pop(0) landmark = landmark_order.pop(0)
while True:
while True: while True:
for _ in range(24): for _ in range(24):
arucos = find_aruco(noah.take_photo()) arucos = find_aruco(noah.take_photo())
@@ -92,6 +93,9 @@ def main():
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) - 10 drive_distance = np.sqrt(position[0]**2 + position[2]**2) - 10
if drive_distance < 30:
break
if angle < 0: if angle < 0:
noah.go_diff(POWER, POWER, 0, 1) noah.go_diff(POWER, POWER, 0, 1)
time.sleep((abs(angle) * TURN_T)/1000) time.sleep((abs(angle) * TURN_T)/1000)
@@ -101,9 +105,7 @@ def main():
time.sleep((abs(angle) * TURN_T * CLOCKWISE_OFFSET)/1000) time.sleep((abs(angle) * TURN_T * CLOCKWISE_OFFSET)/1000)
noah.stop() noah.stop()
noah.go_diff(POWER, POWER + RIGHT_WHEEL_OFFSET, 1, 1) careful_forward((drive_distance * DRIVE_T)/1000)
time.sleep((drive_distance * DRIVE_T)/1000)
noah.stop()
if __name__ == "__main__": if __name__ == "__main__":
main() main()