servo1.setAngle(20) servo2.setAngle(20) servo3.setAngle(20) cap = cv2.VideoCapture(1) focusedVideoCapture = FocusedVideoCapture(cap) focusedVideoCapture.calibrateFocus() vision = Vision(focusedVideoCapture) vision.calibrateCamera() pid = PID(servo1, servo2, servo3, 45, 10) while True: vision.getBallPoint() vision.calculateError() vision.showMotorImage(vision.lastCapture) redError, greenError, blueError = vision.getErrors() pid.update(redError, greenError, blueError) # time.sleep(0.01) if cv2.waitKey(1) & 0xFF == ord('q'): break cap.release() cv2.destroyAllWindows()