M5UnitVのカメラでスライムベスを自動追尾してみた


https://github.com/gitnabeshin/M5UnitV_trackball

# はじめに
* M5UnitVで色々遊んでみるシリーズの第３弾。
* 今回はRoboTakaoさんの[こちらのページ](http://blog.robotakao.jp/blog-entry-399.html)と[公式サイトのコード](https://github.com/m5stack/M5-ProductExampleCodes/tree/master/App/UnitV/track_ball)を参考にカラーボール（スライムベス）の色追尾をやってみます。
 * 公式およびRoboTakaoさんはメカナムホイールの車体を使って自動追尾をされていますが、こちらはキャタピラ付きの車体で行きます。
 * メカナムホイールは前後両輪で計４つのモーター制御を行いますが、こちらは左右で計２つのモーター制御になるので、これに合わせてコードも書き換えます。
* ソフト開発環境については、事前に以下で記載済みの環境がインストールされている事を前提とします。
 * [M5UnitV で物体認識（開発環境セットアップ編）](https://qiita.com/Nabeshin/items/7d49ca51cfdb10b70014)

![IMG_3337D.jpeg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/283854/1dc6ee79-0287-a414-5b83-1b1559219e1a.jpeg)

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">スライムベスを自動追尾するスライム。<br><br>An auto tracking Slime which tracks Slime Beth. <a href="https://t.co/E1SMUIph7w">pic.twitter.com/E1SMUIph7w</a></p>&mdash; Nabeshin (@desmoquattro996) <a href="https://twitter.com/desmoquattro996/status/1346045319503482880?ref_src=twsrc%5Etfw">January 4, 2021</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

# 車体の組み立て

![IMG_3346D.jpeg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/283854/c2ab63a4-a9e9-4919-3293-930a7e6a2175.jpeg)

M5UnitV, M5Stack Gray以外に揃えるのは以下です。

|品名|価格|
|:---|:---|
|[タミヤ 楽しい工作シリーズ No.237 連結式クローラー&スプロケットセット](https://www.amazon.co.jp/gp/product/B088FKPL9X/)|¥1,100|
|[タミヤ 楽しい工作シリーズ No.97 ツインモーターギヤーボックス](https://www.amazon.co.jp/タミヤ-楽しい工作シリーズ-No-97-ツインモーターギヤーボックス-70097/dp/B001Q119AC/)|¥574|
|[タミヤ 楽しい工作シリーズ No.157 ユニバーサルプレート 2枚セット](https://www.amazon.co.jp/gp/product/B001VZHRXG)|¥464|
|[KKHMF 5個 L9110S Hブリッジ ステッパ モータドライブ モジュール](https://www.amazon.co.jp/gp/product/B07GD8Q9T5)|¥396|
|[電池ボックス　単３×２本　リード線・フタ・スイッチ付き](https://akizukidenshi.com/catalog/g/gP-11523/)|¥80|
|[電池ボックスのケーブル端子（メス）](https://akizukidenshi.com/catalog/g/gP-03476/)|¥330/10本 |

* 連結式クローラ（キャタピラ）
 * 今回カッコいいやつを使っていますが、安いので良いって方は以下でもOKです。
 * [タミヤ 楽しい工作シリーズ No.100 トラック&ホイールセット](https://www.amazon.co.jp/タミヤ-楽しい工作シリーズ-No-100-ホイールセット-70100/dp/B001VZJDY2)(¥457) 

* ギアボックスは２種類あります。
 * 写真のものは実は以下の新しい幅広タイプですが、上記リストにある方が使いやすいのでオススメです。
 * いつの間にか新しいタイプが販売されていて自分は知らずに購入してしまい、ユニバーサルプレートの加工が必要になりました。
 * リスト記載のタイプは無加工で使えます。
 * [タミヤ 楽しい工作シリーズ No.168 ダブルギヤボックス 左右独立4速タイプ](https://www.amazon.co.jp/タミヤ-楽しい工作シリーズ-No-168-ダブルギヤボックス-左右独立4速タイプ/dp/B001Q13BIU)(¥626)

* 電池ボックスのケーブル端子
 * 後述のモータドライバのGND/VCC端子につなぐための端子です。
 * 圧着工具があれば端子の部品だけを買って自作した方が安いです。
 * 割り切って再利用性を無視できるなら、直接ハンダづけしちゃっても良いと思います。

まあ、この辺はどっちの組み合わせでも使えるので、好みで使い分ければ良いでしょう。

![IMG_3345D.jpeg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/283854/dda44868-9300-034c-7ab9-2c945ab493f8.jpeg)

* 上の写真は「L9110S Hブリッジ ステッパ モータドライブ モジュール」です。
 * これ１個で２つのモータを制御することができます。
 * なんだかんだで、これが一番安くて使いやすいので自分は何個もストックしています。
* セオリー通り、マイコンの電源はM5Stack内蔵バッテリから供給し、モータ駆動用の電源は電池ボックスから別系統で供給します。

![IMG_3341D.jpeg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/283854/5c9f2387-7837-d2d3-28de-9011b8268f04.jpeg)

* モータドライバの緑のターミナル`MotorA`, `MotorB`はそれぞれ左右のモータと繋ぎます。
 * `MOTOR A`/`車体正面に向かって左側のモータ`
 * `MOTOR B`/`車体正面に向かって右側のモータ`
* M5Stackからモータドライバには`GPIO16, 17, 5, 26`を接続。
 * `A-1A`/`GPIO16`
 * `A-1B`/`GPIO17`
 * `B-1A`/`GPIO5`
 * `B-1B`/`GPIO26`
 * `GND`/`電池ボックスの - (黒いケーブル)`
 * `VCC`/`電池ボックスの + (赤いケーブル)`
* M5Stack, M5UnitVとはGroveケーブルで接続です。
 * M5UnitV買うと一緒についてきます。

![IMG_3344D.jpeg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/283854/29a040a8-1567-a79d-8cc2-3e03e3972c83.jpeg)

* UnitVはレゴのマウントパーツを使って車体に固定しました。
 * 自分はM5Stack Fireに同梱されてきたレゴマウントパーツを使用したが、持ってなければ両面テープとかで適当に。
 * 車体にはスポンジを適当な大きさに切って固定します。
 * 電池ボックスも100均の両面テープで固定します。

![IMG_3343D.jpeg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/283854/932c979b-38fb-b149-ed2d-b8eb315aa457.jpeg)

という事で、ハードの組み立てはこれで終了です。


# MaixPyの`しきい値エディタ`で判定対象物の色判定しきい値を調整
* 次にMaixPyを起動し、追尾させたい物体の色のしきい値を導出します。
 * ここで導出したしきい値は、後述のコードに貼り付ける事で、UnitVで認識できるようになります。
* 認識させたい物体は、こんな感じで白い紙の上に置きます。
 * 影があまり出ない感じで配置するとしきい値が綺麗に決まり易いです。

![IMG_3347.jpeg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/283854/6948e57f-3b5c-d321-61cd-995406b01057.jpeg)


* MaixPyを起動し、デフォルトのサンプルコードを実行、カメラの映像が取れる状態にします。
 * とりあえずカメラの映像が右上に出てればプログラムは何でも良いです。
 * 以下は、新規作成アイコンを押すと出てくる最初のコードを使っています。
* 以下のように、`ツール` --> `マシンビジョン`  --> `しきい値エディタ`を起動。
 * `ソースイメージの場所`を聞かれるので、`フレームバッファ`を選択。
 * ここでボタンを押したタイミングのカメラ画像が使われるので、カメラアングルを合わせた状態でボタンを押します。

![スクリーンショット 2021-01-06 19.24.52.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/283854/d0c8d88f-9cc6-0aa3-3a24-e5f758ef4723.png)

* こんな感じで`しきい値エディタ`が起動します。
 * 追尾対象の色に応じて、下のスライドバーをいじり、良い感じで物体領域が2値化できるまで調整します。
 * 綺麗に2値化出来たら、下部の`LABしきい値`という部分のパラメータをコピーし、後述のUnitV側のコードに貼り付けます。

![スクリーンショット 2021-01-06 19.31.47.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/283854/e10f6c6e-c54b-b84b-cd7b-c835dbd0e284.png)

* せっかくなので、ついでに赤色と青色の物体のしきい値も作っておきました。

```
# orange
target_lab_threshold = (28, 84, 2, 127, 4, 127)

# red
#target_lab_threshold = (0, 100, 31, 127, 19, 127)

# blue
#target_lab_threshold = (0, 100, 35, 127, -128, 127)
```


# UnitVコード
* 以下のコードをUnitVのSDカード直下にboot.pyという名前で配置します。
* [GitHubのコード](https://github.com/m5stack/M5-ProductExampleCodes/blob/master/App/UnitV/track_ball/track_ball.py)からの差分は以下です。
 * 先ほどの`しきい値エディタ`で算出したしきい値を`target_lab_threshold`に指定。
 * GitHubのコードは一部誤りがあったので、横軸方向の検出位置 `dx = 160 - target[5]`の部分を修正しています。
 * 自分の場合、UnitVのケーブルが下に来る向きが使いやすかったので、`sensor.set_vflip(1)`でカメラ画像の上下反転を行っています。
 * また、ちゃんと起動したか分かるよう、起動時にLEDを点滅させます。
 * さらに、ちゃんと検出したか分かるよう、検出した場合にLEDをオレンジに点灯させます。

```python:Track_ball.py

import sensor
import image
import time
import utime
from machine import UART
from Maix import GPIO
from fpioa_manager import *
from modules import ws2812

# RGB LED設定
class_ws2812 = ws2812(8,100)
BRIGHTNESS = 0x10

#fm.register(34,fm.fpioa.UART1_TX)
#fm.register(35,fm.fpioa.UART1_RX)
fm.register(35, fm.fpioa.UART1_TX, force=True)
fm.register(34, fm.fpioa.UART1_RX, force=True)
uart_out = UART(UART.UART1, 115200, 8, None, 1, timeout=1000, read_buf_len=4096)


sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.set_vflip(1)
sensor.run(1)
sensor.skip_frames()

while False:
    uart_out.write('TEST\n')
    utime.sleep_ms(100)

# RGB LEDを点滅
def BLINK_RGB_LED(r,g,b):
    a = class_ws2812.set_led(0,(r,g,b))
    a = class_ws2812.display()
    time.sleep(0.3)
    a = class_ws2812.set_led(0,(0,0,0))
    a = class_ws2812.display()
    time.sleep(0.3)
    a = class_ws2812.set_led(0,(r,g,b))
    a = class_ws2812.display()

def RGB_LED_ON(r,g,b):
    a = class_ws2812.set_led(0,(r,g,b))
    a = class_ws2812.display()

def RGB_LED_OFF():
    a = class_ws2812.set_led(0,(0,0,0))
    a = class_ws2812.display()

# 起動インジケータとしてgreenで点滅、最後に点灯
BLINK_RGB_LED(0,BRIGHTNESS,0)
BLINK_RGB_LED(0,BRIGHTNESS,0)
BLINK_RGB_LED(0,BRIGHTNESS,0)

# orange
target_lab_threshold = (28, 84, 2, 127, 4, 127)

# red
#target_lab_threshold = (0, 100, 31, 127, 19, 127)

# blue
#target_lab_threshold = (0, 100, 35, 127, -128, 127)

while True:
    img=sensor.snapshot()

    blobs = img.find_blobs([target_lab_threshold], x_stride = 2, y_stride = 2, pixels_threshold = 100, merge = True, margin = 20)
    if blobs:
        max_area = 0
        target = blobs[0]
        dx = 0
        for b in blobs:
            if b.area() > max_area:
                max_area = b.area()
                target = b
        # シリアル通信相手がいる場合に検出領域情報を送る
        if uart_out.read(4096):
            # bounding box area
            area = target.area()
            # dx[-160 to 160, 0 is muddle.] x:target[5], y:target[6]  QVGA:320*240
            dx = 160 - target[5]
            hexlist = [(dx >> 8) & 0xFF, dx & 0xFF, (area >> 16) & 0xFF, (area >> 8) & 0xFF, area & 0xFF]
            uart_out.write(bytes(hexlist))
        else:
            pass

        print( 160 - target[5] , target.area())
        tmp=img.draw_rectangle(target[0:4])
        tmp=img.draw_cross(target[5], target[6])
        c=img.get_pixel(target[5], target[6])
        # rgb orange
        RGB_LED_ON(255, 165, 0)

    else:
        # 検出なし rgb led off
        RGB_LED_OFF()
        if uart_out.read(4096):
            hexlist = [0x80, 0x00, 0x00, 0x00, 0x00]
            uart_out.write(bytes(hexlist))
        else:
            pass

```

# M5Stackコード
* 今回はM5Stack Grayを使用しました。
* [GitHubのコード](https://github.com/m5stack/M5-ProductExampleCodes/blob/master/App/UnitV/track_ball/track_ball.ino)からの主な差分は以下。
 * 公式のコードはメカナムホイールの車体で4モータだが、こちらは左右キャタピラによる2モータの構成。
 * モータ制御用に`GPIO17, 16, 5, 26`を使用。これで前後左右停止する`handleXXXX()`関数を追加。
 * speakerノイズ音を消すコード追加 `dacWrite(25, 0); // Speaker OFF`
 * `if(v_data.area > 30000)` で近づきすぎ状態を判定してバックするためのしきい値を拡大。
 * UnitVの取り付け方向に合わせて`state = kRight; // Go right`、`state = kLeft;  // Go left`を入れ替え。
 * `loop()`の最後に`delay(10);`を追加。（これ入れないとシリアル読み込みが始まらなかった）
* コードでやってる制御としては、以下です。
 * スライムベスのオレンジ領域が検出出来なかったら、検出できるまで右回転する。
 * 領域が画面右寄りに検出されたら、車体を右に動かす。
 * 左寄りに検出されたら、車体を左に動かす。
 * 中心部分に検出されたら、前進して近づく。
 * 近づきすぎて画面いっぱいに検出されたら、後退する。

```c:Track_ball.ino

#include <M5Stack.h>

#define MOTOR_A1        17
#define MOTOR_A2        16
#define MOTOR_B1        5
#define MOTOR_B2        26
//21, 22 for UART
//25 speaker

HardwareSerial VSerial(1);

typedef struct {
    int16_t dx;         //object position x
    uint32_t area;      //object detected area
} v_response_t;

void myPrintln( String str ) {
    // Lcdに書くとシリアル通信が遅くなる
    //   M5.Lcd.setCursor(0, 30);
    //   M5.Lcd.println(str);
    Serial.printf("[%s]\n", str);
}

void handleForward() {
    myPrintln(" FORWARD");
    digitalWrite(MOTOR_A1, LOW);   //LEFT
    digitalWrite(MOTOR_A2, HIGH);
    digitalWrite(MOTOR_B1, LOW);   //RIGHT
    digitalWrite(MOTOR_B2, HIGH);
    delay(20);
    digitalWrite(MOTOR_A1, LOW);  //LEFT
    digitalWrite(MOTOR_A2, LOW);
    digitalWrite(MOTOR_B1, LOW);  //RIGHT
    digitalWrite(MOTOR_B2, LOW);
}

void handleStop() {
    myPrintln(" STOP");
    digitalWrite(MOTOR_A1, LOW);  //LEFT
    digitalWrite(MOTOR_A2, LOW);
    digitalWrite(MOTOR_B1, LOW);  //RIGHT
    digitalWrite(MOTOR_B2, LOW);
    delay(20);
}

void handleLeft() {
    myPrintln(" LEFT");
    digitalWrite(MOTOR_A1, HIGH); //RIGHT
    digitalWrite(MOTOR_A2, LOW);
    digitalWrite(MOTOR_B1, LOW);  //LEFT
    digitalWrite(MOTOR_B2, HIGH);
    delay(30);
    digitalWrite(MOTOR_A1, LOW);  //RIGHT
    digitalWrite(MOTOR_A2, LOW);
    digitalWrite(MOTOR_B1, LOW);  //LEFT
    digitalWrite(MOTOR_B2, LOW);
    delay(10);
}

void handleRight() {
    myPrintln(" RIGHT");
    digitalWrite(MOTOR_A1, LOW);   //RIGHT
    digitalWrite(MOTOR_A2, HIGH);
    digitalWrite(MOTOR_B1, HIGH);  //LEFT
    digitalWrite(MOTOR_B2, LOW);
    delay(30);
    digitalWrite(MOTOR_A1, LOW);  //RIGHT
    digitalWrite(MOTOR_A2, LOW);
    digitalWrite(MOTOR_B1, LOW);  //LEFT
    digitalWrite(MOTOR_B2, LOW);
    delay(10);
}

void handleBackward() {
    myPrintln(" BACK");
    digitalWrite(MOTOR_A1, HIGH); //RIGHT
    digitalWrite(MOTOR_A2, LOW);
    digitalWrite(MOTOR_B1, HIGH); //LEFT
    digitalWrite(MOTOR_B2, LOW);
    delay(20);
    digitalWrite(MOTOR_A1, LOW);  //RIGHT
    digitalWrite(MOTOR_A2, LOW);
    digitalWrite(MOTOR_B1, LOW);  //LEFT
    digitalWrite(MOTOR_B2, LOW);
}

void setup() {
    M5.begin();

    VSerial.begin(115200, SERIAL_8N1, 21, 22);

    dacWrite(25, 0); // Speaker OFF

    pinMode(MOTOR_A1, OUTPUT);
    pinMode(MOTOR_A2, OUTPUT);
    pinMode(MOTOR_B1, OUTPUT);
    pinMode(MOTOR_B2, OUTPUT);

    Serial.printf("Setup OK. \n");
}

enum {
    kNoTarget = 0,
    kLeft,          //1
    kRight,         //2
    kStraight,      //3
    kTooClose       //4
};

const uint16_t kThreshold = 30; // If target is in range ±kThreshold, the car will go straight
v_response_t v_data;    // Data read back from V
uint8_t state = 0;  // Car's movement status

void loop() {

    VSerial.write(0xAF);

    if(VSerial.available()) {
        uint8_t buffer[5];
        VSerial.readBytes(buffer, 5);
        // 先頭2byteがdx
        v_data.dx = (buffer[0] << 8) | buffer[1];
        // 残り3byteがarea
        v_data.area = (buffer[2] << 16) | (buffer[3] << 8) | buffer[4];

        //QVGA:320*240の幅dx
        if(v_data.dx > -160 && v_data.dx < 160) {
            if(v_data.area > 30000)
            {
                state = kTooClose;  // Back
            }
            else if(v_data.dx > -kThreshold && v_data.dx < kThreshold)
            {
                state = kStraight;  // Go straight
            }
            else if(v_data.dx <= -kThreshold)
            {
                // UnitVの上下がサンプルとは違って逆になる @sensor.set_vflip(1)
                state = kRight; // Go right
            }
            else if(v_data.dx >= kThreshold)
            {
                // UnitVの上下がサンプルとは違って逆になる 2sensor.set_vflip(1)
                state = kLeft;  // Go left
            }
            else
            {
                state = kNoTarget;  // Rotate to look for
            }
            M5.Lcd.fillScreen(GREEN);
        } else {
            state = kNoTarget;  // Rotate to look for
            M5.Lcd.fillScreen(RED);
        }

        Serial.printf("%d, %d, %d \n", v_data.dx, v_data.area, state);
    }

    //The speed and time here may need to be modified according to the actual situation
    switch(state) {
        case kNoTarget:
            handleRight();
            break;
        case kLeft:
            handleLeft();
            break;
        case kRight:
            handleRight();
            break;
        case kStraight:
            handleForward();
            break;
        case kTooClose:
            handleBackward();
            break;
        default:
            handleStop();
            break;
    }

    // これを入れないとシリアル読み込み出来ない。
    delay(10);
}
```

# 実行

全部配線して起動すると、結構軽快にオレンジを追尾する事ができました。

こんなちっちゃいデバイスでここまで動くと感動します。

![IMG_3342D.jpeg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/283854/833ab6b4-2363-1d7d-0396-c7ff2049a3c9.jpeg)


# 最後に

* 今回、初めて物体追尾に挑戦した。
 * 結構敷居が高い気がしていたが、M5UnitVを使う事で手軽に手元で実現できた。
 * やっぱ実際にやってみると仕組みが色々分かり、勉強になる。
* 次は、物体認識させてそれを自動追尾できるように挑戦したい。
* M5Stack素晴らしい。