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

54
rally.py
View File

@ -72,38 +72,40 @@ def main():
landmark = landmark_order.pop(0)
while True:
for _ in range(24):
arucos = find_aruco(noah.take_photo())
while True:
for _ in range(24):
arucos = find_aruco(noah.take_photo())
if landmark in arucos:
break
noah.go_diff(POWER, POWER, 0, 1)
time.sleep((20 * TURN_T)/1000)
noah.stop()
if landmark in arucos:
break
noah.go_diff(POWER, POWER, 0, 1)
time.sleep((20 * TURN_T)/1000)
noah.stop()
if landmark in arucos:
careful_forward(5, noah)
position = cv2.aruco.estimatePoseSingleMarkers(
np.array([arucos[landmark]]), 14.5, CAMERA_MATRIX, DIST_COEF
)[1][0][0]
angle = np.rad2deg(np.arctan(position[0]/position[2]))
drive_distance = np.sqrt(position[0]**2 + position[2]**2) - 10
if drive_distance < 30:
break
careful_forward(5, noah)
if angle < 0:
noah.go_diff(POWER, POWER, 0, 1)
time.sleep((abs(angle) * TURN_T)/1000)
noah.stop()
else:
noah.go_diff(POWER, POWER, 1, 0)
time.sleep((abs(angle) * TURN_T * CLOCKWISE_OFFSET)/1000)
noah.stop()
position = cv2.aruco.estimatePoseSingleMarkers(
np.array([arucos[landmark]]), 14.5, CAMERA_MATRIX, DIST_COEF
)[1][0][0]
angle = np.rad2deg(np.arctan(position[0]/position[2]))
drive_distance = np.sqrt(position[0]**2 + position[2]**2) - 10
if angle < 0:
noah.go_diff(POWER, POWER, 0, 1)
time.sleep((abs(angle) * TURN_T)/1000)
noah.stop()
else:
noah.go_diff(POWER, POWER, 1, 0)
time.sleep((abs(angle) * TURN_T * CLOCKWISE_OFFSET)/1000)
noah.stop()
noah.go_diff(POWER, POWER + RIGHT_WHEEL_OFFSET, 1, 1)
time.sleep((drive_distance * DRIVE_T)/1000)
noah.stop()
careful_forward((drive_distance * DRIVE_T)/1000)
if __name__ == "__main__":
main()