記事一覧

M5AtomのメカナムホイールローバーにM5UnitVを載せてみた

M5AtomのメカナムホイールローバーにM5UnitVを載せてみた



こんにちはRoboTakaoです。
前回、M5StickCで作ったメカナムホイール をM5Atomに換装しましたが
今回はM5UnitVを載せました。


NX14_04.jpg

M5UnitVについては以前開発環境の構築を行なっているのでそちらを参考にしてください。

参考にしたのは公式サイトにあるサンプルです。
自分のメカナムホイールローバーを動かすために改造しています。


今回はディープラーニングなどは行なっておらず、色を追跡ます。

ただしM5UnitV用のコードを見ると、いくつかおかしいと思う点があったので修正しています
カメラで撮られた画像は320x240ですが、水平方向のx方向と鉛直方向のy方向が逆になっています

M5UnitVのコードで以下を修正


dx = 120 - target[6] #b[6]はy方向のはず 320/2=160のはず

dx = 160 - target[5]

target_lab_threshold については今回おもちゃの赤いボールを認識させたいので設定し直しました。

まずはMaxiPy IDEのデフォルトで立ち上がるコードを起動
右上に画像が表示されていると思います。


この状態でメニューバーから

ツール -> マシンビジョン -> しきい値エディダ

NX14_05.png

LABのスライドバーを動かして認識したい物だけが認識できるように調整します。

下に数値が出ているので、これを選択してコピーしてコードにペーストします。

これで動かすと赤いボールが認識されました。


NX14_06.png

このコードをM5UnitVに転送します。

ツール -> Save open script to board(boot.py)

NX14_07.png

NX14_08.png

NX14_09.png

結線

NX14_10.png

前回はバッテリーは1.2x3=3.6Vから5Vに昇圧していましたが、どうやらM5UnitVは消費電流(仕様では500mA)が多いらしくDCDCコンバータの容量が足りませんでした。とりあえず1.2x4=4.8Vから5Vに昇圧して少しでも電流が稼げるようにしました。後でモバイルバッテリーに変更しようと思います。

動作確認

スイッチを入れるとLEDがグリーンに転倒してボールを探す間は回転します。
ボールを見つけるとLEDが赤になり、追跡します

なんとなく追跡できるようになりました。
改善の余地はありますね。




M5UnitVのpythonコード


import sensor
import image
import lcd
import time
import utime
from machine import UART
from Maix import GPIO
from fpioa_manager import *

fm.register(34,fm.fpioa.UART1_TX)
fm.register(35,fm.fpioa.UART1_RX)
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.run(1)

while False:
uart_out.write('TEST\n')
utime.sleep_ms(100)

target_lab_threshold = (50, 86, 37, 66, -15, 22) #red ball


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]
for b in blobs:
if b.area() > max_area:
max_area = b.area()
target = b
if uart_out.read(4096):
area = target.area()
dx = 160 - target[5] #b[5]は縦振り用のX座標 b[6]が首振り用のY座標
hexlist = [(dx >> 8) & 0xFF, dx & 0xFF, (area >> 16) & 0xFF, (area >> 8) & 0xFF, area & 0xFF]
uart_out.write(bytes(hexlist))
else:
pass
print(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])

else:
if uart_out.read(4096):
hexlist = [0x80, 0x00, 0x00, 0x00, 0x00]
uart_out.write(bytes(hexlist))
else:
pass


M5Atomのスケッチ


#include "M5Atom.h"

const uint8_t Srv_RF = 22, Srv_RR = 19, Srv_LF = 23, Srv_LR = 33; //GPIO No.
const uint8_t srvCH_RF = 0, srvCH_RR = 1, srvCH_LF = 2, srvCH_LR = 3; //チャンネル
const double PWM_Hz = 50; //PWM周波数
const uint8_t PWM_level = 16; //PWM 16bit(0~65535)

int mRF = 0, mRR = 0, mLF = 0, mLR = 0;

int pulseMIN = 2295; //700μsec 50Hz 16bit
int pulseMAX = 7533; //2300μsec 50Hz 16bit

int cont_max = 100;
int cont_thres = 40;
int cont_thres2 = 10;

HardwareSerial VSerial(1);

typedef struct
{
int16_t dx;
uint32_t area;
}v_response_t;

void motor_drive(int motor_RF, int motor_RR, int motor_LF, int motor_LR){
motor_RF = map(-motor_RF, -cont_max, cont_max, pulseMIN, pulseMAX);
motor_RR = map(-motor_RR, -cont_max, cont_max, pulseMIN, pulseMAX);
motor_LF = map(motor_LF, -cont_max, cont_max, pulseMIN, pulseMAX);
motor_LR = map(motor_LR, -cont_max, cont_max, pulseMIN, pulseMAX);

ledcWrite(srvCH_RF, motor_RF);
ledcWrite(srvCH_RR, motor_RR);
ledcWrite(srvCH_LF, motor_LF);
ledcWrite(srvCH_LR, motor_LR);
}

void led_red(){
M5.dis.drawpix(0, 0x000000); //なぜか最初のLEDが付かないためダミー
for(int i=0; i<25; i++){
M5.dis.drawpix(i, 0x00ff00); //red
}
}

void led_green(){
M5.dis.drawpix(0, 0x000000); //なぜか最初のLEDが付かないためダミー
for(int i=0; i<25; i++){
M5.dis.drawpix(i, 0xff0000); //green
}
}

void setup() {
Serial.begin(151200);
M5.begin(true, false, true); // void M5Atom::begin(bool SerialEnable , bool I2CEnable , bool DisplayEnable )

VSerial.begin(115200, SERIAL_8N1, 32, 26);

pinMode(Srv_RF, OUTPUT);
pinMode(Srv_RR, OUTPUT);
pinMode(Srv_LF, OUTPUT);
pinMode(Srv_LR, OUTPUT);

led_green();

//モータのPWMのチャンネル、周波数の設定
ledcSetup(srvCH_RF, PWM_Hz, PWM_level);
ledcSetup(srvCH_RR, PWM_Hz, PWM_level);
ledcSetup(srvCH_LF, PWM_Hz, PWM_level);
ledcSetup(srvCH_LR, PWM_Hz, PWM_level);

//モータのピンとチャンネルの設定
ledcAttachPin(Srv_RF, srvCH_RF);
ledcAttachPin(Srv_RR, srvCH_RR);
ledcAttachPin(Srv_LF, srvCH_LF);
ledcAttachPin(Srv_LR, srvCH_LR);
}

enum
{
kNoTarget = 0,
kLeft,
kRight,
kStraight,
kTooClose
};

const uint16_t kThreshold = 20; // 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);
v_data.dx = (buffer[0] << 8) | buffer[1];
v_data.area = (buffer[2] << 16) | (buffer[3] << 8) | buffer[4];

if(v_data.dx > -160 && v_data.dx < 160)
{
if(v_data.area > 30000)
{
state = kTooClose; // Stop
}
else if(v_data.dx > -kThreshold && v_data.dx < kThreshold)
{
state = kStraight; // Go straight
}
else if(v_data.dx <= -kThreshold)
{
state = kLeft; // Go left
}
else if(v_data.dx >= kThreshold)
{
state = kRight; // Go right
}
else
{
state = kNoTarget; // Rotate
}
led_red();
}
else
{
state = kNoTarget; // Rotate
led_green();
}

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:
motor_drive(15, 15, -15, -15);
delay(10);
motor_drive(0, 0, 0, 0);
break;

case kLeft:
motor_drive(20, -20, -20, 20);
delay(10);
motor_drive(0, 0, 0, 0);
break;

case kRight:
motor_drive(-20, 20, 20, -20);
delay(10);
motor_drive(0, 0, 0, 0);
break;

case kStraight:
motor_drive(20, 20, 20, 20);
delay(10);
motor_drive(0, 0, 0, 0);
break;

case kTooClose:
motor_drive(0, 0, 0, 0);
break;

}
}

スポンサードリンク

コメント

コメントの投稿

非公開コメント

プロフィール

RoboTakao

Author:RoboTakao
みなさんご訪問ありがとうございます。ロボット作りたいけどお小遣いそんなにないし、簡単でローコストでロボットを作るための私のプロジェクトを紹介します。

ウェブサイトもありますのでそちらもよろしくお願いします。
http://robotakao.jp/

スポンサーリンク