def main():
global bridge
global head_x, neck_y, jaw_pos
print "balltrack initializing CvBridge"
bridge = CvBridge()
image_sub = rospy.Subscriber("/v4l/camera/image_raw", sensor_msgs.msg.Image, image_callback)
fido.init_servos()
fido.set_servo(fido.NECK, NECK_DOWN)
head_x = fido.get_servo_position(fido.HEAD)
neck_y = fido.get_servo_position(fido.NECK)
jaw_pos = fido.get_servo_position(fido.JAW)
print "balltrack opening window"
cv.NamedWindow("orig_image", 1)
cv.MoveWindow("orig_image", 0, 0)
cv.NamedWindow("thresholded_image", 1)
cv.MoveWindow("thresholded_image", 700, 0)
print "balltrack init_node"
rospy.init_node("balltrack", anonymous=True)
try:
print "balltrack spinning"
rospy.spin()
except KeyboardInterrupt:
print "Shutting down"
def main():
color_tracker_window = "output"
thresh_window = "thresh"
capture = cv.CaptureFromCAM(-1)
cv.NamedWindow(color_tracker_window, 1)
cv.NamedWindow(thresh_window, 1)
imgScrible = None
global posX
global posY
fido.init_servos()
while True:
frame = cv.QueryFrame(capture)
cv.Smooth(frame, frame, cv.CV_BLUR, 3)
if (imgScrible is None):
imgScrible = cv.CreateImage(cv.GetSize(frame), 8, 3)
imgThresh = GetThresholdedImage(frame)
mat = cv.GetMat(imgThresh)
#Calculating the moments
moments = cv.Moments(mat, 0)
area = cv.GetCentralMoment(moments, 0, 0)
moment10 = cv.GetSpatialMoment(moments, 1, 0)
moment01 = cv.GetSpatialMoment(moments, 0, 1)
#lastX and lastY stores the previous positions
lastX = posX
lastY = posY
#Finding a big enough blob
if (area > 100000):
#Calculating the coordinate postition of the centroid
posX = int(moment10 / area)
posY = int(moment01 / area)
print 'x: ' + str(posX) + ' y: ' + str(posY) + ' area: ' + str(
area)
#drawing lines to track the movement of the blob
if (lastX > 0 and lastY > 0 and posX > 0 and posY > 0):
cv.Line(imgScrible, (posX, posY), (lastX, lastY),
cv.Scalar(0, 255, 255), 5)
#Adds the three layers and stores it in the frame
#frame -> it has the camera stream
#imgScrible -> it has the line tracking the movement of the blob
cv.Add(frame, imgScrible, frame)
cv.ShowImage(thresh_window, imgThresh)
cv.ShowImage(color_tracker_window, frame)
c = cv.WaitKey(10)
if (c != -1):
break
def main():
color_tracker_window = "output"
thresh_window = "thresh"
capture = cv.CaptureFromCAM(-1)
cv.NamedWindow( color_tracker_window, 1 )
cv.NamedWindow( thresh_window, 1 )
imgScrible = None
global posX
global posY
fido.init_servos()
while True:
frame = cv.QueryFrame(capture)
cv.Smooth(frame, frame, cv.CV_BLUR, 3)
if(imgScrible is None):
imgScrible = cv.CreateImage(cv.GetSize(frame), 8, 3)
imgThresh = GetThresholdedImage(frame)
mat = cv.GetMat(imgThresh)
#Calculating the moments
moments = cv.Moments(mat, 0)
area = cv.GetCentralMoment(moments, 0, 0)
moment10 = cv.GetSpatialMoment(moments, 1, 0)
moment01 = cv.GetSpatialMoment(moments, 0, 1)
#lastX and lastY stores the previous positions
lastX = posX
lastY = posY
#Finding a big enough blob
if(area > 100000):
#Calculating the coordinate postition of the centroid
posX = int(moment10 / area)
posY = int(moment01 / area)
print 'x: ' + str(posX) + ' y: ' + str(posY) + ' area: ' + str(area)
#drawing lines to track the movement of the blob
if(lastX > 0 and lastY > 0 and posX > 0 and posY > 0):
cv.Line(imgScrible, (posX, posY), (lastX, lastY), cv.Scalar(0, 255, 255), 5)
#Adds the three layers and stores it in the frame
#frame -> it has the camera stream
#imgScrible -> it has the line tracking the movement of the blob
cv.Add(frame, imgScrible, frame)
cv.ShowImage(thresh_window, imgThresh)
cv.ShowImage(color_tracker_window, frame)
c = cv.WaitKey(10)
if(c!=-1):
break
def main():
global bridge
global head_x, neck_y, jaw_pos
print "balltrack2 initializing CvBridge"
bridge = CvBridge()
image_sub = rospy.Subscriber("/fido/balldetect_image", sensor_msgs.msg.Image, image_callback)
fido.init_servos()
fido.set_servo(fido.NECK, NECK_DOWN)
head_x = fido.get_servo_position(fido.HEAD)
neck_y = fido.get_servo_position(fido.NECK)
jaw_pos = fido.get_servo_position(fido.JAW)
print "balltrack2 init_node"
rospy.init_node('balltrack', anonymous=True)
try:
print "balltrack2 spinning"
rospy.spin()
except KeyboardInterrupt:
print "Shutting2 down"
def main():
color_tracker_window = "output"
thresh_window = "thresh"
capture = cv.CaptureFromCAM(-1)
cv.NamedWindow(color_tracker_window, 1)
cv.MoveWindow(color_tracker_window, 0, 0)
cv.NamedWindow(thresh_window, 1)
cv.MoveWindow(thresh_window, 700, 0)
imgScrible = None
storage = None
global posX
global posY
fido.init_servos()
fido.set_servo(fido.NECK, NECK_DOWN)
head_x = fido.get_servo_position(fido.HEAD)
neck_y = fido.get_servo_position(fido.NECK)
jaw_pos = fido.get_servo_position(fido.JAW)
#frame = cv.QueryFrame(capture)
#imgThresh = GetThresholdedImage(frame)
for f in xrange(2000):
frame = cv.QueryFrame(capture)
#cv.Smooth(frame, frame, cv.CV_BLUR, 3)
cv.Smooth(frame, frame, cv.CV_GAUSSIAN, 9, 9)
#imgScrible = cv.CreateImage(cv.GetSize(frame), 8, 3)
imgThresh = GetThresholdedImage(frame)
# pre-smoothing improves Hough detector
#if storage is None:
# storage = cv.CreateMat(imgThresh.width, 1, cv.CV_32FC3)
#try:
# cv.HoughCircles(imgThresh, storage, cv.CV_HOUGH_GRADIENT, 1, imgThresh.height/4, 50, 20, 10, 240)
# circles = np.asarray(storage)
#except Error, e:
# print e
# circles = None
# find largest circle
#maxRadius = 0
#x = 0
#y = 0
#found = False
#if circles is not None:
# for i in range(len(circles)):
# circle = circles[i]
# if circle[2] > maxRadius:
# found = True
# maxRadius = circle[2]
# x = circle[0]
# y = circle[1]
#cvShowImage( 'Camera', frame)
#if found:
# posX = x
# posY = y
# print 'ball detected at position: ',x, ',', y, ' with radius: ', maxRadius
#else:
# print 'no ball'
mat = cv.GetMat(imgThresh)
#Calculating the moments
moments = cv.Moments(mat, 0)
area = cv.GetCentralMoment(moments, 0, 0)
moment10 = cv.GetSpatialMoment(moments, 1, 0)
moment01 = cv.GetSpatialMoment(moments, 0, 1)
#lastX and lastY stores the previous positions
lastX = posX
lastY = posY
#Finding a big enough blob
if area > 20000:
#Calculating the coordinate postition of the centroid
posX = int(moment10 / area)
posY = int(moment01 / area)
print str(datetime.now()), ' x: ' + str(posX) + ' y: ' + str(posY) + ' area: ' + str(area) + ' head_x: ' + str(head_x) + ' neck_y: ' + str(neck_y) + ' jaw_pos: ' + str(jaw_pos)
#drawing lines to track the movement of the blob
if(lastX > 0 and lastY > 0 and posX > 0 and posY > 0):
#cv.Circle( imgThresh, (posX, posY), maxRadius, cv.Scalar(0,0,255), 3, 8, 0 );
#cv.Line(imgScrible, (posX, posY), (lastX, lastY), cv.Scalar(0, 0, 255), 5)
if posX < CENTER_X - 10:
error_x = (posX - CENTER_X) / MAX_X * (HEAD_RIGHT - HEAD_LEFT)
desired_x = int(error_x) / 4 + head_x
head_x = desired_x
if head_x < HEAD_LEFT:
head_x = HEAD_LEFT
fido.set_servo(fido.HEAD, head_x)
elif posX > CENTER_X + 10:
new_x = (posX - CENTER_X) / MAX_X * (HEAD_RIGHT - HEAD_LEFT)
head_x = int(new_x) / 4 + head_x
if head_x > HEAD_RIGHT:
head_x = HEAD_RIGHT
fido.set_servo(fido.HEAD, head_x)
if posY < CENTER_Y - 10:
new_y = (posY - CENTER_Y) / MAX_Y * (NECK_UP - NECK_DOWN)
neck_y = neck_y - (int(new_y) / 8)
if neck_y > NECK_UP:
neck_y = NECK_UP
fido.set_servo(fido.NECK, neck_y)
elif posY > CENTER_Y + 10:
new_y = (posY - CENTER_Y) / MAX_Y * (NECK_UP - NECK_DOWN)
neck_y = neck_y - (int(new_y) / 8)
if neck_y < NECK_DOWN:
neck_y = NECK_DOWN
fido.set_servo(fido.NECK, neck_y)
jaw_pos =int((float(area) - 60000.0) / 1000000.0 * (fido.JAW_OPEN - fido.JAW_CLOSED_EMPTY) + fido.JAW_CLOSED_EMPTY)
jaw_pos = max(min(jaw_pos, fido.JAW_OPEN), fido.JAW_CLOSED_EMPTY)
fido.set_servo(fido.JAW, jaw_pos)
#Adds the three layers and stores it in the frame
#frame -> it has the camera stream
#imgScrible -> it has the line tracking the movement of the blob
#cv.Add(frame, imgScrible, frame)
cv.ShowImage(thresh_window, imgThresh)
cv.ShowImage(color_tracker_window, frame)
c = cv.WaitKey(10)
if(c!=-1):
break
print "max frames reached, exiting"
def main():
color_tracker_window = "output"
thresh_window = "thresh"
capture = cv.CaptureFromCAM(-1)
cv.NamedWindow(color_tracker_window, 1)
cv.MoveWindow(color_tracker_window, 0, 0)
cv.NamedWindow(thresh_window, 1)
cv.MoveWindow(thresh_window, 700, 0)
imgScrible = None
storage = None
global posX
global posY
fido.init_servos()
fido.set_servo(fido.NECK, NECK_DOWN)
head_x = fido.get_servo_position(fido.HEAD)
neck_y = fido.get_servo_position(fido.NECK)
jaw_pos = fido.get_servo_position(fido.JAW)
#frame = cv.QueryFrame(capture)
#imgThresh = GetThresholdedImage(frame)
for f in xrange(2000):
frame = cv.QueryFrame(capture)
#cv.Smooth(frame, frame, cv.CV_BLUR, 3)
cv.Smooth(frame, frame, cv.CV_GAUSSIAN, 9, 9)
#imgScrible = cv.CreateImage(cv.GetSize(frame), 8, 3)
imgThresh = GetThresholdedImage(frame)
# pre-smoothing improves Hough detector
#if storage is None:
# storage = cv.CreateMat(imgThresh.width, 1, cv.CV_32FC3)
#try:
# cv.HoughCircles(imgThresh, storage, cv.CV_HOUGH_GRADIENT, 1, imgThresh.height/4, 50, 20, 10, 240)
# circles = np.asarray(storage)
#except Error, e:
# print e
# circles = None
# find largest circle
#maxRadius = 0
#x = 0
#y = 0
#found = False
#if circles is not None:
# for i in range(len(circles)):
# circle = circles[i]
# if circle[2] > maxRadius:
# found = True
# maxRadius = circle[2]
# x = circle[0]
# y = circle[1]
#cvShowImage( 'Camera', frame)
#if found:
# posX = x
# posY = y
# print 'ball detected at position: ',x, ',', y, ' with radius: ', maxRadius
#else:
# print 'no ball'
mat = cv.GetMat(imgThresh)
#Calculating the moments
moments = cv.Moments(mat, 0)
area = cv.GetCentralMoment(moments, 0, 0)
moment10 = cv.GetSpatialMoment(moments, 1, 0)
moment01 = cv.GetSpatialMoment(moments, 0, 1)
#lastX and lastY stores the previous positions
lastX = posX
lastY = posY
#Finding a big enough blob
if area > 20000:
#Calculating the coordinate postition of the centroid
posX = int(moment10 / area)
posY = int(moment01 / area)
print 'x: ' + str(posX) + ' y: ' + str(posY) + ' area: ' + str(
area) + ' head_x: ' + str(head_x) + ' neck_y: ' + str(
neck_y) + ' jaw_pos: ' + str(jaw_pos)
#drawing lines to track the movement of the blob
if (lastX > 0 and lastY > 0 and posX > 0 and posY > 0):
#cv.Circle( imgThresh, (posX, posY), maxRadius, cv.Scalar(0,0,255), 3, 8, 0 );
#cv.Line(imgScrible, (posX, posY), (lastX, lastY), cv.Scalar(0, 0, 255), 5)
if posX < CENTER_X - 10:
error_x = (posX - CENTER_X) / MAX_X * (HEAD_RIGHT -
HEAD_LEFT)
desired_x = int(error_x) / 4 + head_x
head_x = desired_x
if head_x < HEAD_LEFT:
head_x = HEAD_LEFT
fido.set_servo(fido.HEAD, head_x)
elif posX > CENTER_X + 10:
new_x = (posX - CENTER_X) / MAX_X * (HEAD_RIGHT -
HEAD_LEFT)
head_x = int(new_x) / 4 + head_x
if head_x > HEAD_RIGHT:
head_x = HEAD_RIGHT
fido.set_servo(fido.HEAD, head_x)
if posY < CENTER_Y - 10:
new_y = (posY - CENTER_Y) / MAX_Y * (NECK_UP - NECK_DOWN)
neck_y = neck_y - (int(new_y) / 8)
if neck_y > NECK_UP:
neck_y = NECK_UP
fido.set_servo(fido.NECK, neck_y)
elif posY > CENTER_Y + 10:
new_y = (posY - CENTER_Y) / MAX_Y * (NECK_UP - NECK_DOWN)
neck_y = neck_y - (int(new_y) / 8)
if neck_y < NECK_DOWN:
neck_y = NECK_DOWN
fido.set_servo(fido.NECK, neck_y)
jaw_pos = int((float(area) - 60000.0) / 1000000.0 *
(fido.JAW_OPEN - fido.JAW_CLOSED_EMPTY) +
fido.JAW_CLOSED_EMPTY)
jaw_pos = max(min(jaw_pos, fido.JAW_OPEN),
fido.JAW_CLOSED_EMPTY)
fido.set_servo(fido.JAW, jaw_pos)
#Adds the three layers and stores it in the frame
#frame -> it has the camera stream
#imgScrible -> it has the line tracking the movement of the blob
#cv.Add(frame, imgScrible, frame)
cv.ShowImage(thresh_window, imgThresh)
cv.ShowImage(color_tracker_window, frame)
c = cv.WaitKey(10)
if (c != -1):
break
print "max frames reached, exiting"
def main():
color_tracker_window = "output"
thresh_window = "thresh"
blob_window = "blob"
cam = create_capture(-1)
cv2.namedWindow(color_tracker_window, 1)
cv2.moveWindow(color_tracker_window, 0, 0)
cv2.namedWindow(thresh_window, 1)
cv2.moveWindow(thresh_window, 700, 0)
cv2.namedWindow(blob_window, 1)
cv2.moveWindow(blob_window, 700, 500)
imgScrible = None
storage = None
global posX
global posY
fido.init_servos()
time.sleep(0.25)
fido.set_servo(fido.NECK, NECK_START)
head_x = fido.get_servo_position(fido.HEAD)
neck_y = fido.get_servo_position(fido.NECK)
jaw_pos = fido.get_servo_position(fido.JAW)
#frame = cv.QueryFrame(capture)
#imgThresh = GetThresholdedImage(frame)
last_known_x = None
last_known_y = None
while True:
ret, frame = cam.read()
#print "cam.read returned {0}".format(ret)
if not ret:
break
cv2.imshow(color_tracker_window, frame)
blurred = cv2.GaussianBlur(frame, (9, 9), 0)
imgThresh = GetThresholdedImage(blurred)
cv2.imshow(thresh_window, imgThresh)
#Calculating the moments
#contours = None
contours, hierarchy = cv2.findContours(imgThresh,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)
area = 0
if contours:
for c in contours:
moments = cv2.moments(c)
a = moments['m00']
if a > area:
area = a
moment10 = moments['m10']
moment01 = moments['m01']
#lastX and lastY stores the previous positions
lastX = posX
lastY = posY
#Finding a big enough blob
if area > 100:
#Calculating the coordinate postition of the centroid
posX = int(moment10 / area)
posY = int(moment01 / area)
last_known_x = posX
last_known_y = posY
elif last_known_x is not None:
if head_x > HEAD_LEFT and head_x < HEAD_RIGHT and neck_y > NECK_DOWN and neck_y < NECK_UP:
posX = last_known_x
posY = last_known_y
else:
fido.ramp_servo(fido.HEAD, HEAD_CENTER, -3 if head_x > HEAD_CENTER else 3)
fido.ramp_servo(fido.NECK, NECK_START, -3 if neck_y > NECK_CENTER else 3)
posX = CENTER_X
posY = CENTER_Y
last_known_x = CENTER_X
last_known_x = CENTER_Y
if True:
print str(datetime.now()), ' x: ' + str(posX) + ' y: ' + str(posY) + ' area: ' + str(area) + ' head_x: ' + str(head_x) + ' neck_y: ' + str(neck_y) + ' jaw_pos: ' + str(jaw_pos)
#drawing lines to track the movement of the blob
if(lastX > 0 and lastY > 0 and posX > 0 and posY > 0):
#cv.Circle( imgThresh, (posX, posY), maxRadius, cv.Scalar(0,0,255), 3, 8, 0 );
#cv.Line(imgScrible, (posX, posY), (lastX, lastY), cv.Scalar(0, 0, 255), 5)
if posX < CENTER_X - 10:
error_x = (posX - CENTER_X) / MAX_X * (HEAD_RIGHT - HEAD_LEFT)
desired_x = int(error_x) / 4 + head_x
head_x = desired_x
if head_x < HEAD_LEFT:
head_x = HEAD_LEFT
fido.set_servo(fido.HEAD, head_x)
elif posX > CENTER_X + 10:
new_x = (posX - CENTER_X) / MAX_X * (HEAD_RIGHT - HEAD_LEFT)
head_x = int(new_x) / 4 + head_x
if head_x > HEAD_RIGHT:
head_x = HEAD_RIGHT
fido.set_servo(fido.HEAD, head_x)
if posY < CENTER_Y - 10:
new_y = (posY - CENTER_Y) / MAX_Y * (NECK_UP - NECK_DOWN)
neck_y = neck_y - (int(new_y) / 8)
if neck_y > NECK_UP:
neck_y = NECK_UP
fido.set_servo(fido.NECK, neck_y)
elif posY > CENTER_Y + 10:
new_y = (posY - CENTER_Y) / MAX_Y * (NECK_UP - NECK_DOWN)
neck_y = neck_y - (int(new_y) / 8)
if neck_y < NECK_DOWN:
neck_y = NECK_DOWN
fido.set_servo(fido.NECK, neck_y)
jaw_pos =int((float(area) - 1000.0) / 25000.0 * (fido.JAW_OPEN - fido.JAW_CLOSED_EMPTY) + fido.JAW_CLOSED_EMPTY)
jaw_pos = max(min(jaw_pos, fido.JAW_OPEN), fido.JAW_CLOSED_EMPTY)
fido.set_servo(fido.JAW, jaw_pos)
#Adds the three layers and stores it in the frame
#frame -> it has the camera stream
#imgScrible -> it has the line tracking the movement of the blob
#cv.Add(frame, imgScrible, frame)
else:
print "area = {}".format(area)
cv2.imshow(blob_window, imgThresh)
c = cv2.waitKey(3)
if(c!=-1):
break
print "exiting"
cv2.destroyAllWindows()
def main():
display = scv.Display()
cam = scv.Camera()
global posX
global posY
fido.init_servos()
fido.set_servo(fido.NECK, NECK_DOWN)
head_x = fido.get_servo_position(fido.HEAD)
neck_y = fido.get_servo_position(fido.NECK)
jaw_pos = fido.get_servo_position(fido.JAW)
while display.isNotDone():
img = cam.getImage()
img.blur()
#imgScrible = cv.CreateImage(cv.GetSize(frame), 8, 3)
imgThresh = GetThresholdedImage(img)
blobs = imgThresh.findBlobs()
circles = None
if blobs:
#print ' found blobs: ', len(blobs)
#circles = blobs.filter([b.isCircle(0.6) for b in blobs])
#if circles:
#print ' found circles: ', len(circles)
max_radius = 0
x = 0
y = 0
biggest_blob = None
for c in blobs:
if c.area > max_radius:
x = c.centroid()[0]
y = c.centroid()[1]
max_radius = c.area()
area = c.area()
biggest_blob = c
biggest_blob.drawHull()
#imgThresh.drawCircle((x, y), max_radius, scv.Color.GREEN, min(3,max_radius))
lastX = posX
lastY = posY
if blobs:
posX = x
posY = y
print 'x: ' + str(posX) + ' y: ' + str(posY) + ' area: ' + str(
area) + ' head_x: ' + str(head_x) + ' neck_y: ' + str(
neck_y) + ' jaw_pos: ' + str(jaw_pos)
#drawing lines to track the movement of the blob
if (lastX > 0 and lastY > 0 and posX > 0 and posY > 0):
#cv.Circle( imgThresh, (posX, posY), maxRadius, cv.Scalar(0,0,255), 3, 8, 0 );
#cv.Line(imgScrible, (posX, posY), (lastX, lastY), cv.Scalar(0, 0, 255), 5)
if posX < CENTER_X - 10:
error_x = (posX - CENTER_X) / MAX_X * (HEAD_RIGHT -
HEAD_LEFT)
desired_x = int(error_x) / 4 + head_x
head_x = desired_x
if head_x < HEAD_LEFT:
head_x = HEAD_LEFT
fido.set_servo(fido.HEAD, head_x)
elif posX > CENTER_X + 10:
new_x = (posX - CENTER_X) / MAX_X * (HEAD_RIGHT -
HEAD_LEFT)
head_x = int(new_x) / 4 + head_x
if head_x > HEAD_RIGHT:
head_x = HEAD_RIGHT
fido.set_servo(fido.HEAD, head_x)
if posY < CENTER_Y - 10:
new_y = (posY - CENTER_Y) / MAX_Y * (NECK_UP - NECK_DOWN)
neck_y = neck_y - (int(new_y) / 8)
if neck_y > NECK_UP:
neck_y = NECK_UP
fido.set_servo(fido.NECK, neck_y)
elif posY > CENTER_Y + 10:
new_y = (posY - CENTER_Y) / MAX_Y * (NECK_UP - NECK_DOWN)
neck_y = neck_y - (int(new_y) / 8)
if neck_y < NECK_DOWN:
neck_y = NECK_DOWN
fido.set_servo(fido.NECK, neck_y)
jaw_pos = int((float(area) - 60000.0) / 1000000.0 *
(fido.JAW_OPEN - fido.JAW_CLOSED_EMPTY) +
fido.JAW_CLOSED_EMPTY)
jaw_pos = max(min(jaw_pos, fido.JAW_OPEN),
fido.JAW_CLOSED_EMPTY)
fido.set_servo(fido.JAW, jaw_pos)
imgThresh.show()
def main():
display = scv.Display()
cam = scv.Camera()
global posX
global posY
fido.init_servos()
fido.set_servo(fido.NECK, NECK_DOWN)
head_x = fido.get_servo_position(fido.HEAD)
neck_y = fido.get_servo_position(fido.NECK)
jaw_pos = fido.get_servo_position(fido.JAW)
while display.isNotDone():
img = cam.getImage()
img.blur()
#imgScrible = cv.CreateImage(cv.GetSize(frame), 8, 3)
imgThresh = GetThresholdedImage(img)
blobs = imgThresh.findBlobs()
circles = None
if blobs:
#print ' found blobs: ', len(blobs)
#circles = blobs.filter([b.isCircle(0.6) for b in blobs])
#if circles:
#print ' found circles: ', len(circles)
max_radius = 0
x = 0
y = 0
biggest_blob = None
for c in blobs:
if c.area > max_radius:
x = c.centroid()[0]
y = c.centroid()[1]
max_radius = c.area()
area = c.area()
biggest_blob = c
biggest_blob.drawHull()
#imgThresh.drawCircle((x, y), max_radius, scv.Color.GREEN, min(3,max_radius))
lastX = posX
lastY = posY
if blobs:
posX = x
posY = y
print 'x: ' + str(posX) + ' y: ' + str(posY) + ' area: ' + str(area) + ' head_x: ' + str(head_x) + ' neck_y: ' + str(neck_y) + ' jaw_pos: ' + str(jaw_pos)
#drawing lines to track the movement of the blob
if(lastX > 0 and lastY > 0 and posX > 0 and posY > 0):
#cv.Circle( imgThresh, (posX, posY), maxRadius, cv.Scalar(0,0,255), 3, 8, 0 );
#cv.Line(imgScrible, (posX, posY), (lastX, lastY), cv.Scalar(0, 0, 255), 5)
if posX < CENTER_X - 10:
error_x = (posX - CENTER_X) / MAX_X * (HEAD_RIGHT - HEAD_LEFT)
desired_x = int(error_x) / 4 + head_x
head_x = desired_x
if head_x < HEAD_LEFT:
head_x = HEAD_LEFT
fido.set_servo(fido.HEAD, head_x)
elif posX > CENTER_X + 10:
new_x = (posX - CENTER_X) / MAX_X * (HEAD_RIGHT - HEAD_LEFT)
head_x = int(new_x) / 4 + head_x
if head_x > HEAD_RIGHT:
head_x = HEAD_RIGHT
fido.set_servo(fido.HEAD, head_x)
if posY < CENTER_Y - 10:
new_y = (posY - CENTER_Y) / MAX_Y * (NECK_UP - NECK_DOWN)
neck_y = neck_y - (int(new_y) / 8)
if neck_y > NECK_UP:
neck_y = NECK_UP
fido.set_servo(fido.NECK, neck_y)
elif posY > CENTER_Y + 10:
new_y = (posY - CENTER_Y) / MAX_Y * (NECK_UP - NECK_DOWN)
neck_y = neck_y - (int(new_y) / 8)
if neck_y < NECK_DOWN:
neck_y = NECK_DOWN
fido.set_servo(fido.NECK, neck_y)
jaw_pos =int((float(area) - 60000.0) / 1000000.0 * (fido.JAW_OPEN - fido.JAW_CLOSED_EMPTY) + fido.JAW_CLOSED_EMPTY)
jaw_pos = max(min(jaw_pos, fido.JAW_OPEN), fido.JAW_CLOSED_EMPTY)
fido.set_servo(fido.JAW, jaw_pos)
imgThresh.show()