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:52:29 +02:00
parent d8647d4466
commit d3db855dad
1 changed files with 4 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
rally.py
View File

@ -47,7 +47,7 @@ def careful_forward(drive_time, arlo, careful_range = 600):
forward_dist = arlo.read_front_ping_sensor() forward_dist = arlo.read_front_ping_sensor()
right_dist = arlo.read_right_ping_sensor() right_dist = arlo.read_right_ping_sensor()
left_dist = arlo.read_left_ping_sensor() left_dist = arlo.read_left_ping_sensor()
if forward_dist > careful_range and all(x > 200 for x in [right_dist, left_dist]): if forward_dist > careful_range and all(x > 300 for x in [right_dist, left_dist]):
print("not blocked") print("not blocked")
turning = None turning = None
arlo.go_diff(POWER, POWER + RIGHT_WHEEL_OFFSET, 1, 1) arlo.go_diff(POWER, POWER + RIGHT_WHEEL_OFFSET, 1, 1)
@ -88,7 +88,7 @@ def main():
if landmark in arucos: if landmark in arucos:
break break
careful_forward(5, noah) careful_forward(3, noah)
position = cv2.aruco.estimatePoseSingleMarkers( position = cv2.aruco.estimatePoseSingleMarkers(
@ -96,7 +96,7 @@ def main():
)[1][0][0] )[1][0][0]
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) - 20
if drive_distance < 30: if drive_distance < 30:
break break
@ -109,7 +109,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()
if not careful_forward((drive_distance * DRIVE_T)/1000, noah, 200): if not careful_forward((drive_distance * DRIVE_T)/1000, noah, 300):
break break
if __name__ == "__main__": if __name__ == "__main__":