LEGO MindStorms Robot Inventorキットのハブ間通信のESP32によるエミュレーションライブラリ

LEGO MindStorms は、プロトタイピングや教育用にも非常によいツールであり、20年以上前のRIS時代から利用しているが、2022年6月にやっと日本でも発売になったRobot Inventorキットでは、以前にも増してセンサーやモーターのコネクタが独自形状で、自作のセンサーやアクチュエータを接続することは、かなりハードルが高いものになっている。

このキットではPythonやビジュアルプログラミングツールで動作をプログラミングできるのだが、これによって、プログラムを実行するHubと呼ばれるメインユニット同士のHub間通信ができるようになっており、BLEによるシグナル名とデータの組み合わせによる無線通信が可能になっている。

これをESP32などのBluetooth内蔵マイコンで送受信できれば、いろいろ夢が広がりそうということで、いろいろ探してみたところ、以下のサイトで解析した結果が公開されていた。

LEGO MINDSTORMS Robot Inventor's Hub to Hub Communication Hacks 

実のところ、このサイトの情報には誤りがあって（もしかしたら、この解析のあとにLEGO社の仕様が変わったのかもしれないが）、LEGO社の製造者IDのエンディアンが逆になっていて、この情報のままでは通信できなかった。それに気がつくまでは、結構七転八倒したわけだが、最終的にはBLEパケットモニタでモニタリングしてやっと気がついたということで、めでたく通信が可能となったわけである。

なんちゅうことはない、Advertisementデータをブロードキャストして、製造者IDとシグナル名が一致するデータを勝手に受信するだけという仕組みである。そういう意味では、送受信の確実性はないのが玉に瑕なのだが、お手軽さには勝てないので、早速実装することに。

以下に、ESP32のM5StickCPlusやM5StampC3で実行確認済みのライブラリを貼っておくので、このアイデアというかソースコードとかは、自由に使ってもらって構わない。

まさかとは思うが、金を取っていろいろやるものに組み込む人はいないとは思っているのだが、勝手にLEGO社の製造者IDを使っているので、その辺はかなりグレーである。その辺のトラブルには巻き込まれたくないので、使用は各自の目の届く範囲に収めておくこと。

今回は調子に乗って、作品を紹介する動画まで作ってみた。参考までに貼っておく。

みなさんが、これで一歩踏み込んだLEGO MindStormsライフを送れることを祈る！

コロナ禍なのでCO2センサの情報をLEGOに送って何かおもろいからくり作りたいなぁ・・・・

// LEGO Hub2Hub Communication Emulator for ESP32
//  Copyright (C) 2022 Kikyoya (Karakuri Studio).
//
// Tested M5StickCPlus, M5StampC3
//
/* 使用法(usage)
Setup()
  void initHub2Hub(String dev="Hub2Hub LEGO"); dev:デバイス名(省略可)

loop()
Sending: Advertising to LEGO HUB
  void sendHub2HubData(String key,String data,int seq=H2H_AUTOSEQ,int period_ms=300);
  key & dataは必須、seqとperiod_msは省略可
  LEGOではkeyはシグナル名と表記されている。日本語を使う場合UTF-8で記述
  dataは22バイトまで（NULL文字入れて23バイト）
  Seqは、送るデータが変わるたびに違う値に更新。省略またはH2H_AUTOSEQで自動更新
  period_msは、データ送信する期間。この間複数回にわたって送信する

Recieving: Receive HUB to HUB data
  受信パケットをスキャンしてKeyに一致するものがあるかどうか判別
    bool scanHub2Hub(String key);
  受信したデータを返す
    String getHub2HubData();
    受信した文字列を返す、受信なしの場合は””が返る 
*/
#include <BLEDevice.h> // Bluetooth Low Energy 
#include <esp_rom_crc.h>  // CRC32 Lib

#define H2H_AUTOSEQ -1
const uint16_t ManuID = 0x0397; // Manufacturer ID(LEGO)

// key string to CRC32(little endian)
uint32_t calCRC(String key){
  return esp_rom_crc32_le(0,(uint8_t*)key.c_str(), key.length());
}

#pragma pack(1)
union aPacket{
  char aData[32];
  struct {
    byte len;
    byte adtype;
    uint16_t manuid;
    byte seq;
    uint32_t key;
    char str[23];
  };
  aPacket(String k,String d,byte s){
    len = d.length()+8;
    adtype = 0xff;
    manuid = ManuID;    // LEGO 0x0397
    seq = s;
    key = calCRC(k);
    strcpy(str,d.c_str());
  };
};
#pragma pack()

void sendHub2HubData(String key,String data,int seq=H2H_AUTOSEQ,int period_ms=300)
{
    static byte _seq=1;
    if(seq==H2H_AUTOSEQ) seq=_seq++;   // 引数なし(=H2H_AUTOSEQ)でseqは自動更新
    BLEAdvertising *pAdv = BLEDevice::getAdvertising(); // get adv. object
    BLEAdvertisementData oAdvData = BLEAdvertisementData(); // get Adv Data
    data.remove(22); // limited 22bytes(23文字以降は削除)

    aPacket ad = aPacket(key,data,seq); // Key & data
    
    pAdv->setAdvertisementType(ADV_TYPE_SCAN_IND);
    oAdvData.addData(ad.aData);
//  for(int i=0;i<32;i++) Serial.printf("%02x ",ad.aData[i]); // for Debug
    pAdv->setAdvertisementData(oAdvData);

    pAdv->start();  // Start Advertising
    delay(period_ms);
    pAdv->stop();   // stop Advertising
}

bool bRec=false;
uint32_t KeyCRC;
String Hub2HubStr;

// Scan call back object
class Hub2HubCB: public BLEAdvertisedDeviceCallbacks {
  void onResult(BLEAdvertisedDevice dev){
    if(dev.haveManufacturerData()){ // data available
        static int r_seq = -1; 
        std::string data = dev.getManufacturerData();
        int manu_id = data[1] << 8 | data[0];
        int seq = data[2];
        uint32_t *hash = (uint32_t*)&(data[3]); // LE
        if((manu_id==ManuID)  // check ManuID & Key
            &&(KeyCRC==*hash)
            &&(seq!=r_seq)){  // check seq no
          Hub2HubStr=String((char*)&data[7]);
          r_seq=seq;
          bRec=true;
        }
        dev.getScan()->stop();   // abort scanning
    }
  }
};
// scan Hub to Hub Packet
bool scanHub2Hub(String k)
{
  KeyCRC=calCRC(k);
  bRec=false;
  BLEDevice::getScan()->start(1);
  return bRec;
}
// get Hub to Hub Data
String getHub2HubData()
{
    if(bRec){
      bRec=false;
      return Hub2HubStr;
    }
    return "";
}
// init Hub to Hub Lib
void initHub2Hub(String dev="Hub2Hub LEGO")
{
    BLEDevice::init(dev.c_str());   // initialize device
// set BLE Scan Callback(コールバック登録)
    BLEDevice::getScan()->setAdvertisedDeviceCallbacks(new Hub2HubCB());
}