記事一覧

M5Atom+M5UnitVのメカナムホイールローバーで顔認識してみた

M5Atom+M5UnitVのメカナムホイールローバーで顔認識してみた



こんにちはRoboTakaoです

以前、M5AtomとM5UnitVを載せたメカナムホイールローバーを紹介しましたが
今回は顔認識させようと思います。


NX14_12.jpeg

M5UnitVには工場出荷時に書き込まれている顔認識モデルが書き込まれているのでそれを使います。

動作確認



顔の写真を見つけたらその方向に向かうようにしました。
本当はPID制御でも入れたいところですが、今回は入れていません

LEDが赤い時には顔認識しています。
無事、認識しました。


M5UnitVのMicroPythonコード


import sensor
import image
import lcd
import time
import utime
import KPU as kpu
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)

task = kpu.load(0x300000) #FlashRomの顔モデルを読み出す
anchor = (1.889, 2.5245, 2.9465, 3.94056, 3.99987, 5.3658, 5.155437, 6.92275, 6.718375, 9.01025)
a = kpu.init_yolo2(task, 0.5, 0.3, 5, anchor)

while True:
img=sensor.snapshot()

code = kpu.run_yolo2(task, img)
if code:
max_area = 0
target = code[0]
for i in code:
if i.w()*i.h() > max_area:
max_area = i.w()*i.h()
target = i
if uart_out.read(4096):
area = target.w()*target.h()
tcx = int(target.x()+target.w()/2)
tcy = int(target.y()+target.h()/2)
dx = 140 - tcx #160 - 20 本当は160だがなぜかセンターがずれるで補正
hexlist = [(dx >> 8) & 0xFF, dx & 0xFF, (area >> 16) & 0xFF, (area >> 8) & 0xFF, area & 0xFF]
uart_out.write(bytes(hexlist))
else:
pass
print(target.w()*target.h())
print(dx)
tmp=img.draw_rectangle(target.rect())
tmp=img.draw_cross(tcx, tcy)
c=img.get_pixel(tcx, tcy)

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;

int M_speed = 14;

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,
kLeftTurn,
kRight,
kRightTurn,
kStraight,
kTooClose
};

const uint16_t kThreshold = 30; // If target is in range ±kThreshold, the car will go straight
const uint16_t kThreshold2 = 70;
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 > 17000)
{
state = kTooClose; // Stop
}
else if(v_data.dx > -kThreshold && v_data.dx < kThreshold)
{
state = kStraight; // Go straight
}
else if(v_data.dx <= -kThreshold && v_data.dx > -kThreshold2)
{
state = kLeft; // Go left
}
else if(v_data.dx <= -kThreshold2)
{
state = kLeftTurn; // Go left turn
}
else if(v_data.dx >= kThreshold && v_data.dx < kThreshold2)
{
state = kRight; // Go right
}
else if(v_data.dx >= kThreshold2)
{
state = kRightTurn; // Go right turn
}
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(M_speed, M_speed, -M_speed, -M_speed);
delay(100);
motor_drive(0, 0, 0, 0);
break;

case kLeft:
motor_drive(M_speed, -M_speed, -M_speed, M_speed);
delay(100);
motor_drive(0, 0, 0, 0);
break;

case kLeftTurn:
motor_drive(M_speed, M_speed, 0, 0);
delay(500);
motor_drive(0, 0, 0, 0);
break;

case kRight:
motor_drive(-M_speed, M_speed, M_speed, -M_speed);
delay(100);
motor_drive(0, 0, 0, 0);
break;

case kRightTurn:
motor_drive(0, 0, M_speed, M_speed);
delay(500);
motor_drive(0, 0, 0, 0);
break;

case kStraight:
motor_drive(M_speed, M_speed, M_speed, M_speed);
delay(100);
motor_drive(0, 0, 0, 0);
break;

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

}
}

スポンサードリンク

コメント

コメントの投稿

非公開コメント

プロフィール

RoboTakao

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

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

スポンサーリンク