作った結果はこんな感じ。
といっても静止画なので全然伝わらないと思いつつ、キリがいいので、記録とし記事化。
組立方法
必要な材料や接続方法は、以前の投稿の内容を組み合わせただけ。
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)
