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:
