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