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:27:38 +02:00
parent afd5be5eb3
commit 4c89c36a78
1 changed files with 38 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

46
rally.py
View File

@ -1,4 +1,4 @@
from time import sleep import time
import numpy as np import numpy as np
import cv2 import cv2
@ -38,6 +38,29 @@ def find_aruco(image):
return {ids[i][0]: box[0] for i, box in enumerate(corners)} return {ids[i][0]: box[0] for i, box in enumerate(corners)}
def careful_forward(drive_time, arlo):
start = time.time()
end = start + drive_time
turning = None
while time.time() < end:
forward_dist = arlo.read_front_ping_sensor()
right_dist = arlo.read_right_ping_sensor()
left_dist = arlo.read_left_ping_sensor()
if forward_dist > 600 and all(x > 200 for x in [right_dist, left_dist]):
print("not blocked")
turning = None
arlo.go_diff(POWER, POWER + RIGHT_WHEEL_OFFSET, 1, 1)
else:
print("blocked")
if turning == "R" or (forward_dist > 600 and right_dist > 200 and turning is None):
arlo.go_diff(POWER, POWER, 1, 0)
turning = "R"
else:
arlo.go_diff(POWER, POWER + RIGHT_WHEEL_OFFSET, 0, 1)
turning = "L"
time.sleep(0.01)
def main(): def main():
landmark_order = LANDMARKS + [ landmark_order = LANDMARKS + [
LANDMARKS[0] LANDMARKS[0]
@ -49,12 +72,19 @@ def main():
landmark = landmark_order.pop(0) landmark = landmark_order.pop(0)
while True: while True:
arucos = find_aruco(noah.take_photo()) for _ in range(36):
arucos = find_aruco(noah.take_photo())
if landmark in arucos:
break
noah.go_diff(POWER, POWER, 0, 1)
time.sleep((10 * TURN_T)/1000)
noah.stop()
if landmark in arucos: if landmark in arucos:
break break
noah.go_diff(40, 40, 1, 0)
sleep(0.3) careful_forward(5, noah)
noah.stop()
position = cv2.aruco.estimatePoseSingleMarkers( position = cv2.aruco.estimatePoseSingleMarkers(
np.array([arucos[landmark]]), 14.5, CAMERA_MATRIX, DIST_COEF np.array([arucos[landmark]]), 14.5, CAMERA_MATRIX, DIST_COEF
@ -64,15 +94,15 @@ def main():
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:
noah.go_diff(POWER, POWER, 0, 1) noah.go_diff(POWER, POWER, 0, 1)
sleep((abs(angle) * TURN_T)/1000) time.sleep((abs(angle) * TURN_T)/1000)
noah.stop() noah.stop()
else: else:
noah.go_diff(POWER, POWER, 1, 0) noah.go_diff(POWER, POWER, 1, 0)
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) noah.go_diff(POWER, POWER + RIGHT_WHEEL_OFFSET, 1, 1)
sleep((drive_distance * DRIVE_T)/1000) time.sleep((drive_distance * DRIVE_T)/1000)
noah.stop() noah.stop()
if __name__ == "__main__": if __name__ == "__main__":