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()
