Como seguir objetos com Maix bit e Servo Motor

Seguir objetos com visão computacional é uma tarefa bastante interessante pois possibilita por exemplo, fazer um robô humanoide que acompanha a trajetória dos objetos.

Por que usar o Servo Motor?

Embora exista inúmeras opções de motores no mercado para fazer os movimentos, o servo motor além de ser preciso como um motor de passo, é barato e leve. Por isso em alguns casos acaba sendo mais interessante sua aplicação.

Lógica de funcionamento

A maior dificuldade dessa aplicação é a conversão da localização do objeto de modo a controlar o servo com o ângulo de -90° a +90°.

Transferidor que ilustra o funcionamento do servo motor com o maix bit. Lógica que foi usada para fazer com que o servo siga o objetos — Fonte: Piliapp

Função map() no Maix bit

Por mais que o Maix bit não tenha essa função de forma nativa, é possível recriar usando como base na documentação do Arduino.

long map(long x, long in_min, long in_max, long out_min, long out_max) {
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

Particularmente optei por não criar uma função, pois não haveria mais de um uso desse mesma lógica, porém caso for necessário, pode ser implementado sem dificuldade. Se for o caso, ser feito até uma biblioteca, mas como não o caso desse exemplo, ficou da seguinte forma:

in_min = 0
in_max = 100
out_min = -90
out_max = 90
        
x = (invert_px - in_min) * (out_max - out_min) / (in_max - in_min) +

Diagrama Esquemático

Como este post é apenas uma demonstração para servir como base para aplicações mais complexas, seguirá o mesmo diagrama do post “servo motor com maix bit“.

Código para Seguir objetos com Maix bit

Por mais que o código abaixo pareça complexo, apenas se trada da união do código de localização de objeto e servo motor com Maix bit. Além disso, tem a mesma equação da função map() do Arduino, para converter a posição em pixel em ângulo para o servo motor.

import sensor,image,lcd,time
import KPU as kpu
import ujson
from machine import Timer,PWM
tim = Timer(Timer.TIMER0, Timer.CHANNEL0, mode=Timer.MODE_PWM)
S1 = PWM(tim, freq=50, duty=0, pin=9)

lcd.init(freq=15000000)
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.set_vflip(1)
sensor.set_hmirror(1)
sensor.run(1)
clock = time.clock()
classes = ['aeroplane', 'bicycle', 'bird', 'boat', 'bottle', 'bus', 'car', 'cat', 'chair', 'cow', 'diningtable', 'dog', 'horse', 'motorbike', 'person', 'pottedplant', 'sheep', 'sofa', 'train', 'tvmonitor']
task = kpu.load("/sd/20class.kmodel") 
anchor = (1.889, 2.5245, 2.9465, 3.94056, 3.99987, 5.3658, 5.155437, 6.92275, 6.718375, 9.01025)
a = kpu.init_yolo2(task, 0.5, 0.3, 5, anchor)

def Servo(servo,angle):
    S1.duty((angle+90)/180*10+2.5)
    
Servo(S1, 0)
time.sleep(5)
rank_array = ([0 for x in range(80)])
all_area   = ([0 for x in range(80)])
location_x   = ([0 for x in range(80)])   
location_y   = ([0 for x in range(80)])

while(True):
    clock.tick()
    img = sensor.snapshot()
    code = kpu.run_yolo2(task, img)
    a = img.draw_circle(160, 120, 3, color=(255, 255, 0), fill=True)
    
    if code:
        number_obj = 0
        count = 0
        
        for i in code:
            a = img.draw_rectangle(i.rect())
            
            centroid_x = int(i.w() / 2)
            centroid_y = int(i.h() / 2)
            
            location_x[number_obj] = i.x() + centroid_x
            location_y[number_obj] = i.y() + centroid_y
            
            all_area[number_obj] = i.w() * i.h()
            
            a = lcd.display(img)
            number_obj += 1
            for i in code:
                lcd.draw_string(i.x(), i.y(), classes[i.classid()], lcd.RED, lcd.WHITE)
                lcd.draw_string(i.x(), i.y()+12, '%f'%i.value(), lcd.RED, lcd.WHITE)
    
        id_area = 0
        for j in range(number_obj):
            if all_area[j] > rank_array[0]:
                rank_array[0]= all_area[j]
                id_area = j

    if rank_array[0] > 0 and code:
        a = img.draw_circle(location_x[id_area], location_y[id_area], 3, color=(255, 0, 0), fill=True)
        percent_location_x = int(location_x[id_area] * 100 / 320)
        percent_location_y = int(location_y[id_area] * 100 / 240)
        json_map = {}
        json_map["x"] = percent_location_x
        json_map["y"] = percent_location_y
        json_percent_location = ujson.dumps(json_map)   
        #print(json_percent_location)
        
        invert_px = percent_location_x

        in_min = 0
        in_max = 100
        out_min = -90
        out_max = 90
        
        x = (invert_px - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
        Servo(S1, int(x))
        #print(x)
        count += 1
    a = lcd.display(img)
a = kpu.deinit(task)

Funcionamento do Maix bit Seguindo Objetos

Apesar do teste usar um palito de churrasco preso no servo motor, serve muito bem para confirmação do funcionamento. Quando o objeto se mexe, ele muda o ângulo do palito de forma proporcional a movimentação e claro que há momentos que fica estático pois o Maix bit deixou de reconhecer o objeto, por isso ele fica parado na ultima posição.