Example #1
0
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
Example #4
0
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"
Example #6
0
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()