LoginSignup
15

More than 3 years have passed since last update.

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

Last updated at Posted at 2020-05-12

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

使用するもの

RoverC
image.png
RoverC(W/O M5StickC) – m5stack-store
https://m5stack.com/products/rovercw-o-m5stickc

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

M5StickC
image.png

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

M5SticV
image.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側ソース(送信のみ)

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側ソース(受信側)


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なのでそれぞれ割って合わせてます

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に送信するついでに矩形の位置を表示するため矩形の情報も送信しています
ディスプレイが正常なら中心の座標だけでいいと思います

image.png

M5StickVをRoverCに設置する

自分はたまたま3Dプリンタを持ってたので自分でモデリングして作りましたが
無い場合はうまいこと固定してくださいw

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

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

RoverCで対象物を追跡する

RoverCはトルク重視のセッティングでメカナムホイールも路面をこすって動くので繊細な動きは苦手です
出力を上げてもなかなか動かないけど動き始めると一気に動き始めます
なので単純に動かそうとしてもハンチングを起こします

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

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

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


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制御のみでジワリと動かすセッティングになっています

グリッパを作る

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

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

グリッパを動かす

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

image.png

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

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

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

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

QRコードの認識

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

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

    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(ゴール)を探す→ゴールに球を置く
というシーケンスで動かします

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

main.ino
#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++;
}




Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
15