def servo_abs(target_angle): servo(target_angle - get_current_angle()) def seek(): debug('Seek called & hunt_dir = ' + str(hunt_dir)) global hunt_step global hunt_dir current_angle = get_current_angle() if current_angle >= 90 or current_angle <= -90: hunt_dir *= -1 servo_abs(clip_angle(current_angle + hunt_dir * hunt_step)) time.sleep(0.05) while True: start_time = time.clock() image = cvutils.wb(camera.getImage().scale(scale_factor), grey_sample) v,s,h = image.toHSV().splitChannels() hue_match = h.colorDistance((target_colour[0][0],target_colour[0][0],target_colour[0][0])).binarize(target_colour[0][1]*3) sat_match = s.colorDistance((target_colour[1][0],target_colour[1][0],target_colour[1][0])).binarize(target_colour[1][1]*3) matched = ((hue_match / 16) * (sat_match / 16)) image_debug(matched) blobs = matched.findBlobs(100, 1) if blobs is not None: blob_size = blobs[-1].area() image_size = image.area() #print blob_size / image_size if blob_size / image_size < blobs_threshold: print 'Blobs too small!' seek() else: (x,y) = blobs[-1].centroid()
error = target - current_angle integral += error derivative = error - prev_error prev_error = error control = kp * error + ki * integral + kd * derivative sys.stderr.write(control) sys.stderr.write(error * kp) sys.stderr.write(integral * ki) sys.stderr.write(derivative * kd) sys.stderr.write(plant(control)) return plant(control) while True: start_time = time.clock() image = cvutils.wb(camera.getImage().scale(0.075), lab_grey_sample) v, s, h = image.toHSV().splitChannels() hue_match = h.colorDistance((lab_goal_blue[0][0], lab_goal_blue[0][0], lab_goal_blue[0][0])).binarize( lab_goal_blue[0][1] * 3 ) sat_match = s.colorDistance((lab_goal_blue[1][0], lab_goal_blue[1][0], lab_goal_blue[1][0])).binarize( lab_goal_blue[1][1] * 3 ) matched = (hue_match / 16) * (sat_match / 16) matched.show() blobs = matched.findBlobs(100, 1) if blobs is not None: blob_size = blobs[-1].area() image_size = image.area() # print blob_size / image_size if blob_size / image_size < blobs_threshold: