img = cv.resize(img, (640, 480))
img = cv.cvtColor(img, cv.COLOR_BGR2RGB)
pil_img = Image.fromarray(img)
pil_img, yolo_res = yolo.detect_image(pil_img)
cubes = get_obj_center(yolo, yolo_res, ['cube'])
gathered_cubes = [c for c in cubes if c[1] < -0.65]
cubes_to_gather = [c for c in cubes if c[1] > -0.65]
wrong_obj = get_obj_center(yolo, yolo_res, ['ball', 'star'])
gathered_wrong_obj = [c for c in wrong_obj if c[1] < -0.7]
if gathered_wrong_obj: # Go backward
controller.set_speed('a', -0.2)
controller.set_speed('b', 0.2)
continue
if not len(gathered_cubes):
target = cubes_to_gather[0] if len(cubes_to_gather) >= 1 else None
look_and_follow(controller, target, look_speed=0.15, asserv_p=0.4)
res_yolo = np.asarray(pil_img)
res_yolo = cv.cvtColor(res_yolo, cv.COLOR_RGB2BGR)
cv.imshow('Demo', res_yolo)
else:
_, center = get_black_line_center(img,
band_center_y=300,
band_width_ratio=1.0)
look_and_follow(controller, center, look_speed=0.15, asserv_p=0.4)
BIN2=27,
PWMB=22,
STBY=23)
cap = RemoteCapture('ws://rosa.local:5678')
try:
while True:
b, img = cap.read()
if not b:
continue
_, center = get_black_line_center(img,
band_center_y=300,
band_width_ratio=1.0)
if center is not None:
h, w, _ = img.shape
x = int((center[0] + 1.0) * 0.5 * w)
y = int((center[1] + 1.0) * 0.5 * h)
cv.circle(img, (x, y), 10, (0, 0, 255), -1)
look_and_follow(controller, center, look_speed=0.2, asserv_p=0.4)
cv.imshow('Follow black line', img)
cv.waitKey(1)
except KeyboardInterrupt:
controller.set_speed('a', 0)
controller.set_speed('b', 0)