angular servo motor can't keep up with angle inputs

[taher50]

I am working on a piece of code that uses OpenCV in my python code. I am using python 2.7.4 with the OpenCV 3.4.0.

In my code, i have built a tracking program to track certain colours in my case its red. I also have a servo motor sitting on top of the servo motor. The centre point of the targeted object is retrieved and printed.

I have verified that the servo motor works fine on its own. when inputting angle from the user, but in my code, i have the servo motor changing angles depending on the position of the red object. In my for loop the angle is retrieved and the duty cycle is sent to the motor.

The problem I am having is that the angle is being retrieved from the code and sent to the servo motor and it can't process it because the next angle has already been sent. This process is happening to fast and my servo motor can't keep up.

I have narrowed the problem down to here. In this for loop i have got the contour information and i just plot bounding boxes around my red objected. I also get the distance from an ultrasonic sensor.

		for c in contours:
			x1, y1, w1, h1 = cv2.boundingRect(c)
			cv2.rectangle(image, (x1,y1), (x1+w1,y1+h1), (0,255,0), 2)
			
			rect = cv2.minAreaRect(c)
			box = cv2.boxPoints(rect)
			
			box = np.int0(box)
			cv2.drawContours(image, [box], 0, (0, 0, 255), 2)
			
			# finally, get the min enclosing circle
			(x, y), radius = cv2.minEnclosingCircle(c)
			
			# convert all values to int
			center = (int(x), int(y))
			radius = int(radius)
			x_cor = int(x)
			y_cor = int(y)
			
			image = cv2.circle(image, center, 1, (255, 0, 0), 2)
			
			Ob_Distance = int(distance())
			
			print "Distance = %d cm  Centre point x = %d y = %d" %(Ob_Distance,  x_cor, y_cor)
			
			if x_cor < XAxisCentre and ServoPosition >= 5:
				ServoPosition = ServoPosition - 2
			elif x_cor > XAxisCentre and ServoPosition <= 175: 
				ServoPosition = ServoPosition + 2
			#end if else
			
			print "servo position = " + str(ServoPosition)
			UpdateServoPositions(pwmObject, ServoPosition)
			
			#time.sleep(0.0001)
			
		#end for

[taher50]

Hi this is my entire code.

#OpenCV_Original.py

# this program tracks a red objects
#Later we will try and move the webcam to track the red object

import cv2
import numpy as np
import os
import RPi.GPIO as GPIO
import time


def main():
	
	GPIO.setmode(GPIO.BCM)
	GPIO.setwarnings(False)

	GPIO_TRIG = 18
	GPIO_ECHO = 24
	servo_gpio_pin = 23
	
	pwmFreq = 100
	IntialDutyCycle = 13
	
	GPIO.setup(GPIO_TRIG, GPIO.OUT)
	GPIO.setup(GPIO_ECHO, GPIO.IN)
	GPIO.setup(servo_gpio_pin, GPIO.OUT)
		
	pwmObject = GPIO.PWM(servo_gpio_pin, pwmFreq)

	lowerBound = np.array([0,135,135])	# Ranges for filtering when light is reflecting of red
	lowerBound1 = np.array([18,255,255])
	upperBound = np.array([165,135,135])
	upperBound1 = np.array([179,255,255])
	
	lowerBound2 = np.array([169,100,100])
	upperBound2 = np.array([189,255,255])
	
	kernelOpen = np.ones((5,5))
	kernelClose = np.ones((20,20))
	
	font = cv2.FONT_HERSHEY_SIMPLEX
	
	cam = cv2.VideoCapture(0) 
	
	if cam.isOpened() == False:
		print "\nerror: Webcam not accessed successfully\n"
		os.system("pause")
		return
	#end if 
	
	print "Original Resolution = " + str(cam.get(cv2.CAP_PROP_FRAME_WIDTH)) + "x" + str(cam.get(cv2.CAP_PROP_FRAME_HEIGHT))
	
	cam.set(cv2.CAP_PROP_FRAME_WIDTH, 320.0)
	cam.set(cv2.CAP_PROP_FRAME_HEIGHT, 240.0)
	
	print "Updated Resolution = " + str(cam.get(cv2.CAP_PROP_FRAME_WIDTH)) + "x" + str(cam.get(cv2.CAP_PROP_FRAME_HEIGHT))
	
	XAxisCentre = int(float(cam.get(cv2.CAP_PROP_FRAME_WIDTH)) / 2.0)
	YAxisCentre = int(float(cam.get(cv2.CAP_PROP_FRAME_HEIGHT)) / 2.0)
	
	pwmObject.start(13)
	
	ServoPosition = int(90)
	#print "servo position = " + str(ServoPosition)
	UpdateServoPositions(pwmObject, ServoPosition)
	
	
	while cam.isOpened() == True and cv2.waitKey(1) != 27:
		ret, image = cam.read()
		
		if not ret or image is None:
			print "\nerror: Frame not read from webcam\n\n"
			os.system("pause")
			break
		# end if
		
		imageHSV = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
		
		imageLow = cv2.inRange(imageHSV,lowerBound,lowerBound1)
		imageHigh = cv2.inRange(imageHSV,upperBound,upperBound1)
		imageMed = cv2.inRange(imageHSV,lowerBound2,upperBound2)
		imageMask = cv2.add(imageLow, imageHigh, imageMed)
		
		imageMask = cv2.GaussianBlur(imageMask, (3, 3), 2)
		
		#imageMask = cv2.dilate(imageMask,kernelOpen)
		#imageMask = cv2.erode(imageMask,kernelOpen)
		
		imageMask = cv2.morphologyEx(imageMask,cv2.MORPH_OPEN,kernelOpen)
		imageMask = cv2.morphologyEx(imageMask,cv2.MORPH_CLOSE,kernelClose)
		
		imageMask, contours, hier = cv2.findContours(imageMask,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)
		
		for c in contours:
			x1, y1, w1, h1 = cv2.boundingRect(c)
			cv2.rectangle(image, (x1,y1), (x1+w1,y1+h1), (0,255,0), 2)
			
			rect = cv2.minAreaRect(c)
			box = cv2.boxPoints(rect)
			
			box = np.int0(box)
			cv2.drawContours(image, [box], 0, (0, 0, 255), 2)
			
			# finally, get the min enclosing circle
			(x, y), radius = cv2.minEnclosingCircle(c)
			
			# convert all values to int
			center = (int(x), int(y))
			radius = int(radius)
			x_cor = int(x)
			y_cor = int(y)
			
			image = cv2.circle(image, center, 1, (255, 0, 0), 2)
			
			Ob_Distance = int(distance())
			
			print "Distance = %d cm  Centre point x = %d y = %d" %(Ob_Distance,  x_cor, y_cor)
			
			if x_cor < XAxisCentre and ServoPosition >= 5:
				ServoPosition = ServoPosition - 2
			elif x_cor > XAxisCentre and ServoPosition <= 175: 
				ServoPosition = ServoPosition + 2
			#end if else
			
			print "servo position = " + str(ServoPosition)
			UpdateServoPositions(pwmObject, ServoPosition)
			
			#time.sleep(0.0001)
			
		#end for
			
			
		cv2.namedWindow("Webcam", cv2.WINDOW_AUTOSIZE)
		cv2.namedWindow("imageMask", cv2.WINDOW_AUTOSIZE)
		
		cv2.imshow("Webcam",image)
		cv2.imshow("imageMask",imageMask)
	# end while
	
	cv2.destroyAllWindows() 

	
	return

def distance():
	
	GPIO.setmode(GPIO.BCM)

	GPIO_TRIG = 18
	GPIO_ECHO = 24
	
	pulse_start = 0
	pulse_end = 0
	pulse_duration = 0
	distance = 0

	GPIO.setup(GPIO_TRIG, GPIO.OUT)
	GPIO.setup(GPIO_ECHO, GPIO.IN)
	
	GPIO.output(GPIO_TRIG, True)
	time.sleep(0.00001)
	GPIO.output(GPIO_TRIG, False)
		
	while GPIO.input(GPIO_ECHO) == 0:
		pulse_start = time.time()
	#end while
		
	while GPIO.input(GPIO_ECHO) == 1:
		pulse_end = time.time()
	#end while
		
	pulse_duration = pulse_end - pulse_start
	distance = (pulse_duration * 34300) / 2
	
	
	
	return distance
	
def UpdateServoPositions(pwmObject, ServoPosition):
	dutyCycle = ((float(ServoPosition) * 0.01) + 0.45) * 10
	pwmObject.ChangeDutyCycle(dutyCycle)
	return

if __name__ == "__main__":
    main()