10, [0, 255, 255], -1) cv2.circle(frame, (int(x), int(y)), 10, [0, 255, 0], -1) print("Pos: " + str(int(x)) + " " + str(int(y))) print("Vel: " + str(int(delX)) + " " + str(int(delY))) cv2.putText(frame, "Pos: " + str(int(x)) + " " + str(int(y)), (0, 20), cv2.FONT_HERSHEY_PLAIN, 1.2, [255, 255, 255], 1) cv2.putText( frame, "Vel: " + str(int(delX)) + " " + str(int(delY)), (0, 40), cv2.FONT_HERSHEY_PLAIN, 1.2, [255, 255, 255], 1) elif initCounter > 30: # Occlusion # Using kalman filter to predict position using saved velocity predictedCords = kalmanFilter.Predict() path.append([predictedCords[0], predictedCords[1]]) cv2.circle(frame, (predictedCords[0], predictedCords[1]), 10, [0, 255, 255], -1) else: # Drive bot running if driveBot.index < len(path): error = np.sqrt(((driveBot.xPos - path[driveBot.index][0])**2) + ((driveBot.yPos - path[driveBot.index][1])**2)) # To check is the bot is close to the set point if error > 2: driveBot.calculateError(path[driveBot.index][0], path[driveBot.index][1]) # Drawing the bot head = np.array([