パスコンパスの日記

何か色々作るのが好きです

【キョロキョロV⑤】m5StickV サーボ2軸マシンで緑色のものを追いかける

m5StickV キョロキョロV ソフト python

　少し時間が空いてしまいましたが前回は緑色検出のしきい値設定に関して書きました。今回はそれに連動してサーボを動かすプログラムを公開したいと思います。

目次

キョロキョロVの記事のまとめ

　サーボを動かすプログラム、回路、メカの3Dデータ公開と組み立て、および色検出のしきい値設定に関して書いています。m5StickVで何か製作される際に少しでも役に立ちましたら幸いです。

yoichi-41.hatenablog.com

プログラムと解説

プログラム

※2020/04/03追記
SG-90 PDFデータシート
http://akizukidenshi.com/download/ds/towerpro/SG90_a.pdf

　上記データシートの様にサーボは本来50Hz固定でduty比を変えることで角度を変えるのですが、この記事では周波数を変えてしまっていました。
それでも動いてはいたのですが、dutyを0.025~0.12に変更する方が良さそうです。
動かすところを抜粋するとこんな形になります。

from machine import Timer,PWM

tim0 = Timer(Timer.TIMER0, Timer.CHANNEL0, mode=Timer.MODE_PWM)
ch0 = PWM(tim0, freq = 50, duty = 5, pin = 6)
d0_count = 10  #if文などでここを変える

duty0 =ChangeDuty(d0_count)

この方法でMaixBitを動かしている記事です。

yoichi-41.hatenablog.com

追記終わり

　まずコードだけ載せて後から解説させていただきます。 　基本的には緑を追跡するサンプルコードをベースにサーボを動かす処理を追加している形です。処理を関数化等していないので少し見づらいかもしれません。

from machine import Timer,PWM
import time
import lcd

import sensor
import image
import lcd

tim0 = Timer(Timer.TIMER0, Timer.CHANNEL0, mode=Timer.MODE_PWM)
tim1 = Timer(Timer.TIMER1, Timer.CHANNEL1, mode=Timer.MODE_PWM)
ch0 = PWM(tim0, freq= 50, duty=10, pin=34)  # 横振りサーボ
ch1 = PWM(tim1, freq= 50, duty=10, pin=35)  # 縦振りサーボ
freqval = 70    #初期角度
val = 1 #角度変化量

lcd.init()
lcd.rotation(2) #add
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.set_windowing((224, 224)) #add
sensor.run(1)
green_threshold   = (0,   80,  -70,   -10,   -0,   30)

#初期角度を向く
ch0.freq(freqval)
ch1.freq(freqval)

tempfreq0 = freqval
tempfreq1 = freqval

while True:
    img=sensor.snapshot()
    blobs = img.find_blobs([green_threshold])
    if blobs:
        for b in blobs:
            if b[4] > 200: # 緑の領域サイズが200以上の場合
                print(blobs)
                tmp=img.draw_rectangle(b[0:4])
                tmp=img.draw_cross(b[5], b[6])
                c=img.get_pixel(b[5], b[6])
                tempfreq0 =-(b[6]-110)/50+tempfreq0 #b[6]が首振り用のY座標
                tempfreq1 =(b[5]-110)/50+tempfreq1 #b[5]は縦振り用のX座標
                if tempfreq0 <40:
                    tempfreq0 = 40
                if tempfreq0 >130:
                    tempfreq0 = 130
                if tempfreq1 <70:
                    tempfreq1 = 70
                if tempfreq1 >100:
                    tempfreq1 = 100
                ch0.freq(tempfreq0)
                ch1.freq(tempfreq1)
    lcd.display(img)

座標

最初に座標を確認しました。

画面を2回ほど回転させた後の座標は上記写真の様になっています。右下が原点で左がxの+方向、上がyの+方向です。こちらは上記のプログラムの「blobs」という配列の中身をPCで見ながら確かめました。
「img.find_blobs関数」で取得した「blobs配列」の中身は以下の様になっています。これのcx, cyの要素を取り出して読んでいます。

{"x":158, "y":93, "w":65, "h":62, "pixels":2520, "cx":190, "cy":126, "rotation":0.829851, "code":1, "count":1}

また、カメラで映る範囲はxもyもおよそ座標値で220が最大の様です。そのため画面の中心の座標は(x, y)=(110, 110)になります。

アルゴリズム

　画面の中心座標から緑の物体の中心座標がどれだけ離れているかでサーボへの指令値を決めます。この部分です。

                tempfreq0 =-(b[6]-110)/50+tempfreq0 #b[6]が首振り用のY座標
                tempfreq1 =(b[5]-110)/50+tempfreq1 #b[5]は縦振り用のX座標

　後は様子を見ながら適当な係数で割って前回の周波数に足しています。中心付近の場合は係数で割って桁落ちが発生するため不感帯になります。また、下記の様に上下限値を決めて角度を制限しています*1。

                if tempfreq0 <40:
                    tempfreq0 = 40
                if tempfreq0 >130:
                    tempfreq0 = 130
                if tempfreq1 <70:
                    tempfreq1 = 70
                if tempfreq1 >100:
                    tempfreq1 = 100

最初は以下の様に座標とサーボの回転方向の整合性が取れず苦労しました。下の動画です。

動作の様子

　上記のプログラムでの動作の様子です。上下のサーボが回路的に接続が怪しかったりと動きが若干不安定ですが、ある程度追従ができています。振動がひどいですが係数を調整すれば動作が遅くなるものの軽減されると思います。

まとめ

　今回はm5StickVのカメラと追加した2軸のサーボを活かしてキョロキョロVで緑色のものを首を振りながら追跡するプログラムをご紹介させていただきました。思い通りに動かせると楽しいですね。
ただ、イベントで長時間動かしてみて見えてきたメカ的な課題等もありまして、ちょっと設計変更をしようかと思っています。次回は意図的にキョロキョロVを歩かせてみた(？)のでそちらに関して書かせていただく予定です。
読んで下さりありがとうございました。

*1:こちらはサーボの初期取り付け角度によって適切な値は変わると思います。