RoverCとM5StickCとM5StickVでグリッパロボットを作る

RoverCはM5StickCを組み合わせて制御するメカナムホイールロボットです
これにM5StickVの画像認識を組み合わせて自律制御させます

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">ちょっとズルしてるけど<br>画像認識でPID制御→前進して掴む→旋回してQR(ゴール)を探す→ゴールに球を置く<br>と言う一連のシーケンスできました<br><br>これ以上は発展が思いつかないので一旦を開発止めようかな(´・ω・`)<a href="https://twitter.com/hashtag/M5StickC?src=hash&amp;ref_src=twsrc%5Etfw">#M5StickC</a> <a href="https://twitter.com/hashtag/M5StickV?src=hash&amp;ref_src=twsrc%5Etfw">#M5StickV</a> <a href="https://twitter.com/hashtag/M5Stack?src=hash&amp;ref_src=twsrc%5Etfw">#M5Stack</a> <a href="https://t.co/1RliWxQjgA">pic.twitter.com/1RliWxQjgA</a></p>&mdash; もけ@ムギ㌠ (@coppercele) <a href="https://twitter.com/coppercele/status/1259900268931514370?ref_src=twsrc%5Etfw">May 11, 2020</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

##使用するもの##
**RoverC**
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/298776/5c974bf2-82a2-160a-3338-2d00b78cec72.png)
RoverC(W/O M5StickC) – m5stack-store
https://m5stack.com/products/rovercw-o-m5stickc

M5StickCを背負って動かすメカナムホイールロボット
スイッチサイエンスでも取り扱っていたが現在は売り切れ
M5Stack公式でも16 in stockなので危うい

**M5StickC**
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/298776/cd2f180f-a233-a51d-ee30-19dc459e1dd3.png)

M5StickC - スイッチサイエンス
https://www.switch-science.com/catalog/5517/
Arduino互換なESP32がケースに入っててボタンとディスプレイとバッテリーがついててWifiとBluetoothも使える欲張りセット

**M5SticV**
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/298776/0d545dea-d735-95d7-7685-c629d1f27a63.png)

M5StickV - スイッチサイエンス
https://www.switch-science.com/catalog/5700/

Kendryte K210を搭載したAIカメラです。高性能なニューラルネットワークプロセッサ（KPU）とデュアルコア 64 bit RISC-V CPUを使用しており、低コストかつ高いエネルギー効率で高性能な画像処理を行うことができます。(スイッチサイエンスからコピペ)

この使い方だとUnitVでいいと思うけど現在在庫切れ

UnitV AI Camera - スイッチサイエンス
https://www.switch-science.com/catalog/6212/

**その他**
サーボSG90

3Dプリンタ
3万円くらいで買ったDa vinchi jr

##組み立てる##

RoverCにM5StickCを差し込んでスケッチを書き込みます

RoverCのドキュメントはJoyCで操作することが前提となってるので
ザックリと書き換えます
自分で適当に作った物なんでもっといい方法があったら教えてください

自分流メカナムホイール(RoverC)制御法 - Qiita
https://qiita.com/coppercele/items/1596b7b9904eb4403191

あと自分はコントローラーで操作できるようにしてありますが
結構面倒なので割愛します
興味があったらこちらをどうぞ

M5StackにbtstackでBluetooth3.0コントローラを接続する(ESP-IDFでのM5Stack開発環境整備) - Qiita
https://qiita.com/coppercele/items/795352a6cf04ce4f6fcd

##M5StickCとM5StickVを接続する##

M5StickCとM5StickVをGroveケーブルで接続します
UARTを使って通信します

こちらを参考にしました
Wi-FiがないM5StickVを、M5StickCと繋ぎLINEに投稿してみるまでの手順 - Qiita
https://qiita.com/nnn112358/items/5efd926fea20cd6c2c43


M5SticV側ソース(送信のみ)

```python
from fpioa_manager import fm
from machine import UART
fm.register(35, fm.fpioa.UART2_TX, force=True)
fm.register(34, fm.fpioa.UART2_RX, force=True)
uart_Port = UART(UART.UART2, 115200,8,0,0, timeout=1000, read_buf_len= 4096)

data_packet = bytearray([0, 0])
uart_Port.write(data_packet)

```

M5StickC側ソース(受信側)

```cpp

void setup() {
  Serial1.begin(115200, SERIAL_8N1, 32, 33);
}
void loop() {
  if (Serial1.available()) {
    uint8_t rx_buffer[2];
    int rx_size = Serial1.readBytes(rx_buffer, 2);
    for (int i = 0; i < rx_size; i++) {
      Serial.printf("%d, ", rx_buffer[i]);
    }
  }
}

```

##M5SticVのカメラで画像認識して結果をM5StickCに送信する##

【キョロキョロV④】m5StickV サーボ2軸マシンでまずは色検出をしてみました - パスコンパスの日記
https://yoichi-41.hatenablog.com/entry/2019/09/15/204835

こちらを参考にオレンジを検出する閾値を設定します

検出結果は7要素の配列で返ってくるので必要な情報をM5StickCに送信します
image — machine vision — MicroPython 1.12 documentation
https://docs.openmv.io/library/omv.image.html#image.image.blob

blobs[検出矩形の左上x座標,左上y座標,幅,高さ,面積,中心x座標,中心y座標]

M5StickVのカメラ画像は320x240,M5StickCのディスプレイは160x80なのでそれぞれ割って合わせてます

```python
lcd.init()
lcd.rotation(2)
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.run(1)

orange_threshold   = (100, 68, 107, -103, 127, 26)

while True:
  img=sensor.snapshot()
  blobs = img.find_blobs([red_threshold])
  if blobs:
    for b in blobs:
      if (b[2] * b[3]) > 1000:
        tmp=img.draw_rectangle(b[0:4],color=(128,0,0))
        tmp=img.draw_cross(b[5], b[6],color=(128,0,0))
        c=img.get_pixel(b[5], b[6])
        img.draw_string(60, 100, "red",color=(128,0,0), scale=2)
        print(b[0:7])
        data_packet = bytearray([int(b[0] / 2), int(b[1] / 3), int(b[2] / 2), int(b[3] / 3) , int(b[5] / 2), 0])
        uart_Port.write(data_packet)
    lcd.display(img)

```
自分のM5StickVはディスプレイ不良だったのでM5stickCに送信するついでに矩形の位置を表示するため矩形の情報も送信しています
ディスプレイが正常なら中心の座標だけでいいと思います

<img width="768" alt="image.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/298776/7afa5fff-ebe3-52e8-8d24-b0ff442a0b49.png">

##M5StickVをRoverCに設置する##
自分はたまたま3Dプリンタを持ってたので自分でモデリングして作りましたが
無い場合はうまいこと固定してくださいｗ

データはこちらにありますので必要な方はご利用ください
Da vinchi jrの精度で作ってあるので調整が必要かもしれません

M5StickV holder for RoverC(Mecanum mobile robot base compatible with M5stickC) by coppercele - Thingiverse
https://www.thingiverse.com/thing:4288799

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr"><a href="https://twitter.com/hashtag/RoverC?src=hash&amp;ref_src=twsrc%5Etfw">#RoverC</a> 用の <a href="https://twitter.com/hashtag/M5StickV?src=hash&amp;ref_src=twsrc%5Etfw">#M5StickV</a> ホルダーできたよー＼(^o^)／<br><br>角度を-5度から90度までUSBケーブルの干渉を避けながら調整可能！(そこまでやる必要無かったよね？)<br>あと縦バージョンも<br><br>これデータアップする需要あるのかな？🤔<a href="https://twitter.com/hashtag/M5StickC?src=hash&amp;ref_src=twsrc%5Etfw">#M5StickC</a> <a href="https://t.co/OLmfOvI7tG">pic.twitter.com/OLmfOvI7tG</a></p>&mdash; もけ@ムギ㌠ (@coppercele) <a href="https://twitter.com/coppercele/status/1250395284514865158?ref_src=twsrc%5Etfw">April 15, 2020</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

##RoverCで対象物を追跡する##

RoverCはトルク重視のセッティングでメカナムホイールも路面をこすって動くので繊細な動きは苦手です
出力を上げてもなかなか動かないけど動き始めると一気に動き始めます
なので単純に動かそうとしてもハンチングを起こします
<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">これPID制御した方がいいかな？ｗ<br><br>予想以上にRoverCのレイテンシが大きいな・・・🤔<a href="https://twitter.com/hashtag/M5StickC?src=hash&amp;ref_src=twsrc%5Etfw">#M5StickC</a> <a href="https://twitter.com/hashtag/M5StickV?src=hash&amp;ref_src=twsrc%5Etfw">#M5StickV</a> <a href="https://t.co/2QeWS40y74">pic.twitter.com/2QeWS40y74</a></p>&mdash; もけ@ムギ㌠ (@coppercele) <a href="https://twitter.com/coppercele/status/1246855128348422146?ref_src=twsrc%5Etfw">April 5, 2020</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

というわけでこんなこともあろうかと(ウソ)研究してあったPID制御を組み込みます

vrr,vrlを上げるとそれぞれ右回転、左回転します

M5StickCはMadgwickフィルタで倒立振子の夢を見るか？ - Qiita
https://qiita.com/coppercele/items/e4d71537a386966338d0

```cpp

float Kp = 0.7;
float Ki = 1.2;
float Kd = 0.0;
float target = 80;
float P, I, D, preP;

float power = 0;
float dt, preTime;

// loop()内
    if (Serial1.available()) {
        uint8_t rx_buffer[6];
        int rx_size = Serial1.readBytes(rx_buffer, 6);

        for (int i = 0; i < rx_size; i++) {
            Serial.printf("%d, ", rx_buffer[i]);
        }
        Serial.printf("\n");
        // M5StickCのディスプレイに矩形を描画
        M5.Lcd.drawRect(rx_buffer[0], rx_buffer[1] + 10, rx_buffer[2], rx_buffer[3], WHITE);

        dt = (micros() - preTime) / 1000000; // 処理時間を求める
        preTime = micros(); // 処理時間を記録

        // targetは80(M5StickCのディスプレイ幅/2)
        // rx_buffer[4]は検出矩形の中心x座標
        P = (target - rx_buffer[4]);
        if (-5 < P && P < 5) {
            // 中心から左右5ピクセル以内に収まっていれば終了
            vrl = 0;
            vrr = 0;
            I = 0;
            if (phase == PHASE_CENTERING) {
                phase = PHASE_FORWARD;
                // 直進させる
                f = 0.4;
            }
            digitalWrite(GPIO_NUM_10, HIGH);
        }
        else {
             // 偏差を積分する
            I += P * dt;
            // 偏差を微分する
            D = (P - preP) / dt;
            // 偏差を記録する
            preP = P;
            // 積分部分が大きくなりすぎると出力が飽和するので大きくなり過ぎたら0に戻す(アンチワインドアップ)
            // 画面端(P成分が大きい)の時はI制御を切ってP制御だけで大まかに動かす
            if (40 < abs(I * Ki) || 35 < abs(Kp * P)) {
                I = 0;
                 // anti windup
            }
            if (0 < I && P < 5) {
　　　　　　　　　// TODOここいらなくない？
                I = 0;
            }
            else if (I < 0 && -5 < P) {
　　　　　　　　　// TODOここ間違ってない？(意図が思い出せない)
                I = 0;
            }
             // 出力を計算する
            power = Kp * P + Ki * I + Kd * D;
            power = constrain(power, -60, 60);
            Serial.printf("power=%5.1f,%4.1f,%4.1f,%4.1f\n", power, Kp * P, Ki * I, Kd * D);
            if (0 < power) {
                vrl = power / 100.0;
                vrr = 0;
            }
            else {
                vrr = power / -100.0;
                vrl = 0;
            }
        }
    }
```
いろいろと調整した結果カメラの端の方の場合はP制御で大まかに動かして
中心近くになったらほぼI制御のみでジワリと動かすセッティングになっています

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">少しレスポンス悪いけどこんな感じですかね・・・(´・ω・`)<a href="https://twitter.com/hashtag/M5StickC?src=hash&amp;ref_src=twsrc%5Etfw">#M5StickC</a> <a href="https://twitter.com/hashtag/M5StickV?src=hash&amp;ref_src=twsrc%5Etfw">#M5StickV</a> <a href="https://t.co/vYB7ShzAnz">pic.twitter.com/vYB7ShzAnz</a></p>&mdash; もけ@ムギ㌠ (@coppercele) <a href="https://twitter.com/coppercele/status/1247953871302844416?ref_src=twsrc%5Etfw">April 8, 2020</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

##グリッパを作る##
対象物追跡ができたあとなにしたら面白いかなって考えてグリッパを作ろうってことにしました
thingiverseを探したところちょうどよさそうなものがあったので
Fusion360に読み込んでゴリゴリ改変します
アームにはレゴ互換穴をあけてあったのでレゴテクニックのConnector pinを使って接続します

Parametric Servo Gripper Not-Lego Brick Set by MechEngineerMike - Thingiverse
https://www.thingiverse.com/thing:2417814

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">これが・・・<br><br>こうなって・・・<br><br>こうじゃ！ <a href="https://t.co/6cpdFZ9nan">pic.twitter.com/6cpdFZ9nan</a></p>&mdash; もけ@ムギ㌠ (@coppercele) <a href="https://twitter.com/coppercele/status/1254064496991039488?ref_src=twsrc%5Etfw">April 25, 2020</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

##グリッパを動かす##
グリッパを動かすにはサーボが必要なんですがPWMを流すGPIOがありません
RoverCの上部にはGroveポートが2つついてるんですがM5StickCの0.26にパラでつながってるのでI2C対応のサーボじゃないと動きません

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/298776/3d50ae5f-6e30-f274-2a02-b217a3488d5f.png)

普通だとPWMを出そうとしても0,26はWireがつかんでるのでledcAttachPin(26, 0)した時点でエラーになります

というわけで「サーボ動かすときはRoverC動かさなきゃいいんじゃね？」ということで無理矢理動かすことにしましたｗ

```cpp
    // WireをM5StickCの無使用ピン36に一時的に割り当て
    Wire.begin(36, 36, 10000);
    // 26をPWM出力に割り当て
    ledcSetup(0, 50, 10);
    ledcAttachPin(26, 0);
    // サーボを任意角度に設定
    ledcWrite(0, degree);
    delay(200);
    ledcWrite(0, 0);
    // 26を解放
    ledcDetachPin(26);
    delay(100);
    // Wireを再初期化
    Wire.begin(0, 26, 10000);
    delay(100);
```

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">I2CとPWMサーボの共存できたwwww <a href="https://t.co/i9zuOBvDsX">pic.twitter.com/i9zuOBvDsX</a></p>&mdash; もけ@ムギ㌠ (@coppercele) <a href="https://twitter.com/coppercele/status/1253285503714988033?ref_src=twsrc%5Etfw">April 23, 2020</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

動画では色の画像認識がされなくなったらサーボを動かすようになっています

##QRコードの認識##
ゴールとしてQRコードを認識させます
micro pythonのデフォルトで用意されているのでそちらを使います
検出したら色情報と同じくM5StickCに通信します
色と区別するために最後に1を追加しています(色は0)

最初は顔認識させて球を持ってこさせようとしてたんですが
よく考えたら顔認識させるためにはカメラに映るように這いつくばらないといけないんですよねｗ



```python
    qrs = img.find_qrcodes()
    if qrs:
        for q in qrs:
           print("qr cord")
           tmp=img.draw_rectangle(q[0:4],color=(128,0,0))
           print(q[0:7])
           data_packet = bytearray([int(q[0] / 2), int(q[1] / 3), int(q[2] / 2), int(q[3] / 3) , int(q[5] / 2), 1])
           uart_Port.write(data_packet)
```

##自律制御させる##
画像認識でPID制御→前進して掴む→旋回してQR(ゴール)を探す→ゴールに球を置く
というシーケンスで動かします

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">ちょっとズルしてるけど<br>画像認識でPID制御→前進して掴む→旋回してQR(ゴール)を探す→ゴールに球を置く<br>と言う一連のシーケンスできました<br><br>これ以上は発展が思いつかないので一旦を開発止めようかな(´・ω・`)<a href="https://twitter.com/hashtag/M5StickC?src=hash&amp;ref_src=twsrc%5Etfw">#M5StickC</a> <a href="https://twitter.com/hashtag/M5StickV?src=hash&amp;ref_src=twsrc%5Etfw">#M5StickV</a> <a href="https://twitter.com/hashtag/M5Stack?src=hash&amp;ref_src=twsrc%5Etfw">#M5Stack</a> <a href="https://t.co/1RliWxQjgA">pic.twitter.com/1RliWxQjgA</a></p>&mdash; もけ@ムギ㌠ (@coppercele) <a href="https://twitter.com/coppercele/status/1259900268931514370?ref_src=twsrc%5Etfw">May 11, 2020</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

泥縄でソースを書いているので説明しようとしてもうまくいかなそうなのでソースを貼り付けておきます
皆さんはもっときれいなソースを書いてくださいね(´・ω・`)


<details><summary>main.ino</summary>
<div>

```cpp
#include <M5StickC.h>
#include <vector>
#include <string>

using namespace std;

int8_t speed_sendbuff[4] = { 0 };

int FORWARD[4] = { 50, 50, 50, 50 };
int LEFT[4] = { -50, 50, 50, -50 };
int BACKWARD[4] = { -50, -50, -50, -50 };
int RIGHT[4] = { 50, -50, -50, 50 };
int ROTATE_L[4] = { -30, 30, -30, 30 };
int ROTATE_R[4] = { 30, -30, 30, -30 };

float f, b, l, r, rr, rl, vrr, vrl = 0.0;
float limit = 1.0;

int phase = 0;

const int PHASE_DEFAULT = 0;
const int PHASE_CENTERING = 1;
const int PHASE_FORWARD = 2;
const int PHASE_SEARCH = 3;
const int PHASE_QR_FORWARD = 4;

void SetChargingCurrent(uint8_t CurrentLevel) {
    Wire1.beginTransmission(0x34);
    Wire1.write(0x33);
    Wire1.write(0xC0 | (CurrentLevel & 0x0f));
    Wire1.endTransmission();
}

int8_t I2CWrite1Byte(uint8_t Addr, uint8_t Data) {
    Wire.beginTransmission(0x38);
    Wire.write(Addr);
    Wire.write(Data);
    return Wire.endTransmission();
}

uint8_t I2CWritebuff(uint8_t Addr, uint8_t *Data, uint16_t Length) {
    Wire.beginTransmission(0x38);
    Wire.write(Addr);
    for (int i = 0; i < Length; i++) {
        Wire.write(Data[i]);
    }
    return Wire.endTransmission();
}

uint8_t setspeed() {
//    Serial.printf("power =%d, %d, %d, %d\n", speed_sendbuff[0],speed_sendbuff[1],speed_sendbuff[2],speed_sendbuff[3]);

    return I2CWritebuff(0x00, (uint8_t*) speed_sendbuff, 4);
}

int non = 0;

float Kp = 0.7;
float Ki = 1.2;
float Kd = 0.0;
float target = 80;
float P, I, D, preP;

float power = 0;
float dt, preTime;

int servoMin = 100;  // (26/1024)*20ms ≒ 0.5 ms  (-90°)
int servoMax = 120; // (123/1024)*20ms ≒ 2.4 ms (+90°)

int now = 120;

bool servoMoving = false;

void servoOpen() {
    Wire.begin(36, 36, 10000);
    ledcSetup(0, 50, 10);
    ledcAttachPin(26, 0);
    now -= 20;
    if (now <= servoMin)
        now = servoMin;

    ledcWrite(0, now);
    delay(200);
    ledcWrite(0, 0);
    ledcDetachPin(26);
    delay(100);
    Wire.begin(0, 26, 10000);
}

void servoClose() {
    Wire.begin(36, 36, 10000);
    ledcSetup(0, 50, 10);
    ledcAttachPin(26, 0);
    now += 20;
    if (servoMax < now)
        now = servoMax;

    ledcWrite(0, now);
    delay(200);
    ledcWrite(0, 0);
    ledcDetachPin(26);
    delay(100);
    Wire.begin(0, 26, 10000);
    delay(100);
}

void setup() {

    // Initialize the M5Stack object

    M5.begin();
    M5.update();
    Wire.begin(0, 26, 10000);
    SetChargingCurrent(4);
    M5.Lcd.setRotation(1);
    M5.Axp.ScreenBreath(9);
    Serial1.begin(115200, SERIAL_8N1, 32, 33);

    pinMode(GPIO_NUM_10, OUTPUT);
    digitalWrite(GPIO_NUM_10, HIGH);

    M5.Lcd.setTextColor(GREEN, BLACK);
    M5.Lcd.setTextSize(1);
    M5.Lcd.setCursor(0, 0);
    M5.Lcd.printf("M5 BT test");
    M5.Lcd.setTextColor(WHITE, BLACK);
//    xTaskCreatePinnedToCore(readUart, "readUart", 8192, NULL, 1, NULL, 0);
    preTime = micros();
}

bool qrFound = false;

void loop() {
    M5.update();
    if (M5.BtnB.wasPressed()) {
        esp_restart();
    }

    delay(10);

    if (servoMoving) {
        return;
    }

    if (Serial1.available()) {

        non = 0;
        uint8_t rx_buffer[6];
        int rx_size = Serial1.readBytes(rx_buffer, 6);

        for (int i = 0; i < rx_size; i++) {
            Serial.printf("%d, ", rx_buffer[i]);
        }
        Serial.printf("\n");
        M5.Lcd.fillRect(0, 10, 160, 70, BLACK);
        M5.Lcd.drawRect(rx_buffer[0], rx_buffer[1] + 10, rx_buffer[2],
                rx_buffer[3], WHITE);

        //    Serial.printf("[4]=%d\n",rx_buffer[4]);

        dt = (micros() - preTime) / 1000000; // 処理時間を求める
        preTime = micros(); // 処理時間を記録
        if (phase == PHASE_CENTERING) {

            P = (target - rx_buffer[4]);
            if (-5 < P && P < 5) {
                vrl = 0;
                vrr = 0;
                I = 0;
                if (phase == PHASE_CENTERING) {
                    phase = PHASE_FORWARD;
                    f = 0.4;
                }
                digitalWrite(GPIO_NUM_10, HIGH);
            }
            else {

                // 偏差を積分する
                I += P * dt;
                // 偏差を微分する
                D = (P - preP) / dt;
                // 偏差を記録する
                preP = P;
                // 積分部分が大きくなりすぎると出力が飽和するので大きくなり過ぎたら0に戻す(アンチワインドアップ)
                if (40 < abs(I * Ki) || 35 < abs(Kp * P)) {
                    I = 0;

                    // anti windup
                }
                if (0 < I && P < 5) {
                    I = 0;
                }
                else if (I < 0 && -5 < P) {
                    I = 0;
                }

                // 出力を計算する
                power = Kp * P + Ki * I + Kd * D;
                power = constrain(power, -60, 60);
                Serial.printf("power=%5.1f,%4.1f,%4.1f,%4.1f\n", power, Kp * P,
                        Ki * I, Kd * D);
                if (0 < power) {
                    vrl = power / 100.0;
                    vrr = 0;
                }
                else {
                    vrr = power / -100.0;
                    vrl = 0;
                }
            }

        }
        else if (phase == PHASE_SEARCH) {
            if (rx_buffer[5] == 1) {
                Serial.printf("QR found\n");
                phase = PHASE_QR_FORWARD;
                rl = 0.0;
                f = 0.4;
            }
        }
    }
    else {
        non++;
        if (30 < non) {
            Serial.printf("no UART %d\n", non);
            vrl = 0.0;
            vrr = 0.0;
            non = 0;
            I = 0;
            if (phase == PHASE_FORWARD) {
                phase = PHASE_SEARCH;
                f = 0.0;
                servoClose();
            }
            else if (phase == PHASE_SEARCH) {
                if (rl == 0.0) {
                    Serial.printf("rotate\n");
                    rl = 0.5;
                }
                else {
                    Serial.printf("rotate stop\n");
                    rl = 0.0;
                }
            }
            if (phase == PHASE_QR_FORWARD) {
                Serial.printf("QR not found\n");
                phase = PHASE_DEFAULT;
                f = 0.0;
                servoOpen();
            }

        }
    }

    if (f == 0 && b == 0 && r == 0 && l == 0 && rl == 0 && rr == 0 && vrr == 0
            && vrl == 0) {
//		Serial.printf("%f,%f,%f,%f,%f,%f\n",f,b,r,l,rr,rl);
        speed_sendbuff[0] = 0;
        speed_sendbuff[1] = 0;
        speed_sendbuff[2] = 0;
        speed_sendbuff[3] = 0;
        setspeed();
    }
    else {
//		Serial.printf("%f,%f,%f,%f,%f,%f\n",f,b,r,l,rr,rl);
        for (int i = 0; i < 4; i++) {
            speed_sendbuff[i] = FORWARD[i] * f;
            speed_sendbuff[i] += BACKWARD[i] * b;
            speed_sendbuff[i] += RIGHT[i] * r;
            speed_sendbuff[i] += LEFT[i] * l;
            speed_sendbuff[i] += ROTATE_L[i] * constrain(rl + vrl, -1.0, 1.0);
            speed_sendbuff[i] += ROTATE_R[i] * constrain(rr + vrr, -1.0, 1.0);
        }

        for (int i = 0; i < 4; i++) {
            limit = 80.0
                    / max(abs(speed_sendbuff[3]),
                            max(abs(speed_sendbuff[2]),
                                    max(abs(speed_sendbuff[1]),
                                            abs(speed_sendbuff[0]))));
        }
//    printf("limit = %f\n", limit);
        if (1.0 < limit) {
            limit = 1.0;
        }
        for (int i = 0; i < 4; i++) {
            speed_sendbuff[i] = speed_sendbuff[i] * limit;
        }

        setspeed();
    }

    if (isSelect) {
    }
    else {
        if (addresses.size() != 0) {
            printDevice(indexDevice);
        }
        if (M5.BtnA.wasPressed()) {
            printf("BtnA\n");
//            if (indexDevice != addresses.size() - 1) {
            printf("try to connect\n");
            connect((char*) addresses[indexDevice].c_str());
            M5.Lcd.fillRect(0, 16, 320, 240, BLACK);
            isSelect = true;
            M5.Lcd.fillRect(0, 16, 320, 240, BLACK);

        }
    }
}

int i = 0;

// 文字列を表示
extern "C" void m5print(const char *str) {
    if (240 < i * 16) {
        i = 1;
        M5.Lcd.fillRect(0, 16, 320, 240, BLACK);
    }
    M5.Lcd.setTextColor(WHITE, BLACK);
    M5.Lcd.setCursor(0, i * 8);
    M5.Lcd.printf(str);
    i++;
}

// システムメッセージ表示用
extern "C" void m5message(char *str) {
    M5.Lcd.setTextColor(GREEN, BLACK);
    M5.Lcd.setTextSize(1);
    M5.Lcd.setCursor(0, 0);
    M5.Lcd.fillRect(0, 0, 320, 16, BLACK);
    M5.Lcd.printf(str);
    i++;
}



```
</div>
</details>