✨

rally.py

@@ -100,7 +100,7 @@ def main():
             )[1][0][0]
 
             angle = np.rad2deg(np.arctan(position[0]/position[2]))
-            drive_distance = np.sqrt(position[0]**2 + position[2]**2) - 30
+            drive_distance = np.sqrt(position[0]**2 + position[2]**2) - 100
             if drive_distance < 30:
                 break
 
@@ -114,6 +114,15 @@ def main():
                 noah.stop()
 
             if not careful_forward((drive_distance * DRIVE_T)/1000, noah, 400):
+                arucos = find_aruco(noah.take_photo())
+                if landmark in arucos:
+                    position = cv2.aruco.estimatePoseSingleMarkers(
+                        np.array([arucos[landmark]]), 14.5, CAMERA_MATRIX, DIST_COEF
+                    )[1][0][0]
+                    noah.go_diff(POWER, POWER, 1, 1)
+                    time.sleep((90 * DRIVE_T)/1000)
+                    noah.stop()
+
                     break
 
 if __name__ == "__main__":

rally2.py

@@ -18,7 +18,7 @@ LANDMARK_POSITIONS = {
 
 POWER = 70
 
-TURN_T = 9.8  # 1 degree
+TURN_T = 9.6  # 1 degree
 DRIVE_T = 22  # 1 centimeter
 
 RIGHT_WHEEL_OFFSET = 4