概要

MicroPythonの勉強も兼ねて、使っていないまま眠らせていたパーツ類を有効活用したい！
興味を持って購入したもののあまり活用できていなかったMaixBit^[1] (旧モデル)とはるか昔に購入した某ディ○ゴスティーニの部品を活用して実用性皆無のロボットを作成してみました。

動作

2軸マニピュレータの先端に搭載されたMaixBitのカメラで人の顔を検出し、二つのサーボモータで首先を横、縦の２方向に振ることができます。待機中は首を横に振って人の索敵？を行い、人を見つけたあとは画像内の顔位置から回転角を計算して相手を正面に捉え、サウンドを鳴らす＆LEDを点灯しながら頭を縦に振ってお辞儀を行います。

なお、顔検出位置の追尾に当たっては、画像中心と顔位置のピクセル偏差をmmに換算し、焦点距離とのarctanによって回転すべき角度を算出しています。
（なお、ガバガバ計算のため適当なゲインをかけたりして調整しています...汗）

主な材料

注：MaixBitはファームウェア及び検出モデルの事前書き込みが必要ですが、その手順については本記事では割愛いたします。

接続

サーボモータ、ブザーの制御のためにPIN9, 10, 15をPWM信号出力用として利用しています。
(ちなみにMaixBitは４つのチャンネルをもつハードウェアタイマーが３つ搭載されており、最大12チャンネルののPWM制御が可能です)
また、サーボモータの駆動用の電源はMaixBitの5Vから供給しました。

今回の利用Pinは以下の通りです。

ソースコード

音声再生、及びサーボ制御(RS306MD用)のPWM制御については、自作の関数を作成しました。
なお、RS306MDはPWM方式とTTL方式での動作が可能であり、TTL方式を利用するとサーボの角度情報等も取得可能ですが、今回は簡単のためにPWM制御を選択しました。

# pwm_servo.py

# assuming that freq is 50Hz
# 560μs : +144deg, 1520μs : 0deg, 2480μs : -144deg
from time import sleep_ms
class Servo:
    def __init__(self, ch):
        self.ch = ch

    def _angle2duty(self, angle, freq):
        angle = 144 if angle > 144 else angle
        angle = -144 if angle < -144 else angle
        T = 1.0/freq
        pwidth = 560e-6 + (144 - angle)*(960e-6/144)
        duty = pwidth / T
        return duty * 100

    def angle(self, angle, freq=50, wait=1000):
        self.ch.duty(self._angle2duty(angle, freq))
        sleep_ms(wait)

# speak.py

import machine
import time

# https://www.aihara.co.jp/~taiji/browser-security/js/equal_temperament.html
C = 269.292
Cs = 285.305
D = 302.270
Ds = 320.244
E = 339.286
F = 359.461
Fs = 380.836
G = 403.482
Gs = 427.474
A = 452.893
As = 479.823
B = 508.355
scale = [C,D,E,F,G,A,B]

class Speaker:
  def __init__(self, pin=10, volume=2):
    self.pwm_init = False
    self.pin = pin
    self._timer = None
    self._volume = volume
    self._beat_time = 500
    self._timer = machine.Timer(
        machine.Timer.TIMER0,
        machine.Timer.CHANNEL0,
        period=200,
        mode= machine.Timer.MODE_ONE_SHOT,
        callback=self._timeout_cb,
        start=False
    )
    self._pwm_timer = machine.Timer(
        machine.Timer.TIMER0,
        machine.Timer.CHANNEL1,
        mode=machine.Timer.MODE_PWM,
    )


  def checkInit(self):
    if self.pwm_init == False:
      self.pwm_init = True
      self.pwm = machine.PWM(self._pwm_timer, freq = 1, duty = 0, pin=self.pin)

  def _timeout_cb(self, timer):
    self.checkInit()
    self.pwm.duty(0)
    time.sleep_ms(1)
    self.pwm.freq(1)

  def tone(self, freq=1800, duration=200, volume=None, timer=True):
    # 可聴域に制限
    duration = min(max(30, duration), 2000)
    freq = min(max(20, freq), 20000)
    self.checkInit()
    if volume == None:
      self.pwm.init(freq=freq, duty=self._volume)
    else:
      self.pwm.init(freq=freq, duty=volume)
    if timer:
      self._timer.period(duration)
      self._timer.start()
      time.sleep_ms(duration-15)
    else:
      time.sleep_ms(duration)
      self.pwm.duty(0)
      time.sleep_ms(1)
      self.pwm.freq(1)

  def sing(self, freq=1800, beat=1, volume=None):
    self.tone(freq, int(beat*self._beat_time), volume)

  def sleep(self, duration=200, timer=True):
    self.tone(0, duration, volume=0)

  def setBeat(self, value=120):
    self._beat_time = int(60000 / value)

  def setVolume(self, val):
    self._volume = val

  def sample(self):
    self.tone(G*2)
    self.tone(Fs*2)
    self.tone(Ds*2)
    self.tone(A/2*2)
    self.tone(Gs/2*2)
    self.tone(E*2)
    self.tone(G*2)
    self.tone(C*2*2,500)

以上２ファイルをimport用のモジュールとして事前に転送したのち、main.pyとして以下のファイルを転送します（boot.py, main.pyという名前にしておくと電源投入後に自動実行されます）。

# main.py

import time
from Maix import GPIO
import KPU as kpu
from fpioa_manager import fm
from board import board_info
import sensor
import lcd
from machine import Timer,PWM
from rs306md import Servo
from speak import *
import math

f_dist = 0.0036
center = (160,120)
mm_per_pix = 0.0053 / 320

def track_face(servo0, servo1, spk, curr_angle, curr_code, wait=100):
    x_ul, y_ul, width, height = curr_code[0].rect()
    x = x_ul + width/2
    x_diff_pix = x - center[0]
    x_diff_mm = x_diff_pix * mm_per_pix
    r =  20
    angle = int(curr_angle + math.atan(x_diff_mm/f_dist) * r)
    servo0.angle(angle, wait=100)
    spk.sample()
    servo1.angle(-40)
    servo1.angle(-20)

lcd.init()
sensor.reset()
sensor.set_pixformat(sensor.RGB565)

sensor.set_framesize(sensor.QVGA)
sensor.set_vflip(1)
sensor.run(1)

fm.register(board_info.LED_R, fm.fpioa.GPIO0, force=True)
led_r = GPIO(GPIO.GPIO0, GPIO.OUT)

#task = kpu.load("/sd/facedetect.kmodel")
task = kpu.load(0x300000)
anchor = (1.889, 2.5245, 2.9465, 3.94056, 3.99987, 5.3658, 5.155437, 6.92275, 6.718375, 9.01025)
kpu.init_yolo2(task, 0.5, 0.3, 5, anchor)

tim0 = Timer(Timer.TIMER1, Timer.CHANNEL0, mode=Timer.MODE_PWM)
tim1 = Timer(Timer.TIMER1, Timer.CHANNEL1, mode=Timer.MODE_PWM)
ch0 = PWM(tim0, freq=50, duty=0, pin=15)
ch1 = PWM(tim1, freq=50, duty=0, pin=9)
servo0 = Servo(ch0)
servo1 = Servo(ch1)
spk = Speaker()

angle = 0
diff = 6
reverse_flg = False
servo1.angle(-20)
cnt = 0
while True:
    img = sensor.snapshot()
    code = kpu.run_yolo2(task, img)
    if code != None and cnt > 5:
        led_r.value(0)
        img.draw_rectangle(code[0].rect())
        x, y = code[0].rect()[:2]
        lcd.display(img)
        track_face(servo0, servo1, spk,  angle, code)
        cnt = 0

    else:
        lcd.display(img)
        led_r.value(1)
        servo0.angle(angle, wait=2)

    if reverse_flg:
        diff = -diff
        reverse_flg = False
    elif angle > 90:
        angle = 90
        reverse_flg = True
    elif angle < -90:
        angle = -90
        reverse_flg = True

    angle += diff
    cnt += 1

kpu.deinit(task)
fm.unregister(board_info.LED_R)

動作の様子

gif画像のため音声は省かれていますが、人物検出中には緑色LEDが点灯しています。

まとめ

micropythonの学習という目的の元、自宅に眠っていたジャンクパーツをかき集めて簡単なロボット？を作成してみました。実用性皆無でしたが、在籍している学校の玄関に設置したところ比較的好評でした笑
また、今回の作品ではMaixBitに顔検出のモデルを載せましたが、学習ずみのモデルとして物体認識や数字認識、マスク着用の検出モデルなども存在します。それらのモデルを活用してみても面白いことができそうです。

---

1. MaixBitとは中国のSipeed社が販売している開発ボードで、カメラ、2.4インチ液晶ディスプレイがセットのうえ、K210というニューラルネットワーク演算に適したチップが搭載されており、マイコンでありながら、顔検出や音声認識のためのAIモデルが動かせるという代物です。 ↩︎

2. 同社が販売しているRS304MDというモデルの互換品と思われます。 ↩︎

3. 固定用の部材も秋葉原のジャンクショップで買った激安カメラについていたものを利用しました。 ↩︎