はじめに
- M5UnitVで色々遊んでみるシリーズの第３弾。
- 今回はRoboTakaoさんのこちらのページと公式サイトのコードを参考にカラーボール（スライムベス）の色追尾をやってみます。
- 公式およびRoboTakaoさんはメカナムホイールの車体を使って自動追尾をされていますが、こちらはキャタピラ付きの車体で行きます。
- メカナムホイールは前後両輪で計４つのモーター制御を行いますが、こちらは左右で計２つのモーター制御になるので、これに合わせてコードも書き換えます。
- ソフト開発環境については、事前に以下で記載済みの環境がインストールされている事を前提とします。
- M5UnitV で物体認識（開発環境セットアップ編）
スライムベスを自動追尾するスライム。
— Nabeshin (@desmoquattro996) January 4, 2021
An auto tracking Slime which tracks Slime Beth. pic.twitter.com/E1SMUIph7w
車体の組み立て
M5UnitV, M5Stack Gray以外に揃えるのは以下です。
-
連結式クローラ（キャタピラ）
-
今回カッコいいやつを使っていますが、安いので良いって方は以下でもOKです。
-
ギアボックスは２種類あります。
-
写真のものは実は以下の新しい幅広タイプですが、上記リストにある方が使いやすいのでオススメです。
-
いつの間にか新しいタイプが販売されていて自分は知らずに購入してしまい、ユニバーサルプレートの加工が必要になりました。
-
リスト記載のタイプは無加工で使えます。
-
電池ボックスのケーブル端子
-
後述のモータドライバのGND/VCC端子につなぐための端子です。
-
圧着工具があれば端子の部品だけを買って自作した方が安いです。
-
割り切って再利用性を無視できるなら、直接ハンダづけしちゃっても良いと思います。
まあ、この辺はどっちの組み合わせでも使えるので、好みで使い分ければ良いでしょう。
- 上の写真は「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素晴らしい。