はじめに

M5UnitVで色々遊んでみるシリーズの第３弾。
今回はRoboTakaoさんのこちらのページと公式サイトのコードを参考にカラーボール（スライムベス）の色追尾をやってみます。
- 公式およびRoboTakaoさんはメカナムホイールの車体を使って自動追尾をされていますが、こちらはキャタピラ付きの車体で行きます。
- メカナムホイールは前後両輪で計４つのモーター制御を行いますが、こちらは左右で計２つのモーター制御になるので、これに合わせてコードも書き換えます。
ソフト開発環境については、事前に以下で記載済みの環境がインストールされている事を前提とします。
- M5UnitV で物体認識（開発環境セットアップ編）

スライムベスを自動追尾するスライム。

An auto tracking Slime which tracks Slime Beth. pic.twitter.com/E1SMUIph7w
— Nabeshin (@desmoquattro996) January 4, 2021

車体の組み立て

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

品名	価格
タミヤ楽しい工作シリーズ No.237 連結式クローラー&スプロケットセット	¥1,100
タミヤ楽しい工作シリーズ No.97 ツインモーターギヤーボックス	¥574
タミヤ楽しい工作シリーズ No.157 ユニバーサルプレート 2枚セット	¥464
KKHMF 5個 L9110S Hブリッジステッパモータドライブモジュール	¥396
電池ボックス　単３×２本　リード線・フタ・スイッチ付き	¥80
電池ボックスのケーブル端子（メス）	¥330/10本

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

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

モータドライバの緑のターミナル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買うと一緒についてきます。

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

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

MaixPyの`しきい値エディタ`で判定対象物の色判定しきい値を調整

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

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

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

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

# 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のコードからの差分は以下です。
- 先ほどのしきい値エディタで算出したしきい値をtarget_lab_thresholdに指定。
- GitHubのコードは一部誤りがあったので、横軸方向の検出位置 dx = 160 - target[5]の部分を修正しています。
- 自分の場合、UnitVのケーブルが下に来る向きが使いやすかったので、sensor.set_vflip(1)でカメラ画像の上下反転を行っています。
- また、ちゃんと起動したか分かるよう、起動時にLEDを点滅させます。
- さらに、ちゃんと検出したか分かるよう、検出した場合にLEDをオレンジに点灯させます。

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のコードからの主な差分は以下。
- 公式のコードはメカナムホイールの車体で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);を追加。（これ入れないとシリアル読み込みが始まらなかった）
コードでやってる制御としては、以下です。
- スライムベスのオレンジ領域が検出出来なかったら、検出できるまで右回転する。
- 領域が画面右寄りに検出されたら、車体を右に動かす。
- 左寄りに検出されたら、車体を左に動かす。
- 中心部分に検出されたら、前進して近づく。
- 近づきすぎて画面いっぱいに検出されたら、後退する。

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);
}

実行

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

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

最後に

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