3軸リアクションホイール倒立振子を作るアドカレ の23日目です。
1. はじめに：数字の羅列から「直感」へ
前回（Day 22）では、STM32G4の「クリスタルレスUSB」機能を駆使して、従来のUART比100倍以上の速度を誇る爆速テレメトリ通信を確立しました。
しかし、いかに通信が速くても、ターミナルソフトに流れるログはこのような無機質な数字の羅列に過ぎません。
12.34, -5.67, 120.5
12.40, -5.50, 120.6
...
これらの数字だけを追いかけて、「あ、今ロール軸が傾いて、ピッチ軸が起き上がろうとしてるな」と直感的に理解するのは困難です。倒立振子のような高速な力学系の制御調整において、「現在の姿勢を直感的に把握すること」 は必須要件です。
そこで本日は、PC上で動作するビジュアル開発環境 Processing を使い、手元のセンサ（ICM-42688）と完全に同期して動く3Dモデル（デジタルツイン） を画面上に召喚します。
2. 魔法のツール「Processing」の導入
Arduino界隈ではおなじみの Processing を使用します。
Javaベースの言語ですが、ウィンドウの生成から3D図形の描画までをわずか数行で記述できる、非常に強力なクリエイティブコーディング環境です。
- 導入手順:
- 公式サイトへアクセス: processing.org/download
- ダウンロード: お使いのOS（Windows/Mac/Linux）版をダウンロードします。
- インストール: processing-4.4.10-windows-x64.msi(←私が使用したバージョン)をインストールするだけで使用可能となります。
3. STM32側の実装 (ICM-42688 + EKF)
ハードウェア構成は以下の通りです。
- MCU: STM32G474RE
- Sensor: TDK InvenSense ICM-42688 (SPI接続)
- Cable: Day 22で作成した魔改造USBケーブル
IMUセンサは低ノイズで高性能な ICM-42688 を使用しています。SPI通信による高速バースト読み出しで生データを取得し、STM32内部で拡張カルマンフィルタ(EKF)を用いて姿勢角を計算、その結果をUSB CDC経由でPCへ送信します。
メインループのコード
main.c の主要部分です。センサからのデータ取得、物理量変換、EKFによる姿勢推定、そしてUSB送信までの一連の流れがここに集約されています。
/* USER CODE BEGIN WHILE */
while (1)
{
// --- 1. 6軸データ一括取得 (Burst Read) ---
// SPI通信で加速度(6byte)+ジャイロ(6byte)を一気に読み出す
uint8_t raw[12];
SPI_Read_Burst(REG_ACCEL_DATA_X1, raw, 12);
// --- 2. データの結合 (Big Endian -> 16bit int) ---
int16_t ax_raw = (int16_t)((raw[0] << 8) | raw[1]);
int16_t ay_raw = (int16_t)((raw[2] << 8) | raw[3]);
int16_t az_raw = (int16_t)((raw[4] << 8) | raw[5]);
int16_t gx_raw = (int16_t)((raw[6] << 8) | raw[7]);
int16_t gy_raw = (int16_t)((raw[8] << 8) | raw[9]);
int16_t gz_raw = (int16_t)((raw[10] << 8) | raw[11]);
// --- 3. 物理量への変換 ---
// Accel: ±16G (default) -> 2048 LSB/g
float ax_g = (float)ax_raw / 2048.0f;
float ay_g = (float)ay_raw / 2048.0f;
float az_g = (float)az_raw / 2048.0f;
// Gyro: ±2000dps (default) -> 16.4 LSB/dps
// オフセット補正と rad/s への変換
float gx_dps = ((float)gx_raw / 16.4f) - gyro_offset_x;
float gy_dps = ((float)gy_raw / 16.4f) - gyro_offset_y;
float gz_dps = ((float)gz_raw / 16.4f) - gyro_offset_z;
float gx_rad = gx_dps * 0.01745329f; // deg -> rad 変換
float gy_rad = gy_dps * 0.01745329f;
float gz_rad = gz_dps * 0.01745329f;
// ==========================================
// 拡張カルマンフィルタ (EKF) 更新
// ==========================================
// DWTサイクルカウンタで前回の更新からの経過時間dtを精密に計算
uint32_t current_tick = DWT->CYCCNT;
float dt = (float)(current_tick - last_tick) / SystemCoreClock;
last_tick = current_tick;
// EKFの状態更新ステップを実行
EKF_Update(&ekf, gx_rad, gy_rad, gz_rad, ax_g, ay_g, az_g, dt);
// Processing用にCSV形式で送信 (Roll, Pitch, Yaw)
// EKF構造体内部で既にクォータニオンからオイラー角(Degree)に変換済み
printf("%.2f,%.2f,%.2f\r\n", ekf.Roll, ekf.Pitch, ekf.Yaw);
HAL_Delay(16); // 描画レート(約60fps)に合わせて待機
}
4. Processing側の実装 (受信＆描画)
PC側では、STM32から送られてきた「Roll, Pitch, Yaw」のデータストリームを受け取り、それをリアルタイムで3D空間上のオブジェクト（箱）の回転に適用します。
import processing.serial.*;
Serial myPort; // シリアルポート
float roll = 0; // ロール角
float pitch = 0; // ピッチ角
float yaw = 0; // ヨー角
void setup() {
size(800, 600, P3D); // 3D描画モード
// シリアルポートの一覧を表示
printArray(Serial.list());
// ★重要★ 自分のポート番号に合わせてインデックス[0]を変更してください
// Windowsの場合、"COMx" がリストの何番目にあるか確認します。
// 分からなければ [0], [1]... と順番に試せばOKです。
String portName = Serial.list()[1];
myPort = new Serial(this, portName, 115200); // ボーレートはUSB CDCなら何でもOK
// データ区切り文字（改行コード）まで読み込む設定
myPort.bufferUntil('\n');
}
void draw() {
background(30); // 背景色（ダークグレー）
// ライト設定（立体的に見せる）
lights();
directionalLight(255, 255, 255, 0, 0, -1);
// 画面中央に移動
translate(width/2, height/2, 0);
// ★重要★ 座標系の変換
// Processingの座標系に合わせて回転させます
// 軸の向きが実機と逆なら、マイナスをつけたり入れ替えたりして調整してください
rotateX(radians(pitch)); // ピッチ (X軸回転)
rotateZ(radians(roll)); // ロール (Z軸回転 ※ProcessingはYが下、Zが奥行き)
rotateY(radians(yaw)); // ヨー (Y軸回転)
// Cubliっぽい箱を描く
noStroke();
// 本体（メタリックな色）
fill(100, 200, 255);
box(200, 200, 150); // 幅, 高さ, 厚み (mmイメージ)
// 向きがわかるように「目印」をつける
translate(0, -120, 0); // 少し上に移動
fill(255, 100, 100); // 赤色
sphere(20); // 小さい球
}
// シリアルデータを受信したときに呼ばれる関数
void serialEvent(Serial p) {
try {
// 改行まで読み込む
String inString = p.readString();
if (inString != null) {
inString = trim(inString); // 空白除去
String[] data = split(inString, ','); // カンマで分割
// データが3つ（Roll, Pitch, Yaw）揃っているか確認
if (data.length >= 3) {
roll = float(data[0]);
pitch = float(data[1]);
yaw = float(data[2]);
// デバッグ表示（コンソールに出力）
println("R:" + roll + " P:" + pitch + " Y:" + yaw);
}
}
} catch (Exception e) {
e.printStackTrace();
}
}
※技術的な補足：可視化におけるジンバルロックについて
鋭い方はお気づきかもしれませんが、STM32内部ではクォータニオンで計算していますが、PCへの送信時にオイラー角に戻しています。
そのため、もし倒立振子を垂直に90度持ち上げた場合、Processingの画面上ではジンバルロック（表示の荒ぶり）が発生する可能性があります。
しかし、今回はあえてオイラー角で送っています。理由は2つです。
- 直感的なデバッグ: クォータニオン（）の数字を見ても人間には姿勢が想像できませんが、オイラー角なら「あ、30度傾いてる」と瞬時に分かります。
- 制御範囲: 倒立振子の制御範囲（ 程度）ではジンバルロックは発生しません。
「完全なデジタルツイン」を目指すならクォータニオンを直接送るべきですが、「開発ツール」としては人間が理解しやすいオイラー角の方が実用性が高いという判断です。
5. 動作デモ：実機と画面がリンクする快感
実際にシステムを稼働させた様子です。
ブレッドボード上のセンサモジュールを傾けると、画面の中の青い箱がまるで目に見えない糸で繋がっているかのように、全く同じ動きをします。- 遅延ゼロ: USB Full Speed (12Mbps) 通信の恩恵により、操作に対する遅延（ラグ）は体感レベルで全く感じません。
- 高安定性: 拡張カルマンフィルタ(EKF)を通しているため、センサ特有のジッター（微振動）が綺麗に取り除かれており、静止時はピタッと止まります。
6. まとめ
たったこれだけの作業で、開発環境の質は劇的に進化しました。
- ジンバルロックの可視化: 計算上の特異点で挙動が破綻していないか、一目で確認できます。
- フィルタ特性の評価: センサを振った時に、画面の動きが遅れてついてくるのか（遅延）、過敏に反応しすぎるのか（ノイズ）が視覚的に判断できます。
「見えないものは制御できない」。
これで、Cubliの挙動を完全に把握する「目（センサ）」と、それを映し出す「鏡（デジタルツイン）」が整いました。
明日は Day 24。いよいよソフトウェア開発の総仕上げ、「コンパイルなしでPIDゲインを書き換えるCLI（コマンドラインインターフェース）ツール」 の実装に挑みます！
アドベントカレンダー参加中
STM32×AIで「3軸倒立振子」を作る25日間（ひとりアドカレ）Advent Calendar 2025