PCファンをUSB扇風機として使えるように整流板を作ってみた。

 以前余ったファンをUSBで使えるように、5Vから12Vに昇圧してファンガードを両面につけてUSB扇風機として使うようにしてみたのだけれども、やっぱり風量が足りない。ポータブルファンとかと比べると回転数に対しての風量が少ない気がする。

PC用のファンなので実は風量よりも風向きがあまり良くないのではないかということで調べてみると、AINEXから風の直進性を高めるアイテムが出ていたので試してみたかったのだけれども販売終了していたので同じようなものを作ってみた。

整流板としてはとりあえず形状は何でも良さそうではあるけどもオリジナルを忠実に再現してヘキサゴン形状にした。大きさもスペックから同じぐらいの大きさにしてみた。ファンと嵌合する部分の長さがわからなかったけど予想でこんな感じに。Onshapeフリープランなのでfan boosterで検索すれば見つけられるかも。

というわけで例によって3Dプリンタ持っている人にお願いしてプリントしてもらった。ファンと合わせるくぼみのせいで余計なサポートが付いてしまったようで申し訳ないけど。

これ実は反対面をサポートにしたほうが良かったのでは？と思ったけどもまぁとりあえずこれでも問題なさそう。

ファンに装着するとこんな感じ。オリジナルはきつかったみたいだけどそれを考慮してゆとりをもたせたおかげでいい感じに装着できた。

長さ方向はこんな感じ。オリジナルと同じぐらいの長さになってると思う。

効果の程は結構絶大で、ネジ止めせずにつけたり外したりして試してもかなり風の直進性が高まってることが感じられる。自分に風を向けていると今まではファンの回転数調整はいらないと思っていたが少し下げられるように回転数を調整したいぐらいに涼しい。このファンだと2mぐらいでも風が感じられるぐらいには直進性が増していた。

次は風量を調整できるようにしないと…

もう少し小さいファンで持ち手をつけてハンディーファンでも作れそう。

EEZ StudioとArduino IDEでGUIを作ってみた

 この前ESP32-S3を張り替えたJC4827W543Cだけど、ROMも8MBにアップグレードしたことだしEEZ StudioでGUIアプリを作ってみることに。逆にArduino IDEだけでGUI作るの大変そう。

まずはEEZ Studioのインストール。とりあえずWindows用のexeファイルをダウンロードしたらインストーラーに従ってインストールできた。
起動するとHomeが表示されるのでLVGLを選んで、Nameにプロジェクト名をいれて、LVGL VerisionはArduino IDEにインストールするLVGLバージョンに合わせて選択する。今回は最新版を使用した。プロジェクトの保存先は覚えておかないとuiフォルダの中身をArduinoのプロジェクトにコピーする時困るので気をつけないと。

プロジェクトを作成したらまずはSettingsでDisplay widthとheightをNV3041Aに合わせるために480と272に変更する。あとはBuildのところでLVGL includeをlvgl.hに変更しておく。Mainタブに戻っても最初に作られた画面の解像度は自動で修正されないのでこちらも同じ画面に修正した。とりあえず適当にボタンを追加してみた。

スパナのマークを押すとソースファイルがプロジェクトフォルダの中のuiフォルダに生成される。これをArduinoのプロジェクトフォルダにコピーしてやる感じ。

ということで次はArduino側のプロジェクトを作成する。lvglライブラリはEEZ Studioで指定したバージョンをArduino IDEからインストールしておく。他にArduino_GFXライブラリとタッチパネルのGT911用にTouchLibライブラリが必要だった。

inoファイルの中身は

#include <lvgl.h>
#include <Arduino.h>
#include <Arduino_GFX_Library.h>
#include "touch.h"
#include "ui.h"

#define SCREEN_WIDTH 480
#define SCREEN_HEIGHT 272

#define LCD_BL 1

Arduino_DataBus *bus = new Arduino_ESP32QSPI(45, 47, 21, 48, 40, 39);
Arduino_GFX *gfx = new Arduino_NV3041A(bus, GFX_NOT_DEFINED, 0, true);
static lv_color_t buf1[SCREEN_WIDTH * 40];

void my_flush_cb(lv_display_t *disp, const lv_area_t *area, uint8_t *px_map)
{
    uint32_t w = (area->x2 - area->x1 + 1);
    uint32_t h = (area->y2 - area->y1 + 1);

    gfx->draw16bitRGBBitmap(area->x1, area->y1, (uint16_t *)px_map, w, h);

    lv_display_flush_ready(disp);
}

void my_touch_read(lv_indev_t *indev, lv_indev_data_t *data)
{
    if (touch_has_signal() && touch_touched())
    {
        data->state = LV_INDEV_STATE_PRESSED;
        data->point.x = touch_last_x;
        data->point.y = touch_last_y;
    }
    else
    {
        data->state = LV_INDEV_STATE_RELEASED;
    }
}

void backlight_init()
{
    pinMode(LCD_BL, OUTPUT);
    digitalWrite(LCD_BL, HIGH);
}

void lvgl_init_all()
{
    if (!gfx->begin()) {
        Serial.println("gfx begin failed!");
    }
    gfx->fillScreen(0x0000);
    backlight_init();

    touch_init(gfx->width(), gfx->height(), gfx->getRotation());

    lv_init();

    lv_display_t *disp = lv_display_create(SCREEN_WIDTH, SCREEN_HEIGHT);
    lv_display_set_buffers(disp, buf1, NULL, sizeof(buf1), LV_DISPLAY_RENDER_MODE_PARTIAL);
    lv_display_set_flush_cb(disp, my_flush_cb);
    lv_display_set_default(disp);
    lv_indev_t *indev = lv_indev_create();
    lv_indev_set_type(indev, LV_INDEV_TYPE_POINTER);
    lv_indev_set_read_cb(indev, my_touch_read);
    lv_indev_set_display(indev, disp);
}

void setup()
{
    Serial.begin(115200);
    Serial.println("START");

    lvgl_init_all();
    ui_init();
    Serial.println("EEZ started");
}

void loop()
{
    lv_timer_handler();
    ui_tick();
    lv_tick_inc(5);
    delay(5);
}

こんな感じにしてみた。バックライトはとりあえず輝度固定。あとはタッチパネル用にtouch.hをDemoから頂戴してきてプロジェクトフォルダに放り込む。そんでもってEEZ Studioで生成したuiフォルダのhファイルとcファイルをすべて同じところに放り込む。

あとはビルドして書き込めば完成。最初なかなか映らなくて困っていたんだけど、loopのなかにlv_tick_inc(5);を入れ忘れいていただけだったorz。

とりあえずテスト画面が表示されたので次は各コンポーネントを試してみた。

まずはtextから。これは文字通りtextを表示させるものなんだけど、コンポーネントの設定で名前(GeneralのNameってところ)を入れておくとArduinoで関数を読み込むと表示を変えることもできる。

lv_label_set_text(objects.test, "Hello");

こんな感じでArduinoのどこかで上のように実行すればNameにtextって入れてあるtextの内容をHelloに書き換えれる。センサーの値を取ってきてそれをtextに表示するみたいなことも可能。

お次はButton。ButtonはEventsのところでイベントハンドラーを追加する。Clickedを選択して、Actionは新しいActionを作成してAction自体に名前をつける。あとはMainとかがあるタブの左側に<>User Actionsっていうタブがあるのでそこで先程作ったAction(今回はmovetotestを選択すると左の方にTipsがあって、それのコードをArduinoのコードに追加すると実行可能。

#include "actions.h"

void action_movetotest(lv_event_t *e) {
    //lv_scr_load(objects.test); 
    //Serial.println("Button");
    lv_label_set_text(objects.test, "Hello");
}

Actions.hをIncludeしてこんな感じでActionに対しての関数を追加する感じ。コメントアウトされてるけどlv_scr_loadを使えばtestっていうページを追加して表示したり、ボタンを押したらtextの内容を書き換えたり。

Buildをした後にArduinoのプロジェクトの中にコピーするのが面倒になったのでSettingsのDestination folderを直接指定してみた。これで忘れることがないはず…

EEZ Studioを使うと簡単にUIが作れるので便利かも。

ESP32でGPSラップタイマーを自作してみる

 知り合いがGPSラップタイマーが欲しいけど高いよーとか言っているのでこの前購入したGPSモジュールで試しに作ってみることに。ATGM336Hのスペック上10Hzまで行けるらしいし。

とりあえずESP32でGPSのMNEAセンテンスを読んで、指定している座標Aと座標Bの間を通ったらカウントする感じ。これならタイム測定ポイントを正確に指定できるかなと。最終的にはWeb設定画面を表示して設定変更したり、内部メモリで選択できるようにしたいな。

まずはちゃんと座標間を通ったときにカウントできるかのテスト。最初に通ったらカウントを開始して、2回目通ったらLAPタイムをSerial.printするだけのテスト。M5Atom Liteで動作確認してみた。

#define GPS_RX 32
#define GPS_TX 26

char nmeaLine[128];
int idx = 0;
double LAT0 = 38.14;
double LON0 = 140.77;

HardwareSerial GPS(2);

struct Vec2 {
  double x;
  double y;
  uint64_t time_ms;  // 経過ms
};

struct Line {
  double x1, y1, x2, y2;
};

Vec2 prevPos;
bool firstFix = true;
bool firstLineCross = true;
uint64_t lastLapTime = 0;

Vec2 latlonToXY(double lat, double lon, uint64_t t) {
  Vec2 p;
  p.x = (lon - LON0) * cos(LAT0 * DEG_TO_RAD) * 111320.0;
  p.y = (lat - LAT0) * 110540.0;
  p.time_ms = t;
  return p;
}

double cross(double ax, double ay, double bx, double by) {
  return ax * by - ay * bx;
}

double signedDistance(const Vec2& p, const Line& L) {
  double lx = L.x2 - L.x1;
  double ly = L.y2 - L.y1;
  double px = p.x - L.x1;
  double py = p.y - L.y1;
  return cross(lx, ly, px, py);
}

bool checkLineCross(const Vec2& prev, const Vec2& now, const Line& line, uint64_t& crossTime) {
  double d1 = signedDistance(prev, line);
  double d2 = signedDistance(now, line);
  if (d1 * d2 >= 0) return false;
  // 交点の補間
  double ratio = fabs(d1) / (fabs(d1) + fabs(d2));
  double cx = prev.x + (now.x - prev.x) * ratio;
  double cy = prev.y + (now.y - prev.y) * ratio;
  // 線分範囲内判定
  double minX = fmin(line.x1, line.x2);
  double maxX = fmax(line.x1, line.x2);
  double minY = fmin(line.y1, line.y2);
  double maxY = fmax(line.y1, line.y2);
  if (cx < minX || cx > maxX || cy < minY || cy > maxY)
    return false;  // 線分の延長上なら無視
  crossTime = prev.time_ms + (uint64_t)((now.time_ms - prev.time_ms) * ratio);
  return true;
}
Line startLine;
void setupStartLine() {
  Vec2 A = latlonToXY(38.142650747977115, 140.77813371163867, 0);
  Vec2 B = latlonToXY(38.14193773632026, 140.77804079991844, 0);
  startLine = { A.x, A.y, B.x, B.y };
}

double nmeaToDeg(const char* val, const char* dir) {
  double raw = atof(val);
  int deg = (int)(raw / 100);
  double min = raw - deg * 100;
  double result = deg + min / 60.0;

  if (dir[0] == 'S' || dir[0] == 'W')
    result *= -1;
  return result;
}

void parseRMC(char* line) {
  char* token;
  int field = 0;
  char utc[16] = { 0 };
  char status[2] = { 0 };
  char lat[16] = { 0 }, lon[16] = { 0 };
  char ns[2] = { 0 }, ew[2] = { 0 };
  char date[8] = { 0 };
  token = strtok(line, ",");
  while (token) {
    field++;
    if (field == 2) strcpy(utc, token);
    if (field == 3) strcpy(status, token);
    if (field == 4) strcpy(lat, token);
    if (field == 5) strcpy(ns, token);
    if (field == 6) strcpy(lon, token);
    if (field == 7) strcpy(ew, token);
    if (field == 10) strcpy(date, token);
    token = strtok(NULL, ",");
  }
  if (status[0] != 'A') return;
  double latitude = nmeaToDeg(lat, ns);
  double longitude = nmeaToDeg(lon, ew);
  uint64_t gpsTime = esp_timer_get_time() / 1000;  // μs → ms

  if (firstFix) {
    LAT0 = latitude;
    LON0 = longitude;
    setupStartLine();
    prevPos = latlonToXY(latitude, longitude, gpsTime);
    firstFix = false;
    Serial.println("Fix complete, waiting for START line...");
    return;
  }
  Vec2 pos = latlonToXY(latitude, longitude, gpsTime);
  // ラップ判定
  uint64_t crossTime;
  if (checkLineCross(prevPos, pos, startLine, crossTime)) {
    if (firstLineCross) {
      lastLapTime = crossTime;
      firstLineCross = false;
      Serial.println("START!");
    } else if (crossTime - lastLapTime > 8000) {
      uint64_t lapMs = crossTime - lastLapTime;
      uint32_t minutes = lapMs / 60000;
      uint32_t seconds = (lapMs % 60000) / 1000;
      uint32_t millisec = lapMs % 1000;
      lastLapTime = crossTime;
      Serial.printf("LAP! %02u:%02u.%03u\n", minutes, seconds, millisec);
    }
  }
  prevPos = pos;
}

void readGPS() {
  while (GPS.available()) {
    char c = GPS.read();
    if (c == '\n') {
      nmeaLine[idx] = 0;
      if (strstr(nmeaLine, "GNRMC") || strstr(nmeaLine, "GPRMC")) {
        parseRMC(nmeaLine);
      }
      idx = 0;
    } else if (idx < 127)
      nmeaLine[idx++] = c;
  }
}
void setup() {
  Serial.begin(115200);
  GPS.begin(115200, SERIAL_8N1, GPS_RX, GPS_TX);
  setupStartLine();  // 初期値
  Serial.println("GPS LapTimer");
}
void loop() {
  readGPS();
}

実際にサーキットを走ったりして確認するのは大変なので、まずは先日作ったシリアルロガーで、近所を歩いて一周してきたログをPythonでシリアルポートに流して動作確認をしてみたが、とりあえずは動いてそう。10Hzで取ったログを10倍速とかで流してもちゃんと反応してくれる。(Baudrate的に本当に10倍速になってるかはわからないけど…)
ちなみにMNEAセンテンスは絞ってる。

あとはディスプレイに表示できるようにしないと。屋外でも視認性が良さそうなディスプレイを探さないとな。あとはセクタータイム測定したり、GPSのデータをそのままロギングできたら便利そう。

ESP32でシリアルデータをロギングしてみた。

 先日ATGM336Hの動作確認ができたので、実際にGPSから出ているデータをロギングしてみることに。

今回はESP32とWebSocketを使って、ブラウザ上にデータを表示してそれをログデータとして保存してみようと思う。外でノートパソコンを持ってウロウロするのもアレだし…

#include <WiFi.h>
#include <WebServer.h>
#include <WebSocketsServer.h>

const char* ssid = "ESP32_LOG";
const char* password = "12345678";

WebServer server(80);
WebSocketsServer webSocket = WebSocketsServer(8080);

#define RXD2 32
#define TXD2 26 

const char MAIN_page[] PROGMEM = R"rawliteral(
<!DOCTYPE html>
<html>
<head>
<meta charset="UTF-8">
<title>ESP32 Serial2 Logger</title>
<style>
body { font-family: monospace; margin:20px; }
textarea {
  width:100%;
  height:400px;
}
button {
  font-size:18px;
  padding:8px 20px;
}
</style>
</head>
<body>
<h2>ESP32 UART2 Monitor</h2>
<textarea id="log"></textarea><br><br>
<button onclick="saveLog()">保存してダウンロード</button>
<script>
let ws;
function connectWS(){
  ws = new WebSocket("ws://" + location.hostname + ":8080/");
  ws.onmessage = function(event) {
    const ta = document.getElementById("log");
    ta.value += event.data;
    ta.scrollTop = ta.scrollHeight;
  };
  ws.onclose = function(){
    // 自動再接続
    setTimeout(connectWS, 2000);
  };
}
connectWS();
function saveLog() {
  const text = document.getElementById("log").value;
  const blob = new Blob([text], {type: "text/plain"});
  const url = URL.createObjectURL(blob);
  const a = document.createElement("a");
  a.href = url;
  a.download = "serial2_log.txt";
  a.click();
  URL.revokeObjectURL(url);
}
</script>
</body>
</html>
)rawliteral";

void setup()
{
  Serial.begin(115200);
  Serial2.begin(115200, SERIAL_8N1, RXD2, TXD2);
  WiFi.softAP(ssid, password);
  Serial.println("AP Started");
  Serial.println(WiFi.softAPIP());
  server.on("/", []() {
    server.send_P(200, "text/html", MAIN_page);
  });
  server.begin();
  webSocket.begin();
}

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

  static String line = "";

  while (Serial2.available())
  {
    char c = Serial2.read();
    //Serial.write(c);
    line += c;
    if (c == '\n')
    {
      webSocket.broadcastTXT(line);
      line = "";
    }
  }
}

こんな感じでとりあえずブラウザのtextareaに淡々と表示し続けて最後にsaveボタンを押す簡単なやつにしてみた。

10Hz設定だと流石に流れるのが早すぎるのか表示が追いついていない気がする。WiFi切ってもしばらく流れてるし。とりあえずiPhoneで保存したやつをPCに送って拡張子を.nmeaに変更。GSXSeeで開くとちゃんと軌跡が表示できた。

ATGM336HなGPSモジュールを買ってみた

 アリエクで安いGPSモジュールがあったので購入してみた。ATGM336HっていうシリーズのGPS。580円だった。AT6558が使われてるモジュールらしい。

とりあえずアンテナは小さいやつがついているのにしてみたけども、感度はどうなんだろう？

裏面にはホットスタート用のバッテリとEEPROM(24C32A)がついている。以前UBX-G7020のGPSモジュールを購入したときはEEPROMがついてなくて設定が保存できなかったので設定が保存できるのは便利そう。あと3.3VレギュレータのRT9193-33GBが付いているのでこれならVCCに5V入れても大丈夫そう。ただしTXとRXは3.3Vレベルなので注意。
アンテナのコネクタとバッテリが近いのでアンテナのコネクタがバッテリーに触らないように注意かも。テープでも貼っておくか…

とりあえず動作確認をしてみた。適当なシリアル変換アダプタに接続して、GnssToolKit3で動作確認ができる。室内だと窓際で1個補足できたぐらいでFIXすることはできなかった。

$GPTXT,01,01,02,MA=CASIC*27
$GPTXT,01,01,02,IC=AT6558F-5N-32-1C580900*06
$GPTXT,01,01,02,SW=URANUS5,V5.3.0.0*1D
$GPTXT,01,01,02,TB=2020-03-26,13:25:12*4B
$GPTXT,01,01,02,MO=GB*77

起動時にはこんな感じでバージョン情報とかが出る模様。

デフォルトのBaudrateは9600なんだけど、10Hzとかで動かしたら足りなくなるかもしれないということでBaudrateを変更してみた。

$PCAS01,5*19

をArduino IDEとかのシリアルコンソールからCR+CF付きで送信すると即座にBaudrateが115200に変更できた。もとに戻すには

$PCAS01,1*1D

を送ると9600に戻る。電源を切っても保持されるっぽいので設定時に一回送ればOK。

次にアップデートレートの変更だけども、10Hzに変更するには

$PCAS02,100*1E

のようにコマンドを送れば10Hz(100ms周期)でデータが送られてくる。1Hzに戻すには

$PCAS02,1000*2E

とすればOK。他にも色々変更できるっぽいけど、設定は先程のGnssToolKit3を使ったほうが便利かもしれない。GUIで色々いじれる。View→Configurationで設定画面が開ける。

バイク用にバンク角もログれるGPSロガーがほしいという知り合いがいたのでとりあえずNMEAのGPSなのであとは独自のセンテンスを追加してやれば一緒にログれそうな気がする。

MNEAセンテンスをロギング用にGGA + RMC + ZDAだけに絞るには

$PCAS03,1,0,0,0,1,0,1,0,0,0,,,0,0*03

このコマンドでおｋ。もとに戻す場合は

$PCAS03,1,1,1,1,1,1,1,1,0,1,,,1,0*02

こんな感じでもとに戻った気がする。そういえばSaveコマンドとか送らなくても全部保存されているようで、再起動してもそのままだった。

あとはツール側でそれを表示できるかどうか？とりあえずはGPXSeeっていうフリーソフトで試してみようかな。
まずは適当にログれるようなやつをM5 Atomあたりで作ってみようかな。

タミヤのスリムタイヤキットを自作してみる

 BLDCモータをベクトル制御できるようになったところで、やっぱり車体に搭載して動かしてみたいのでタイヤを作ってみることに。
タミヤのスリムタイヤキットはモータへの負担が小さく駆動率が高いということで、こういったダイレクトドライブモータとの相性が良いと思うんだけど、今回使用したモータに取り付けるのに加工しないといけないので、ホイールごとプリントしてもらえばいいかなと。

ホイールだけプリントしてスリムタイヤのゴムの部分だけ使っても良さそうだけど、実は大きなOリングをそのまま使えないかと言うことでOリングでやってみることに。(大学の時研究室でもOリングをタイヤにしていたし)
Oリングみたいなタイヤはモデル化のときとかに接地面が安定するので…

ということでOリングを使えるようなホイールの3Dモデルを作ってみた。スリムタイヤは横幅3.5mmっぽいのでOリングでも良い所ありそう。P-48だと外経も55mmっぽいしタミヤのスリムタイヤと同じ感じにできそう？
小さいほうがトルクが出そう？ということでP-46のOリングでホイールを設計してみた。

見た目がめっちゃプーリー。

モータとOリングを取り付けるとこんな見た目に。外経が52mmぐらいの予定。Oリングとぴったりサイズで作るとゆるゆるになったりするんじゃないかということで少し大きめに作ってOリングを引っ張るように調整しないといけないかも？

アリエクで線経が4mmで外径が52mmという少し太めのOリングを見つけてしまったのでとりあえず取り寄せてタイヤを作ってみようかな。250円で10本入りっぽいし。

ちなみにこのタイヤをユニバーサルプレートに装着して何かを作ろうとしてもキャスターのサイズが合わないのでキャスターとかも高さを調整する必要がありそう。まぁタミヤのギアボックス用にできてるもんな…

追記:
3Dプリントしてもらいました

BLDCモータドライバをCAN対応にしてみた。

 先日作ったFOC対応のBLDCモータドライバはCANトランシーバも積んでいるのでCANに対応させてみた。

こんな感じで電源とCANだけ接続してやればOK。I2Cみたいに束ねられるので配線が楽かも。マスター側のポートも1ポートで良いし。

基板設計時にもRS485かCANか迷ったけどとりあえずCANのほうがハードウェア側で色々やってくれるから楽なんじゃないかなということで。

とりあえずCANのプロトコルはOdrive風にしてみた。しかしOdriveとSimpleFOCだと単位が違うのが少し面倒。rpsとrad/sの変換を入れようとしてるけども、まずはSTM32F103でCAN通信をやりながらSimpleFOCでベクトル制御を動かせるかを試したいので値は変換しない状態でやってみようかと思う。

#include "SPI.h"
#include "SimpleFOC.h"
#include "SimpleFOCDrivers.h"
#include "encoders/MT6701/MagneticSensorMT6701SSI.h"
#include <EEPROM.h>
#include "stm32f103-can.h"

#define Protocol_Version 0x01
#define Hw_Version_Major 0x00
#define Hw_Version_Minor 0x00
#define Hw_Version_Variant 0x00
#define Fw_Version_Major 0x00
#define Fw_Version_Minor 0x00
#define Fw_Version_Revision 0x00
#define Fw_Version_Unreleased 0x00

#define NODE_ID_DEFAULT 0x000
#define NODE_BC 0x3F

uint16_t nodeID = 0x005;

#define MOTOR_POLE 7

#define CS1 PA15
#define SCK1 PB3
#define MISO1 PB4
#define MOSI1 PB5
#define DRV_EN PB12
#define TX3_SCL2 PB10
#define RX3_SDA2 PB11
#define CAN_RX PB8
#define CAN_TX PB9
#define V_CURR PA0
#define W_CURR PA1
#define U_CURR PA2
#define TEMP PA3
#define VOLTAGE PA4
#define LED_BUILTIN PC13

HardwareSerial Serial3(TX3_SCL2, RX3_SDA2);
SPIClass SPI_1(MOSI1, MISO1, SCK1);
MagneticSensorMT6701SSI sensor(CS1);
InlineCurrentSense current_sense = InlineCurrentSense(0.01, -50 * 4, W_CURR, V_CURR, U_CURR);
BLDCMotor motor = BLDCMotor(MOTOR_POLE);
BLDCDriver6PWM driver = BLDCDriver6PWM(PA8, PB13, PA9, PB14, PA10, PB15, DRV_EN);  //W V U EN

typedef union {  //IEEE 754 Float
  uint8_t uint[8];
  float value[2];
} FLOAT_BYTE_UNION;

uint8_t eepromRead(uint8_t reqID) {
  uint8_t lastData = 0xFF;
  for (uint16_t i = 0; i < EEPROM.length(); i += 2) {
    uint8_t dataID = EEPROM.read(i);
    uint8_t data = EEPROM.read(i + 1);
    //Serial1.println(dataID);
    if (dataID == reqID) {
      lastData = data;
      //Serial1.print("EEPROM Read:");
      //Serial1.println(i);
    }
    if (dataID == 0xFF | dataID == 0x00) {
      break;
    }
  }
  //Serial1.print("EEPROM Data:");
  //Serial1.println(lastData);
  return lastData;
}

void eepromWrite(uint8_t reqID, uint8_t data) {
  uint16_t lastID = 0xFFFF;
  for (uint16_t i = 0; i < EEPROM.length(); i += 2) {
    uint8_t eepromId = EEPROM.read(i);
    if (eepromId == 0xFF | eepromId == 0x00) {
      lastID = i;
      //Serial1.print("EEPROM Write:");
      //Serial1.println(i);
      break;
    }
  }
  if (lastID == EEPROM.length()) {
    lastID = 0;
  }
  EEPROM.write(lastID, reqID);
  EEPROM.write(lastID + 1, data);
}
void CANread() {
  if (CANMsgAvail()) {
    CAN_msg_t CAN_RX_msg;
    CANReceive(&CAN_RX_msg);
    uint16_t cmdID = uint16_t(CAN_RX_msg.id & 0x1F);
    uint16_t nodeIDrcv = CAN_RX_msg.id >> 5;
    CAN_msg_t CAN_TX_msg;
    memset(&CAN_TX_msg, 0, sizeof(CAN_msg_t));
    CAN_TX_msg.type = DATA_FRAME;
    CAN_TX_msg.format = STANDARD_FORMAT;
    CAN_TX_msg.id = ((nodeID << 5) | cmdID);
    if (nodeIDrcv == nodeID) {
      switch (cmdID) {
        // case 0x003:  //Get Motor Error
        //   break;
        // case 0x004:  //Get Encoder Error
        //   break;
        // case 0x005:  //Get Sensorless Error
        //   break;
        case 0x006:  //Set Axis Node ID
          if (CAN_RX_msg.type == DATA_FRAME) {
            uint32_t reqNodeID = CAN_RX_msg.data[0] | CAN_RX_msg.data[1] << 8 | CAN_RX_msg.data[2] << 16 | CAN_RX_msg.data[3] << 24;
            //Serial1.println(reqNodeID,HEX);
            if ((reqNodeID < 0x3F) && (reqNodeID != eepromRead(1))) {
              eepromWrite(1, reqNodeID);
              //Serial1.println("nodeID update");
            }
          }
          break;
        case 0x009:  //Get Encoder Estimates
          if (CAN_RX_msg.type == REMOTE_FRAME) {
            FLOAT_BYTE_UNION output;
            output.value[0] = motor.shaft_angle;
            output.value[1] = motor.shaft_velocity;
            memcpy(CAN_TX_msg.data, output.uint, 8);
            CAN_TX_msg.len = 8;
            CANSend(&CAN_TX_msg);
          }
          break;
        case 0x00C:  //Set Input Pos
          if (CAN_RX_msg.type == DATA_FRAME) {
            FLOAT_BYTE_UNION input;
            memcpy(input.uint, CAN_RX_msg.data, 8);
            motor.controller = MotionControlType::angle;
            target_angle = input.value[0];
          }
          break;
        case 0x00D:  //Set Input Vel
          if (CAN_RX_msg.type == DATA_FRAME) {
            FLOAT_BYTE_UNION input;
            memcpy(input.uint, CAN_RX_msg.data, 8);
            motor.controller = MotionControlType::velocity;
            target_angle = input.value[0];
          }
          break;
        case 0x00E:  //Set Input Torque
          if (CAN_RX_msg.type == DATA_FRAME) {
            FLOAT_BYTE_UNION input;
            memcpy(input.uint, CAN_RX_msg.data, 8);
            motor.controller = MotionControlType::torque;
            target_angle = input.value[0];
          }
          break;
        // case 0x014:  //Get IQ
        //   break;
        case 0x015:  //Get Temperature(FET IEEE 754 Float, Motor IEEE 754 Float)
          if (CAN_RX_msg.type == REMOTE_FRAME) {
            FLOAT_BYTE_UNION output;
            output.value[0] = (1.43 - analogRead(ATEMP) * 3.3 / 4095) / 0.0043 + 25;
            output.value[1] = analogRead(AVREF);
            memcpy(CAN_TX_msg.data, output.uint, 8);
            CAN_TX_msg.len = 8;
            CANSend(&CAN_TX_msg);
          }
          break;
        case 0x017:  //Get Bus Voltage and Current
          if (CAN_RX_msg.type == REMOTE_FRAME) {
            FLOAT_BYTE_UNION output;
            output.value[0] = analogRead(VOLTAGE) * 3.3 / 4095.0 * 4.0;
            output.value[1] = motor.current.q;
            memcpy(CAN_TX_msg.data, output.uint, 8);
            CAN_TX_msg.len = 8;
            CANSend(&CAN_TX_msg);
          }
          break;
        // case 0x01c:  //Get ADC Voltage
        //   break;
        default:
          break;
      }
    }
    if (CAN_RX_msg.type == REMOTE_FRAME) {
      switch (cmdID) {
        case 0x000:  //Get Version
          CAN_TX_msg.data[0] = Protocol_Version;
          CAN_TX_msg.data[1] = Hw_Version_Major;
          CAN_TX_msg.data[2] = Hw_Version_Minor;
          CAN_TX_msg.data[3] = Hw_Version_Variant;
          CAN_TX_msg.data[4] = Fw_Version_Major;
          CAN_TX_msg.data[5] = Fw_Version_Minor;
          CAN_TX_msg.data[6] = Fw_Version_Revision;
          CAN_TX_msg.data[7] = Fw_Version_Unreleased;
          CAN_TX_msg.len = 8;
          CANSend(&CAN_TX_msg);
          break;
        case 0x001:  //Heartbeat Message
          CAN_TX_msg.data[1] = nodeID >> 8;
          CAN_TX_msg.data[0] = nodeID;
          CAN_TX_msg.len = 8;
          CANSend(&CAN_TX_msg);
          break;
        default:
          break;
      }
    }
  }
}
float target_angle = 0;
Commander command = Commander(Serial3);
void doTarget(char* cmd) {
  command.scalar(&target_angle, cmd);
}

void doMotor(char* cmd) {
  command.motor(&motor, cmd);
}

void doAnalog(char* cmd) {
  if (cmd[0] == 'T') Serial3.println(analogRead(TEMP));
  else if (cmd[0] == 'V') {
    Serial3.print("Voltage: ");
    Serial3.println(analogRead(VOLTAGE) * 0.0032);
  }
};

void setup() {
  analogReadResolution(12);
  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(LED_BUILTIN, HIGH);
  Serial3.setTx(TX3_SCL2);
  Serial3.setRx(RX3_SDA2);
  Serial3.begin(115200);
  SimpleFOCDebug::enable(&Serial3);

  sensor.init(&SPI_1);
  motor.linkSensor(&sensor);
  driver.voltage_power_supply = 8;
  driver.init();
  motor.linkDriver(&driver);
  current_sense.linkDriver(&driver);
  // control loop type and torque mode
  motor.torque_controller = TorqueControlType::foc_current;
  motor.controller = MotionControlType::velocity;
  motor.motion_downsample = 0.0;

  // velocity loop PID
  motor.PID_velocity.P = 0.2;
  motor.PID_velocity.I = 20.0;
  motor.PID_velocity.D = 0.0;
  motor.PID_velocity.output_ramp = 1000.0;
  motor.PID_velocity.limit = 8.0;
  // Low pass filtering time constant
  motor.LPF_velocity.Tf = 0.01;
  // angle loop PID
  motor.P_angle.P = 20.0;
  motor.P_angle.I = 0.0;
  motor.P_angle.D = 0.0;
  motor.P_angle.output_ramp = 0.0;
  motor.P_angle.limit = 40.0;
  // Low pass filtering time constant
  motor.LPF_angle.Tf = 0.0;
  // current q loop PID
  motor.PID_current_q.P = 1.5;
  motor.PID_current_q.I = 15.0;
  motor.PID_current_q.D = 0.0;
  motor.PID_current_q.output_ramp = 1000.0;
  motor.PID_current_q.limit = 5.0;
  // Low pass filtering time constant
  motor.LPF_current_q.Tf = 0.005;
  // current d loop PID
  motor.PID_current_d.P = 1.5;
  motor.PID_current_d.I = 15.0;
  motor.PID_current_d.D = 0.0;
  motor.PID_current_d.output_ramp = 1000.0;
  motor.PID_current_d.limit = 5.0;
  // Low pass filtering time constant
  motor.LPF_current_d.Tf = 0.02;
  // Limits
  motor.velocity_limit = 200.0;
  motor.voltage_limit = 5.0;
  motor.current_limit = 2.0;
  // sensor zero offset - home position
  motor.sensor_offset = 0.0;
  // general settings
  // pwm modulation settings
  motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
  motor.modulation_centered = 1.0;

  motor.useMonitoring(Serial3);

  motor.linkCurrentSense(&current_sense);
  current_sense.init();

  motor.init();
  motor.sensor_direction = Direction::CW;
  motor.initFOC();

  command.add('T', doTarget, "target angle");
  command.add('M', doMotor, "motor");
  command.add('A', doAnalog, "analog read");
  motor.monitor_downsample = 0;
  //motor.monitor_variables = 0;

  Serial3.println(F("Motor ready."));
  Serial3.println(F("Set the target angle using serial terminal:"));

  bool ret = CANInit(CAN_250KBPS, 2);//PB8,PB9
  nodeID = eepromRead(1);
  if (nodeID >= 0x3F) {
    eepromWrite(1, NODE_ID_DEFAULT);
    nodeID = NODE_ID_DEFAULT;
  }
  CAN1->FMR |= 0x1UL;
  CAN1->FMR &= 0xFFFFC0FF;
  CAN1->FMR |= 0x1C << 8;
  CANSetFilter(0, 1, 0, 0, ((nodeID << 5) << 21), 0xFC000004);
  CANSetFilter(1, 1, 0, 0, ((NODE_BC << 5) << 21), 0xFC000004);
  CAN1->FMR &= ~(0x1UL);

  _delay(1000);
}

void loop() {
  motor.loopFOC();
  motor.move(target_angle);
  motor.monitor();
  command.run();
  CANread();
}

Odriveのほうもバージョンによってコマンドが変わっていたりするけどとりあえず使いそうなところだけ有効にしてみた。ノードIDはEEPROMに記録するようにした。同じファームを書いてあとからCANでノードIDを書き換えられる。ODriveではモードを切り替えないと現在のモード以外でのコマンドは受け付けないけど、モード切替なしにデータを受け取ったモードに自動で変更するようにしてみた。

CANのライブラリは以前フィルタを試したときに使わせてもらったスケッチと同じように元ネタのstm32f103.inoのsetupとloopを削除して"stm32f103-can.h"として別ファイルにして使用してみた。

いちいちCANでコマンドを入力するのが面倒なので簡易的なPythonツールを作成しようかなと思う。もしくはESP32で直接やってしまったほうが早いかなぁ？

あと通信が切れたときのためにモータの動作を止められるように通信Watchdogをつけておいたほうがいいかもしれないかなぁ。モータが回りっぱなしになってしまうし。