記事一覧

M5Atom+M5StickC+M5UnitVでWiFiカメラ付きメカナムホイール ローバー

M5Atom+M5StickC+M5UnitVでWiFiカメラ付きメカナムホイール ローバー



こんにちはRoboTakaoです

以前から製作していたM5Atom のメカナムホイール ですが、M5UnitVのカメラを使って
WiFiカメラ付きのメカナムホイールにしたいと考えていました。

いろいろ試してみたのですがM5Atomだけではストリーミングと操縦の両立ができなく
結局M5StickCでストリーミングしました。
そもそもM5UnitVはAIカメラなので適した使い方ではなくて無理があるのだと思います。

ということで下記の構成でWiFiカメラ付きのメカナムホイール ローバーを作りました。

NX14_17.jpg

NX14_14.jpeg


構成
4つの360度回転サーボ
モバイルバッテリー
M5Atom BlynkでiPhoneからサーボを制御
M5UnitV 今回は単にカメラ機能UARTでM5StickCに画像送信
M5StickC UARTで受けたを画像をWiFiでストリーミング

M5Atomは1台目のiPhoneのBlynkでコントロール
M5StickCからのストリーミングは2台目のiPhoneのブラウザで表示

結線

NX14_16.png

まずは動作
ちょっと不安定ですがiPhoneのブラウザにカメラのストリーミング画像を出しながら
Blynkで操作できました



スケッチ

M5StickC+M5UnitVでのWiFiストリーミングはHomeMadeGarbageさんのブログを参考(というかほぼそのまま)にしました。

M5StickCのスケッチ


#include <WiFi.h>
#include <WiFiClient.h>
#include <WebServer.h>
#include <ESPmDNS.h>
#include <M5StickC.h>

const char* ssid = "XXXXXXXXXXXXXX";
const char* password = "XXXXXXXXXXXXXXX";

WebServer server(80);

typedef struct {
uint32_t length;
uint8_t *buf;
} jpeg_data_t;

jpeg_data_t jpeg_data;
static const int RX_BUF_SIZE = 100000;

static const uint8_t packet_begin[3] = { 0xFF, 0xD8, 0xEA };

void sendPic() {
int state = 1;
WiFiClient client = server.client();

while(state){
if (Serial2.available()) {
digitalWrite(10, LOW);
uint8_t rx_buffer[10];
int rx_size = Serial2.readBytes(rx_buffer, 10);
if (rx_size == 10) {
if ((rx_buffer[0] == packet_begin[0]) && (rx_buffer[1] == packet_begin[1]) && (rx_buffer[2] == packet_begin[2])) {
jpeg_data.length = (uint32_t)(rx_buffer[4] << 16) | (rx_buffer[5] << 8) | rx_buffer[6];
Serial.println(jpeg_data.length);

int rx_size = Serial2.readBytes(jpeg_data.buf, jpeg_data.length);

client.write(jpeg_data.buf, jpeg_data.length);
state = 0;
}
}
}
}
}

void streamPic() {
int state = 1;

while(state){
if (Serial2.available()) {
//digitalWrite(10, LOW);
uint8_t rx_buffer[10];
int rx_size = Serial2.readBytes(rx_buffer, 10);
if (rx_size == 10) {
if ((rx_buffer[0] == packet_begin[0]) && (rx_buffer[1] == packet_begin[1]) && (rx_buffer[2] == packet_begin[2])) {
jpeg_data.length = (uint32_t)(rx_buffer[4] << 16) | (rx_buffer[5] << 8) | rx_buffer[6];
Serial.println(jpeg_data.length);

int rx_size = Serial2.readBytes(jpeg_data.buf, jpeg_data.length);
state = 0;
}
}
}
}

state = 1;

while(state){
if (Serial2.available()) {
//digitalWrite(10, LOW);
uint8_t rx_buffer[10];
int rx_size = Serial2.readBytes(rx_buffer, 10);
if (rx_size == 10) {
if ((rx_buffer[0] == packet_begin[0]) && (rx_buffer[1] == packet_begin[1]) && (rx_buffer[2] == packet_begin[2])) {
jpeg_data.length = (uint32_t)(rx_buffer[4] << 16) | (rx_buffer[5] << 8) | rx_buffer[6];
Serial.println(jpeg_data.length);

int rx_size = Serial2.readBytes(jpeg_data.buf, jpeg_data.length);
state = 0;
}
}
}
}


WiFiClient client = server.client();

String response = "HTTP/1.1 200 OK\r\n";
response += "Content-Type: multipart/x-mixed-replace; boundary=frame\r\n\r\n";
server.sendContent(response);

while (1) {
//digitalWrite(10, HIGH);

if (Serial2.available()) {
//digitalWrite(10, LOW);
uint8_t rx_buffer[10];
int rx_size = Serial2.readBytes(rx_buffer, 10);
if (rx_size == 10) {
if ((rx_buffer[0] == packet_begin[0]) && (rx_buffer[1] == packet_begin[1]) && (rx_buffer[2] == packet_begin[2])) {
jpeg_data.length = (uint32_t)(rx_buffer[4] << 16) | (rx_buffer[5] << 8) | rx_buffer[6];
Serial.println(jpeg_data.length);

int rx_size = Serial2.readBytes(jpeg_data.buf, jpeg_data.length);
}
response = "--frame\r\n";
response += "Content-Type: image/jpeg\r\n\r\n";
server.sendContent(response);

client.write(jpeg_data.buf, jpeg_data.length);
server.sendContent("\r\n");

if (!client.connected())
break;
}else{
int rx_size = Serial2.readBytes(rx_buffer, 1);
break;
}
}

if (!client.connected())
break;
}
}

void setup() {
pinMode(10, OUTPUT);
digitalWrite(10, LOW);

M5.begin();
M5.Axp.ScreenBreath(0);

Serial.begin(115200);
Serial2.begin(1152000, SERIAL_8N1, 32, 33);

jpeg_data.buf = (uint8_t *) malloc(sizeof(uint8_t) * RX_BUF_SIZE);

WiFi.mode(WIFI_STA);
WiFi.begin(ssid, password);
Serial.println("");

// Wait for connection
while (WiFi.status() != WL_CONNECTED) {
delay(500);
Serial.print(".");
}
Serial.println("");
Serial.print("Connected to ");
Serial.println(ssid);
Serial.print("IP address: ");
Serial.println(WiFi.localIP());

if (MDNS.begin("esp32")) {
Serial.println("MDNS responder started");
}

server.on("/", []() {
server.send(200, "text/plain", "this works as well");
});

server.on("/pic", HTTP_GET, sendPic);
server.on("/stream", HTTP_GET, streamPic);

server.begin();
Serial.println("HTTP server started");
}

void loop() {
server.handleClient();
}


M5Atomのスケッチ


#define BLYNK_PRINT Serial

#include "M5Atom.h"
#include <WiFi.h>
#include <WiFiClient.h>
#include <BlynkSimpleEsp32.h>

char auth[] = "jXXXXXXXXXXXXXXXXXXX"; //メールで送られるAuth Token
char ssid[] = "XXXXXXXXXXXXXX";
char pass[] = "XXXXXXXXXXXXXXX";

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;

//右ジョイスティックのデータ受信
BLYNK_WRITE(V0) {
Serial.println("right");
int x = param[0].asInt();
int y = param[1].asInt();

if(abs(y) > cont_thres){
if(x > cont_thres){ //前輪旋回
mRF = y;
mRR = 0;
mLF = -y;
mLR = 0;
}else if(x < -cont_thres){ //後輪旋回
mRF = 0;
mRR = y;
mLF = 0;
mLR = -y;
}else{ //超信地旋回
mRF = y;
mRR = y;
mLF = -y;
mLR = -y;
}
}else if(cont_thres2 < y && y < cont_thres){
if(x > cont_thres){ //右前信地旋回
mRF = x;
mRR = x;
mLF = 0;
mLR = 0;
}else if(x < -cont_thres){ //右信地旋回
mRF = 0;
mRR = 0;
mLF = x;
mLR = x;
}
}else if(-cont_thres < y && y < -cont_thres2){
if(x > cont_thres){ //左前信地旋回
mRF = 0;
mRR = 0;
mLF = x;
mLR = x;
}else if(x < -cont_thres){ //左後信地旋回
mRF = x;
mRR = x;
mLF = 0;
mLR = 0;
}
}else if(abs(y) <= cont_thres2 && abs(x)>cont_thres){ //前後進
mRF = x;
mRR = x;
mLF = x;
mLR = x;
}else{
mRF = 0;
mRR = 0;
mLF = 0;
mLR = 0;
}
}

//左ジョイスティックのデータ受信
BLYNK_WRITE(V1) {
Serial.println("left");
int x = param[0].asInt();
int y = param[1].asInt();

if(y < -cont_thres){
if(x > cont_thres){ //右斜め前
mRF = 0;
mRR = -y;
mLF = -y;
mLR = 0;
}else if(x < -cont_thres){ //右斜め後
mRF = y;
mRR = 0;
mLF = 0;
mLR = y;
}else{ //右スライド
mRF = y;
mRR = -y;
mLF = -y;
mLR = y;
}
}else if(y > cont_thres){
if(x > cont_thres){ //左斜め前
mRF = y;
mRR = 0;
mLF = 0;
mLR = y;
}else if(x < -cont_thres){ //左斜め後
mRF = 0;
mRR = -y;
mLF = -y;
mLR = 0;
}else{ //左スライド
mRF = y;
mRR = -y;
mLF = -y;
mLR = y;
}
}else if(abs(x) > cont_thres){ //前後進
mRF = x;
mRR = x;
mLF = x;
mLR = x;
}else{
mRF = 0;
mRR = 0;
mLF = 0;
mLR = 0;
}
}

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 )
pinMode(Srv_RF, OUTPUT);
pinMode(Srv_RR, OUTPUT);
pinMode(Srv_LF, OUTPUT);
pinMode(Srv_LR, OUTPUT);

Blynk.begin(auth, ssid, pass);

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

void loop() {
M5.update();
Blynk.run();

motor_drive(-mRF, -mRR, mLF, mLR);
delay(10);
}


M5UnitVのPythonコード


from machine import UART
from board import board_info
from fpioa_manager import fm
from Maix import GPIO
import sensor, lcd


sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.set_vflip(0)
sensor.set_hmirror(0)
sensor.run(1)
sensor.skip_frames()


fm.register(35, fm.fpioa.UART1_TX, force=True)
fm.register(34, fm.fpioa.UART1_RX, force=True)

uart = UART(UART.UART1, 1152000,8,0,0, timeout=1000, read_buf_len=4096)

while(True):
img = sensor.snapshot()
img_buf = img.compress(quality=20)
img_size1 = (img.size()& 0xFF0000)>>16
img_size2 = (img.size()& 0x00FF00)>>8
img_size3 = (img.size()& 0x0000FF)>>0
data_packet = bytearray([0xFF,0xD8,0xEA,0x01,img_size1,img_size2,img_size3,0x00,0x00,0x00])
uart.write(data_packet)
uart.write(img_buf)

スポンサードリンク

コメント

コメントの投稿

非公開コメント

プロフィール

RoboTakao

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

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

スポンサーリンク