画像認識で追いかけるロボットカーを作ってみた

スポンサーリンク

作った結果はこんな感じ。
f:id:sato_susumu:20200924235332j:plain
といっても静止画なので全然伝わらないと思いつつ、キリがいいので、記録とし記事化。

組立方法

必要な材料や接続方法は、以前の投稿の内容を組み合わせただけ。
www.sato-susumu.com
www.sato-susumu.com

事前準備

Huskylensをポチポチ操作して、認識したいものを学習させておく。

プログラム

MakeCode環境用のpythonプログラム。
このプログラムは初期状態では、Huskylens色認識で認識した色を追いかける。
Huskylens側のダイヤルスイッチでアルゴリズムを切り替えれば、特定の顔やオブジェクトを追いかけることもできる。

state = -1
# 状態遷移したときの時間
active_begin = 0

# Huskylens初期化
basic.show_icon(IconNames.CHESSBOARD)
huskylens.init_i2c()
basic.show_icon(IconNames.HEART)

# とりあえず初期アルゴリズム設定
# 設定せずにHuskylens本体アルゴリズム設定にまかせてもいい
# あとから本体のアルゴリズム設定を変えてもいい
huskylens_switch_algorithm(3)

def on_forever():
    # キビキビ動かすためにループを作って、その中で処理。
    # LED表示など時間のかかる処理もなるべく行わないように注意。
    set_state(0)
    while True:
        huskylens.request()
        count = 0
        if huskylens.isAppear_s(HUSKYLENSResultType_t.HUSKYLENS_RESULT_BLOCK) == True:
            if huskylens.is_appear(1, HUSKYLENSResultType_t.HUSKYLENS_RESULT_BLOCK) == True:
                count = 1
        if count > 0:
            xc = huskylens.reade_box(1, Content1.X_CENTER)
            yc = huskylens.reade_box(1, Content1.Y_CENTER)
            w = huskylens.reade_box(1, Content1.WIDTH)
            h = huskylens.reade_box(1, Content1.HEIGHT)
        else:
            xc = -1
            yc = -1
            w = -1
            h = -1
        now_time = input.running_time()
        active_duration = now_time - active_begin

        if state == 0:
            idle_state_proc(count, xc, yc, w, h, active_duration)
        elif state == 1:
            back_state_proc(count, xc, yc, w, h, active_duration)
        elif state == 2:
            forward_state_proc(count, xc, yc, w, h, active_duration)
        elif state == 3:
            turn_state_proc(count, xc, yc, w, h, active_duration)
        elif state == 4:
            search_left_state_proc(count, xc, yc, w, h, active_duration)
        elif state == 5:
            search_right_state_proc(count, xc, yc, w, h, active_duration)
        elif state == 6:
            search_forward_state_proc(count, xc, yc, w, h, active_duration)
basic.forever(on_forever)

def idle_state_proc(count, xc, yc, w, h, active_duration):
    update_servo_value(0, 0, 0, 0)
    if count > 0:
        if w >= 150:
            set_state(1)
            return
        if xc >= 120 and xc <= 320 - 120:
            set_state(2)
            return
        else:
            set_state(3)
            return
    if active_duration > 2000:
        num = randint(1,4)
        if num == 1:
            set_state(4)
            return
        if num == 2:
            set_state(5)
            return
        if num == 3:
            set_state(6)
            return

def back_state_proc(count, xc, yc, w, h, active_duration):
    if active_duration < 2000:
        update_servo_value(0, -50, 0, 0)
        return
    if count == 0:
        set_state(0)
        return
    if w < 150:
        set_state(0)
        return

def forward_state_proc(count, xc, yc, w, h, active_duration):
    if count == 0:
        set_state(0)
        return
    if w >= 150:
        set_state(0)
        return
    if xc <= 60 or xc >= 320 - 60:
        set_state(0)
        return
    if active_duration > 300:
        update_servo_value(0, 50, 0, 0)
        return

def turn_state_proc(count, xc, yc, w, h, active_duration):
    if count == 0:
        set_state(0)
        return
    if w >= 150:
        set_state(0)
        return
    if xc >= 120 and xc <= 320 - 120:
        set_state(0)
        return
    if active_duration > 300:
        if xc < 120:
            update_servo_value(0, 0, 30, 0)
            return
        else:
            update_servo_value(0, 0, -30, 0)
            return

def search_left_state_proc(count, xc, yc, w, h, active_duration):
    if count > 0:
        set_state(0)
        return
    if active_duration > 2500:
        set_state(0)
        return
    update_servo_value(0, 0, 30, 0)

def search_right_state_proc(count, xc, yc, w, h, active_duration):
    if count > 0:
        set_state(0)
        return
    if active_duration > 2500:
        set_state(0)
        return
    update_servo_value(0, 0, -30, 0)

def search_forward_state_proc(count, xc, yc, w, h, active_duration):
    if count > 0:
        set_state(0)
        return
    if active_duration > 2500:
        set_state(0)
        return
    update_servo_value(0, 30, 0, 0)

def set_state(new_state):
    global state, active_begin
    if state != new_state:
        active_begin = input.running_time()
        # basic.show_number(new_state)
    state = new_state

def huskylens_switch_algorithm(mode):
    if mode == 0:
        huskylens.init_mode(protocolAlgorithm.ALGORITHM_FACE_RECOGNITION)
        basic.show_string("F")
    if mode == 1:
        huskylens.init_mode(protocolAlgorithm.ALGORITHM_OBJECT_TRACKING)
        basic.show_string("O")
    if mode == 2:
        huskylens.init_mode(protocolAlgorithm.ALGORITHM_LINE_TRACKING)
        basic.show_string("L")
    if mode == 3:
        huskylens.init_mode(protocolAlgorithm.ALGORITHM_COLOR_RECOGNITION)
        basic.show_string("C")
    if mode == 4:
        huskylens.init_mode(protocolAlgorithm.ALGORITHM_TAG_RECOGNITION)
        basic.show_string("T")

def update_servo_value(lx, ly, rx, ry):
    # micro:bitのLEDが見やすいようにお尻方向にカメラを搭載した。
    # 「LEDが前」から「カメラが前」に変更し、位置関係を真逆にしたため反転させる。
    lx = -lx
    ly = -ly
    rx = -rx
    ry = -ry

    magnitude = Math.sqrt((lx/100.0)**2 + (ly/100.0)**2)
    magnitude = Math.min(magnitude, 1.0)
    base_angle = Math.atan2(ly, lx)
    angle = base_angle - Math.PI / 4
    rightX = rx / 100.0
    v1 = magnitude * Math.cos(angle) + rightX
    v2 = magnitude * Math.sin(angle) - rightX
    v3 = magnitude * Math.sin(angle) + rightX
    v4 = magnitude * Math.cos(angle) - rightX

    # left front
    SuperBit.motor_run(SuperBit.enMotors.M1, v1 * 255)
    # right front
    SuperBit.motor_run(SuperBit.enMotors.M3, v2 * 255)
    # left rear
    SuperBit.motor_run(SuperBit.enMotors.M2, v3 * 255)
    # right rear
    SuperBit.motor_run(SuperBit.enMotors.M4, v4 * 255)