ProcessingでM5StackのPoseをモデルに反映する

引き続いて、M5Stackのpitch/roll/yaw情報をUDPで送って、Processing上のモデルに反映してみます。

M5Stackからはpitch/roll/yaw情報をカンマ区切りのテキストで送ります。

このテキストをProcessingで受信して、直方体のモデルのposeに反映します。正負が逆になったx/z軸は値を反転。座標系の取り方でどうにでもなるような気もしますが。

Processingのソースは見ての通り単純です。最初3Dの描画できなくて調べたらベータ版では動かないようで4.1.1にアップデートしたらきちんと動作しました。

codeは、

https://github.com/chateight/processing/blob/main/sketch_221220b.pde

/*
read m5stack udp data(IMU data ; pitch, roll, yaw) and move the imaginary object

*/
import hypermedia.net.*;

UDP udp;
final String IP = "0.0.0.0";

PFont myFont;

float[] pry = new float[3];      // pitch, roll, yaw store work


void setup() {
  
  udp = new UDP(this, 3002);
  udp.listen( true );

  size(500,500,P3D);
  frameRate(30);
  loop();
}

void draw() {
  background(0);
 
  translate(width/2, height/2);
  rotateX(radians(-pry[1]));
  rotateY(radians(pry[0]));
  rotateZ(radians(-pry[2]));
  box(150, 150, 50);
}

void receive( byte[] data, String ip, int port ) {
  String message = new String( data );
  println( "received : \""+message+"\" from "+ip+" on port "+port );
  
  String[][] matchedTexts = matchAll(message, "[0-9|-]+.[0-9]+");

  if (matchedTexts != null) {
    int index = 0;
    for (String[] matchedText : matchedTexts){
      pry[index] = float(matchedText[0]);
      index++;
    }
  }
  println(pry[0], pry[1], pry[2]);    // pitch, roll, yaw
  
}

動かして見たのは、今のYouTube画面で参照ください。yawは時間経過とともにずれていくのでいくらか補正はしていますが、十分じゃ無いですね。

 

admin

Processingで受信データをグラフ化する

M5Stackからのデータを視覚化するのに、一番手軽に使えそうなツールだと思うProcessingでM5StackからUDPで送られてくる温度データを時系列でグラフ化してみました。Bluetoothで送るのは接続不安定でストレスになるから、簡単に送れるUDPを使います。電池寿命とか関係ないし、

<M5Stackから送られるメッセージ形式>

日時(秒まで)と温度データ

<Processingのコード>

メッセージから、温度部分を抜き出して表示しています。Processingの画像処理は専用のAPIになっています。正規化処理もJavaとは別物ですが、

receive()メソッドは、UDPリッスンで受信データがあれば起動されるでしょうが、drawの読み出し(val)と書き込みが競合すれば値は不定になる可能性はありそうですが、視覚的なものなので排他制御をするまでもないでしょう。

                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                             

import hypermedia.net.*;
import controlP5.*;

UDP udp;
final String IP = "0.0.0.0";

ControlP5 cp5;
Chart chart_tmp;
final int NUM_GRAPH_DATA = 200;

PFont myFont;

float val;

void setup() {

  size(650, 700);
  myFont = createFont("Arial", 30); 
  
  udp = new UDP(this, 3002);
  udp.listen( true );

  frameRate(10);
  cp5 = new ControlP5(this);
  
  chart_tmp = cp5.addChart("tmp sensor");
  chart_tmp.setView(Chart.LINE)                             
            .setRange(20, 22)                         
            .setSize(600, 200)                               
            .setPosition(10, 250)                           
            .setColorCaptionLabel(color(0,0,255))          
            .setStrokeWeight(1.5)                            
            .getColor().setBackground(color(224, 224, 224))  
            ;
  chart_tmp.addDataSet("temp");
  chart_tmp.setData("temp", new float[NUM_GRAPH_DATA]);
  chart_tmp.setColors("temp", color(0, 0, 192), color(255,255,128));

}

void draw() {
  
  background(255);
  fill(0);

  chart_tmp.unshift("temp", val);
}

void receive( byte[] data, String ip, int port ) {
  String message = new String( data );
  println( "received : \""+message+"\" from "+ip+" on port "+port );
  
  String[] matchedTexts = match(message, "[0-9|-]*.[0-9]*$");

  if (matchedTexts != null) {
    println(matchedTexts[0]);
    val = float(matchedTexts[0]);
  }

}

動画はこちらを参照ください、グラフ下端が20℃、上端が22℃で範囲を拡大して見やすくしています。

processing – SD 480p

 

admin

 

 

M5StackのBluetooth Serial(追加)

以前にやった時もそうですが、接続が安定しない原因は何なのかの単純化ですが、相変わらずロジックは分かりません。

M5stack Bluetoothでのmacとの送受信

Arduino IDEで以下のようなソースを動作、

#include <M5Stack.h>
#include "BluetoothSerial.h"

BluetoothSerial SerialBT;

int cnt = 0;

void setup() {
  Serial.begin(115200);
  SerialBT.begin("ESP32");
}

void loop() {
  SerialBT.println("Hello World : " + String(cnt));
  cnt += 1;
  delay(1000);
}

ここで、ArduinoIDEのシリアルをBluetoothに選択、

 

すると、コンソールに結果出力されますが、この状態でMacのコンソールを開く(つまりコンソールが2台ある状態)だと、両方のコンソールにランダム風に結果が出力されます。これはまともでしょう、

いずれの状態でもMac本体のBluetoothはペアリングはしていない。

これだけシンプルでも、再コンパイルとかで期待したように動かなくなります。回避方法はMacの再起動のみ。

再起動すると、以下のようにBluetooth接続も復活しますが。

この後でArduinoIDE側のシリアルをUSBに切り替えて、Macコンソールを起動、

ここまでは正常ですが、ここでM5Stackをリセットすると接続が切れて復旧しない。これはArduinoIDE側のシリアルをBluetoothにしていても同じだから、Macを立ち上げて最初にBluetoothシリアル使うまではいいけれども、その後なんらかのアクションをすると必ずMacをリブートしなければいけないという不条理な状態になっています。

 

admin

バランスロボット

二輪で、本体の傾きをセンスしてバランスをとりながら倒れないように制御するロボット(倒立振子というらしい)ですが、イメージとしては秋田の竿燈みたいなものか。竿燈は二次元の制御だけど、二輪ロボットは一軸制御だから制御は相対的には楽そうに見えます。

回転モーメントが大きいほど、つまり高さ方向が長いほどフィードバック時間に余裕がありそうに思えますね。

原理は、

https://matome.eternalcollegest.com/post-2136401209031725401

完成品としては、

https://docs.m5stack.com/en/app/bala2fire

や、

https://www.switch-science.com/products/8297

あたりがありそうですが、M5stack使って自作するというのもありそうです。

例えばこれとか、

https://maker.pro/arduino/projects/build-arduino-self-balancing-robot

PID制御はカットアンドトライでパラメータを決めるのが現実らしい。PIDライブラリというのがArduinoには用意されていて、Arduino IDEのライブラリで検索するとたくさん出てきます。

 

admin

M5stackでTelloを制御

M5stackでUDPが使えるから、Telloのコマンド制御もできるよねと思いましが、既に先人のコードができてます。

https://qiita.com/Mitsu-Murakita/items/b86ad79d3590adb3b5b9

TelloのSSIDだけを追加すれば動きましたが、動作がイマイチもっさり、というのはスイッチの反応が遅くて多少長押ししないとtake offもしない。

で時間待ちの500msを150msにするとほぼイメージ通りに動きますが、疑問点が一つ。

wasPressed()関数って、ボタン押されたことを記憶できる関数のはずなのになぜ長押ししないといけないか?

loop()の中でボタン機能の確認コードを作って確認してみると、どうもdelay()中にボタンを押してもwasPressed()には反映されない模様です、だからdelay()で設定した時間以上にボタンを押していないといけないようです、仕様として変だと思いますが。

~~~
  if ( M5.BtnA.wasPressed() ) {
    Serial.println("BtnA.wasPressed() == TRUE");
  }
  //Serial.println("loop");

  delay(300);
~~~

 

あとフリップ動作は、イマイチスイッチを押すタイミングとM5stackを傾けるタイミングが掴めない。

 

admin

ラズパイでWebSocket通信(その3)

 

https://isehara-3lv.sakura.ne.jp/blog/2022/11/29/ラズパイでwebsocket通信(その2)/

の続編で、

① M5stackからラズパイにUDPでデータ送信して、

② ラズパイとクライアント間でweb_socket使ってリアルタイムのデータ更新(今は5秒毎にM5stackから送信)、

③ 受信データをchart.js使ってグラフ化(データ最大5回分)、

します。

WebSocketで送られるデータ形式は汎用性考慮してjson形式を使っています。

全体のコードは、

M5stack側、

https://github.com/chateight/multi_sensor_udp_comm

ラズパイ側、

https://github.com/chateight/web_socket

となります。現状、測定値を送信するのはthermo_sensorのコードだけですが、他のセンサーでも同じルーチン(udp_loop(String))呼び出せば送信できます。

web_socketはセッションが有効な時しか働かないので、セッションが切れればデータは更新されません。

ブラウザ画面の表示はこちらを、

m5stack_ws – 720WebShareName

 

admin

M5stackからUDPでラズパイにメッセージ送信

M5stackをIoT端末として使うときにサーバー(この場合はラズパイ)にメッセージを送る必要がありますが、Wi-Fiが一番汎用性がありそう。

というわけで、UDP使ってメッセージを送ってみます。ラズパイ側はサーバーにNode.js使うので同じくNode.jsスタイルで記述します。

<M5stack側>

NTPサーバー使って現在時刻の取得、mDNS機能を使ってラズパイのアドレスを検索しています。参考サイトは、以下の通りです。

・タイムサーバーの使い方

https://kuracux.hatenablog.jp/entry/2018/10/08/160000

・IPアドレスを4オクテット記法に変換する

https://qiita.com/dojyorin/items/ac56a1c2c620782d90a6

#include <m5stack.h>
#include <WiFi.h>
#include <wifiudp.h>
#include <espmdns.h>


// Wi-Fi connection info.
const char* ssid     = "your said";
const char* password = "your pw";
const int port = 3002;

String string_buf = "message from M5stack";

//ntp server and time info
const char* ntpServer =  "ntp.nict.jp";
const long  gmtOffset_sec = 9 * 3600;
const int   daylightOffset_sec = 0;
struct tm timeinfo;

// The udp library class
WiFiUDP udp;

// mDNS target & M5stack
const String target_dev = "raspberrypi";
IPAddress ip;         // IPAdress of the target device
String ip_addr = "";  // 4 octet style target address
const char *mdns_name = "m5stick";  //M5stack mDNS name

// convert IP address to 4 octet style
String ipToString(uint32_t ip){
  String result = "";

  result += String((ip & 0xFF), 10);    // base number : 10
  result += ".";
  result += String((ip & 0xFF00) >> 8, 10);
  result += ".";
  result += String((ip & 0xFF0000) >> 16, 10);
  result += ".";
  result += String((ip & 0xFF000000) >> 24, 10);

  return result;
}

void mdns(){
  mdns_init();
  ip = MDNS.queryHost(target_dev);
  // target dev IP
  Serial.println("");
  Serial.print(target_dev + " : ");
  Serial.println(ip);
  // M5 self IP
  Serial.println("");
  Serial.print("Connected to ");
  Serial.println(ssid);
  Serial.print("IP address: ");
  Serial.println(WiFi.localIP());
  ip_addr = ipToString(ip);
  // register mDNS name
  if (MDNS.begin(mdns_name)) {
    Serial.println("MDNS responder started");
  }
}

void print_wifi_state(){
	M5.Lcd.clear(BLACK);  // clear LCD
  M5.Lcd.setTextColor(YELLOW);
  M5.Lcd.setCursor(3, 3);
	M5.Lcd.println("");
	M5.Lcd.println("WiFi connected.");
	M5.Lcd.print("IP address: ");
	M5.Lcd.println(WiFi.localIP());
	M5.Lcd.print("Port: ");
	M5.Lcd.println(port);
}

void setup_wifi(){
	M5.Lcd.setTextColor(RED);
  M5.Lcd.setTextSize(2);
  M5.Lcd.setCursor(3, 10);
  M5.Lcd.print("Connecting to ");
  M5.Lcd.println(ssid);

	// setup wifi
  WiFi.mode(WIFI_STA);  // WIFI_AP, WIFI_STA, WIFI_AP_STA or WIFI_OFF
  WiFi.begin(ssid, password);
	// WiFi.begin();

	// Connecting ..
  while (WiFi.status() != WL_CONNECTED) {
    delay(100);
    M5.Lcd.print(".");
  }

	// print state
  print_wifi_state();
  udp.begin(port);
  
  // mDNS
  mdns();
}

void setup() {
	M5.begin();
  Serial.begin(115200);
	// setup wifi
	setup_wifi();
    //init and get the time
  configTime(gmtOffset_sec, daylightOffset_sec, ntpServer);
}

void loop(){
    // get current rime
    if (!getLocalTime(&timeinfo)) {
      Serial.println("Failed to obtain time");
    return;
    }
    String cur_time = String(timeinfo.tm_year + 1900) 
      + "/" + String(timeinfo.tm_mon) 
      + "/" + String(timeinfo.tm_mday) 
      + " " + String(timeinfo.tm_hour) 
      + ":" + String(timeinfo.tm_min) 
      + " " + String(timeinfo.tm_sec) + " ";
    Serial.println(cur_time);

    //udp send
    const char *ipadr = ip_addr.c_str();
    udp.beginPacket(ipadr, port);
    // current time
    int i = 0;
    while( cur_time.length() > i){
      char tmp = cur_time.charAt(i);
      udp.write(tmp);
      i++;
    }
    // contents
    i = 0;
    while( string_buf.length() > i){
      char tmp = string_buf.charAt(i);
      udp.write(tmp);
      i++;
    }
    udp.endPacket();
    delay(1000);
}

 

<ラズパイ側>

Node.js配下のスクリプトです。

const dgram = require('dgram');
const port = 3002;
const client ='0.0.0.0';

const socket = dgram.createSocket('udp4');

socket.bind(port, client);

socket.on('message', (message, remote) => {
    console.log(remote.address + ':' + remote.port +' - ' + message);
});

参考サイトは、

・ラズパイのavahiユティリティのインストールと使い方

$ sudo apt-get install avahi-utils

Wi-Fi内のデバイスリスト

$ avahi-browse -at

・Node.jsのUDP通信ドキュメント

https://runebook.dev/ja/docs/node/dgram

・what’s socket.io

https://socket.io/docs/v4/

ラズパイ側の出力、

 

admin

MLX90614 thermal sensor(M5stack)

センサーシリーズの第三段目の赤外線温度センサー。この際、センサー種類を切り替える都度ソフトダウンロードが面倒なので、複数のセンサー(今現在、レーザー距離計、熱中症アラームセンサーと赤外線温度センサー)に対応できるようにコードを書き換え。

具体的にはsetup()処理の中で、センサーの初期化コマンドを出して正常にレスポンスが返って来たものをターゲットにして、loop()処理中で呼び出す処理を振り分けします。以下はsetup()処理だけ切り出し、

void setup(void)
{
  M5.begin();             // Init M5Stack
  M5.Power.begin();       // Init power  
  Wire.begin();           // Wire init, adding the I2C bus.
  Serial.begin(115200);

  if (setup_dist() == 1){
    dev_id = 1;
  } else if (setup_env() == 1){
    dev_id = 2;
  } else if (setup_thermo() == 1){
    dev_id = 3;
  }
  else{
    while(1){
      sensor_error();
      delay(1000);
    }
  }
}

温度センサーはライブラリ、

https://github.com/adafruit/Adafruit-MLX90614-Library

をダウンロードして使用。

使ってみた感じは、非接触ではあるけれどもターゲットにかなり近寄らないときちんとした値が取れないようです。

————————

P.S. 2022/11/24

MLX90614には視野角で何種類かあって、このモデルはおよそ90度なのでその視界に入るオブジェクトの平均値が表示されるからです。

————————

こんな感じに、

全体のコードは以下のリンクで、

https://github.com/chateight/multi_sensor_hdl

 

admin

M5stackで使えるメモリ容量を大きくとりたい

M5stackでビルドした時に必要メモリサイズが出力されますが、なぜか搭載メモリ(Flash)サイズに比較してずいぶん小さい。

やりたいことは色々なセンサーの種類をsetup()ルーチン中で判別して必要なコードにloop()から呼び出しをするということなので、センサーの種類が増えていくとメモリをどんどん消費していくから。

写真の場合にはmDNSを使おうとした場合ですが、ライブラリのサイズが大きいようで既に6割以上を消費しています。

実際のメモリは16MB搭載しているはずですが、1.3MBぐらいしか使えないことになっています。もちろん基本のファームウェア領域も存在するんだろうけど、それにしては少なすぎ。

これを拡張するには、ボードの種類をM5Stack-Core-ESP32からM5Stack-fireにすると拡大します。ただしこれでも見えるサイズは6.5MBぐらいなので搭載メモリの半分以下ですが、5倍に拡大されたと思えば大幅改善で、さらにRAM領域はおよそ10倍に拡張されているので、おそらくRAM領域は不足するような事態には特別にRAM領域を消費するようなコードを書かない限りは大丈夫そうです。

ボード種類を変えることによる副作用は今のところ見えていません。このやり方はArduino IDEに限らずVScodeのPlatformIOでも同じです。

M5stackでもコードは起動後は不変だからFlash領域を使用して、変数やスタック領域などの書き換え必要な領域にはRAM領域を使用するようになっています。

 

admin

 

 

 

M5stackでmDNSを扱う

同じWi-Fiエリアで例えばラズパイと通信しようとすると固定IPは使わないだろうからアドレス解決にmDNSを使う必要があります。ラズパイ側はavahiでmDNSがデフォルトで使えるようになってますね。

参考サイトは、

https://shuzo-kino.hateblo.jp/entry/2021/02/22/230207

・公式ドキュメント

https://espressif.github.io/esp-protocols/mdns/en/index.html

を参考に、動かしてみました。

mDNSをサポートするライブラリは”espmdns.h”になります。

#include <m5stack.h>

#include <wifi.h>
#include <wificlient.h>
#include <webserver.h>
#include <espmdns.h>

const char* ssid = "said"; // set your said
const char* password = "pw"; // set your pw

String target_dev = "NODE-1cdc";

WebServer server(80);

void handleRoot() {
  server.send(200, "text/plain", "hello from M5stack server using mDNS!");
}

void handleNotFound() {
  String message = "File Not Found\n\n";
  message += "URI: ";
  message += server.uri();
  message += "\nMethod: ";
  message += (server.method() == HTTP_GET) ? "GET" : "POST";
  message += "\nArguments: ";
  message += server.args();
  message += "\n";
  for (uint8_t i = 0; i < server.args(); i++) {
    message += " " + server.argName(i) + ": " + server.arg(i) + "\n";
  }
  server.send(404, "text/plain", message);
}

void setup(void) {
  M5.begin(true, false, true);
  M5.Power.begin();       // Init power  
  M5.lcd.setTextSize(2);  // Set the text size to 2. 
  M5.Lcd.setTextColor(WHITE);     // set font color to white
  M5.lcd.fillScreen(BLACK);
  delay(500);
  WiFi.mode(WIFI_STA);
  WiFi.begin(ssid, password);
  Serial.println("");

  // Wait for connection
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  M5.lcd.setCursor(50, 110);
  M5.Lcd.print("connected to Wi-Fi");

  mdns_init();
  IPAddress ip = MDNS.queryHost(target_dev);
  Serial.println("");
  Serial.print(target_dev + " : ");
  Serial.println(ip);

  Serial.println("");
  Serial.print("Connected to ");
  Serial.println(ssid);
  Serial.print("IP address: ");
  Serial.println(WiFi.localIP());

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

  server.on("/", handleRoot);

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

  server.onNotFound(handleNotFound);

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

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

・HTTPサーバーとしてのmDNS機能の確認

M5stackにHTTPサーバー機能を持たせてそのアドレス検索にmDNSを使っています。

・ネットワーク内のmDNSデバイスのIPアドレス検索

53行目から57行目がそのコードになります。

・M5stackにはWi-Fi接続完了時にメッセージ出しています

 

admin