RoverCはM5StickCを組み合わせて制御するメカナムホイールロボットです
これにM5StickVの画像認識を組み合わせて自律制御させます
ちょっとズルしてるけど
— もけ@ムギ㌠ (@coppercele) May 11, 2020
画像認識でPID制御→前進して掴む→旋回してQR(ゴール)を探す→ゴールに球を置く
と言う一連のシーケンスできました
これ以上は発展が思いつかないので一旦を開発止めようかな(´・ω・`)#M5StickC #M5StickV #M5Stack pic.twitter.com/1RliWxQjgA
##使用するもの##
RoverC
RoverC(W/O M5StickC) – m5stack-store
https://m5stack.com/products/rovercw-o-m5stickc
M5StickCを背負って動かすメカナムホイールロボット
スイッチサイエンスでも取り扱っていたが現在は売り切れ
M5Stack公式でも16 in stockなので危うい
M5StickC
M5StickC - スイッチサイエンス
https://www.switch-science.com/catalog/5517/
Arduino互換なESP32がケースに入っててボタンとディスプレイとバッテリーがついててWifiとBluetoothも使える欲張りセット
M5SticV
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に送信するついでに矩形の位置を表示するため矩形の情報も送信しています
ディスプレイが正常なら中心の座標だけでいいと思います
##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
#RoverC 用の #M5StickV ホルダーできたよー＼(^o^)／
— もけ@ムギ㌠ (@coppercele) April 15, 2020
角度を-5度から90度までUSBケーブルの干渉を避けながら調整可能！(そこまでやる必要無かったよね？)
あと縦バージョンも
これデータアップする需要あるのかな？🤔#M5StickC pic.twitter.com/OLmfOvI7tG
##RoverCで対象物を追跡する##
RoverCはトルク重視のセッティングでメカナムホイールも路面をこすって動くので繊細な動きは苦手です
出力を上げてもなかなか動かないけど動き始めると一気に動き始めます
なので単純に動かそうとしてもハンチングを起こします
これPID制御した方がいいかな？ｗ
— もけ@ムギ㌠ (@coppercele) April 5, 2020
予想以上にRoverCのレイテンシが大きいな・・・🤔#M5StickC #M5StickV pic.twitter.com/2QeWS40y74
というわけでこんなこともあろうかと(ウソ)研究してあった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制御のみでジワリと動かすセッティングになっています
少しレスポンス悪いけどこんな感じですかね・・・(´・ω・`)#M5StickC #M5StickV pic.twitter.com/vYB7ShzAnz
— もけ@ムギ㌠ (@coppercele) April 8, 2020
##グリッパを作る##
対象物追跡ができたあとなにしたら面白いかなって考えてグリッパを作ろうってことにしました
thingiverseを探したところちょうどよさそうなものがあったので
Fusion360に読み込んでゴリゴリ改変します
アームにはレゴ互換穴をあけてあったのでレゴテクニックのConnector pinを使って接続します
Parametric Servo Gripper Not-Lego Brick Set by MechEngineerMike - Thingiverse
https://www.thingiverse.com/thing:2417814
これが・・・
— もけ@ムギ㌠ (@coppercele) April 25, 2020
こうなって・・・
こうじゃ！ pic.twitter.com/6cpdFZ9nan
##グリッパを動かす##
グリッパを動かすにはサーボが必要なんですがPWMを流すGPIOがありません
RoverCの上部にはGroveポートが2つついてるんですがM5StickCの0.26にパラでつながってるのでI2C対応のサーボじゃないと動きません
普通だとPWMを出そうとしても0,26はWireがつかんでるのでledcAttachPin(26, 0)した時点でエラーになります
というわけで「サーボ動かすときはRoverC動かさなきゃいいんじゃね？」ということで無理矢理動かすことにしましたｗ
// 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);
I2CとPWMサーボの共存できたwwww pic.twitter.com/i9zuOBvDsX
— もけ@ムギ㌠ (@coppercele) April 23, 2020
動画では色の画像認識がされなくなったらサーボを動かすようになっています
##QRコードの認識##
ゴールとしてQRコードを認識させます
micro pythonのデフォルトで用意されているのでそちらを使います
検出したら色情報と同じくM5StickCに通信します
色と区別するために最後に1を追加しています(色は0)
最初は顔認識させて球を持ってこさせようとしてたんですが
よく考えたら顔認識させるためにはカメラに映るように這いつくばらないといけないんですよねｗ
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(ゴール)を探す→ゴールに球を置く
というシーケンスで動かします
ちょっとズルしてるけど
— もけ@ムギ㌠ (@coppercele) May 11, 2020
画像認識でPID制御→前進して掴む→旋回してQR(ゴール)を探す→ゴールに球を置く
と言う一連のシーケンスできました
これ以上は発展が思いつかないので一旦を開発止めようかな(´・ω・`)#M5StickC #M5StickV #M5Stack pic.twitter.com/1RliWxQjgA
泥縄でソースを書いているので説明しようとしてもうまくいかなそうなのでソースを貼り付けておきます
皆さんはもっときれいなソースを書いてくださいね(´・ω・`)
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++;
}