3軸リアクションホイール倒立振子を作るアドカレの24日目です。
Day 23までの成果により、Cubliの姿勢（Roll, Pitch, Yaw）をPC画面上でリアルタイムに可視化することが可能となりました。
しかし、制御システム開発において最も時間を要するのは、アルゴリズムの実装そのものではなく、その後の 「パラメータ調整（Tuning）」 です。
倒立振子の安定化制御にはPID制御則が用いられますが、最適なゲイン（$K_p, K_i, K_d$）は数理モデル上の計算値だけでは決定できません。実機の摩擦や応答遅れを考慮した、現物合わせの微調整が不可欠です。
従来の「ソースコード変更 → コンパイル → フラッシュ書き込み」というサイクルでは、1回の試行に数分を要し、効率が著しく低下します。
そこで本日は、稼働中のSTM32に対してUSB経由でコマンドを送信し、再ビルドなしで制御パラメータを即座に書き換える**「コマンドラインインターフェース (CLI)」**を実装します。
1. 課題設定：PID制御とパラメータ調整の即時性
PID制御の操作量 $u(t)$ は以下の式で表されます。
$$u(t) = K_p e(t) + K_i \int e(t) dt + K_d \frac{d e(t)}{dt}$$
ここで、比例ゲイン $K_p$、積分ゲイン $K_i$、微分ゲイン $K_d$ の3つのパラメータは、システムの極配置や応答特性を決定する重要な係数です。
これらを調整する際、従来の手順では以下のオーバーヘッドが発生していました。
- コード修正:
float Kp = 1.5f; - ビルド: 約1〜2分
- 書き込み & リセット: 約30秒
- 動作確認: 「発振したため失敗」→ 1.へ戻る
このリードタイムを 0秒 に短縮することが、本日の目標です。
2. 実装上の技術的課題：USB割り込みと送信処理の衝突
CLIの実装は、基本的には「UART/USBからの文字列受信」「解析」「実行」のプロセスですが、STM32のUSB CDCミドルウェアを使用する場合、特有の排他制御の問題が発生します。
直面した問題：送信データの消失
受信割り込みコールバック関数内で、コマンド解析と応答メッセージの送信（printf）を行おうとしたところ、「コマンド処理は実行されるが、PC側に応答が返ってこない」 という現象が発生しました。
原因：割り込みコンテキストでの競合
STM32のUSB CDCライブラリにおいて、データ受信は割り込みコンテキスト（ISR: Interrupt Service Routine）である CDC_Receive_FS 関数内で処理されます。
一方、応答に使用する CDC_Transmit_FS 関数は、USB周辺回路（USB IP）のステータスを確認し、Ready状態であれば送信を開始します。
受信割り込みが発生している最中は、USB IPは「受信処理中（Busy）」の状態にある場合があります。このタイミングでISR内から送信関数を呼び出すと、リソース競合が発生し、送信リクエストが拒否（または破棄）されてしまいます。
解決策：受信と実行の分離（Deferred Execution）
この問題を解決するため、「受信処理」と「コマンド実行処理」を明確に分離する設計を採用しました。
-
受信（ISR内）: 受信した文字をリングバッファ（または配列）に格納し、改行コード（
\rまたは\n）を検出したら「コマンド受信フラグ」を立てるのみとする。 -
実行（メインループ内）:
while(1)ループ内でフラグをポーリング監視し、フラグが立っていた場合のみコマンド解析と応答送信（printf）を行う。
これにより、USBの受信トランザクションが完全に完了した後で送信処理を行うことが保証されます。
3. 実装コード解説
上記の設計に基づき実装した主要なコードを示します。
① usbd_cdc_if.c (受信フック)
受信割り込み関数 CDC_Receive_FS には、自作の1文字処理関数 CLI_Process_Char を呼び出すフックのみを追加します。
static int8_t CDC_Receive_FS(uint8_t* Buf, uint32_t *Len){
/* USER CODE BEGIN 6 */
extern void CLI_Process_Char(uint8_t c); // プロトタイプ宣言
// 受信したデータ長分だけループを回し、1文字ずつ処理へ回す
for(uint32_t i=0; i<*Len; i++){
CLI_Process_Char(Buf[i]);
}
/* USER CODE END 6 */
USBD_CDC_SetRxBuffer(&hUsbDeviceFS, UserRxBufferFS);
USBD_CDC_ReceivePacket(&hUsbDeviceFS);
return (USBD_OK);
}
② main.c (受信バッファリング処理)
ここで入力文字をバッファに蓄積し、エンターキーの検出を行います。ここでは重たい処理（文字列解析やprintf）を行わないことが重要です。
#define RX_BUFFER_SIZE 64
char rx_buffer[RX_BUFFER_SIZE];
uint8_t rx_index = 0;
volatile uint8_t command_ready_flag = 0;
void CLI_Process_Char(uint8_t c) {
if (c == '\r' || c == '\n') {
// 改行検出：文字列終端を付加し、処理フラグを立てる
rx_buffer[rx_index] = '\0';
rx_index = 0; // 次回のためにリセット
command_ready_flag = 1;
} else {
// バッファリング（オーバーフロー防止付き）
if (rx_index < RX_BUFFER_SIZE - 1) {
rx_buffer[rx_index++] = c;
}
}
}
③ main.c (メインループでの実行)
メインループ内でフラグを回収し、コマンド実行関数を呼び出します。
/* USER CODE BEGIN WHILE */
while (1)
{
// ... (センサ取得や姿勢制御の処理) ...
// --- CLI コマンド処理 ---
if (command_ready_flag == 1) {
command_ready_flag = 0; // フラグクリア
CLI_Execute(rx_buffer); // コマンド解析と実行
}
// ... (テレメトリ送信など) ...
}
④ main.c (コマンド解析と実行)
文字列を区切り文字（スペース）で分割し、コマンドに応じた処理を実行します。文字列から浮動小数点数への変換には標準ライブラリの atof 等を使用します。
void CLI_Execute(char *cmd_line) {
char *argv[10]; // 引数は最大10個
int argc = 0;
// 文字列をスペースで分割 (strtok)
char *token = strtok(cmd_line, " ");
while (token != NULL && argc < 10) {
argv[argc++] = token;
token = strtok(NULL, " ");
}
if (argc == 0) return; // 空なら何もしない
// --- コマンド分岐 ---
// 1. help
if (strcmp(argv[0], "help") == 0) {
printf("=== Cubli CLI Help ===\r\n");
printf(" led <on/off/toggle> : Control LED\r\n");
printf(" motor <on/off> : Safety Switch\r\n");
printf(" pid <p> <i> <d> : Set PID Gains\r\n");
printf(" reset : Reset Sensor Angle\r\n");
printf(" mode <pid/lqr/ai> : Switch Control Mode\r\n");
printf(" status : Show Parameters\r\n");
}
// 2. led
else if (strcmp(argv[0], "led") == 0) {
if (argc < 2) { printf("Usage: led <on/off/toggle>\r\n"); return; }
if (strcmp(argv[1], "on") == 0) HAL_GPIO_WritePin(GPIOA, GPIO_PIN_5, GPIO_PIN_SET);
else if (strcmp(argv[1], "off") == 0) HAL_GPIO_WritePin(GPIOA, GPIO_PIN_5, GPIO_PIN_RESET);
else if (strcmp(argv[1], "toggle") == 0) HAL_GPIO_TogglePin(GPIOA, GPIO_PIN_5);
printf("LED Updated.\r\n");
}
// 3. motor
else if (strcmp(argv[0], "motor") == 0) {
if (argc < 2) { printf("Usage: motor <on/off>\r\n"); return; }
if (strcmp(argv[1], "on") == 0) {
motor_enable = 1;
printf("Motor ENABLED (Danger!)\r\n");
} else {
motor_enable = 0;
printf("Motor DISABLED (Safe)\r\n");
}
}
// 4. pid
else if (strcmp(argv[0], "pid") == 0) {
if (argc < 4) { printf("Usage: pid <kp> <ki> <kd>\r\n"); return; }
Kp = atof(argv[1]);
Ki = atof(argv[2]);
Kd = atof(argv[3]);
printf("PID Set: P=%.4f, I=%.4f, D=%.4f\r\n", Kp, Ki, Kd);
}
// 5. reset
else if (strcmp(argv[0], "reset") == 0) {
request_sensor_reset = 1; // メインループ内で処理させるフラグを立てる
// またはここで直接 EKF_Init(&ekf) などを呼んでも良い
printf("Sensor Reset Requested.\r\n");
}
// 6. mode
else if (strcmp(argv[0], "mode") == 0) {
if (argc < 2) { printf("Usage: mode <pid/lqr/ai>\r\n"); return; }
if (strcmp(argv[1], "pid") == 0) {
current_mode = MODE_PID;
printf("Mode: PID Control\r\n");
}
else if (strcmp(argv[1], "lqr") == 0) {
current_mode = MODE_LQR;
printf("Mode: LQR (Experimental)\r\n");
}
else if (strcmp(argv[1], "ai") == 0) {
current_mode = MODE_AI;
printf("Mode: AI/RL (Not Implemented)\r\n");
}
else {
printf("Unknown mode. Use: pid, lqr, ai\r\n");
}
}
// 7. status (確認用)
else if (strcmp(argv[0], "status") == 0) {
printf("--- Status ---\r\n");
printf("Mode: %d (0:Stop, 1:PID, 2:LQR, 3:AI)\r\n", current_mode);
printf("Motor: %s\r\n", motor_enable ? "ON" : "OFF");
printf("PID: %.4f, %.4f, %.4f\r\n", Kp, Ki, Kd);
}
// 8. monitor (ログ出力切替)
else if (strcmp(argv[0], "monitor") == 0) {
if (argc < 2) {
printf("Usage: monitor <on/off>\r\n");
return;
}
if (strcmp(argv[1], "on") == 0) {
monitor_flag = 1;
printf(">> Monitoring START\r\n");
}
else if (strcmp(argv[1], "off") == 0) {
monitor_flag = 0;
printf(">> Monitoring STOP\r\n");
}
}
else {
printf("Unknown command: %s. Type 'help'.\r\n", argv[0]);
}
}
}
4. 実装したコマンド一覧
効率的なデバッグを行うため、以下のコマンドセットを定義しました。
| コマンド | 引数 | 説明 |
|---|---|---|
help |
なし | 利用可能なコマンド一覧を表示します。 |
led |
on / off / toggle
|
基板上のLEDを制御します。CLIの疎通確認用として有用です。 |
pid |
**Kp Ki Kd**
|
本機能の核です。制御ゲインを即時書き換えます。 |
monitor |
on / off
|
センサデータの連続送信（テレメトリ）を制御します。コマンド入力時はOFFにしてログの洪水を防ぎます。 |
status |
なし | 現在設定されているパラメータ変数の値を一覧表示します。 |
特に monitor コマンドが重要です。普段はログ出力を停止しておき、必要な時だけ monitor on することで、通信帯域を圧迫することなくコマンド入力が可能になります。
5. 動作デモ
実際にPCのターミナルソフト（TeraTerm等）からNucleoボードを操作している様子です。
🎥 動画の技術的ハイライト
- 疎通確認 (led toggle):
- 動画の最初で
led toggleを実行しています。 - コマンド送信後、基板中央の小さな緑色LED (LD2) が点灯・消灯しているのが分かります（少し見えにくいですが、確実にPCからの命令で動いています）。
- モニタリング制御 (monitor on/off):
- コマンドを打った瞬間、センサの値（Roll, Pitch, Yaw）が高速で流れ始めます。
- 手で基板を揺らすと、数値が変動しているのが確認できます。これにより、CLIの操作性とデータの可視化を両立しています。
- パラメータのホットスワップ (pid ...):
- ログを一度止め、
statusで現在のゲインを確認。 -
pid 5.5 0.1 2.0と打ち込み、再度statusを確認すると、即座に値が反映されています。 - コンパイル時間ゼロ秒で、制御特性を変更できるようになった瞬間です。
6. まとめ
本日の実装により、Cubli開発における必須サブシステムが出揃いました。
- 目 (Day 22/23): 6軸センサ + EKF + 3D可視化
- 口 (Day 22): クリスタルレスUSBによる高速通信
- 耳 (Day 24): CLIによる対話型パラメータ調整
これらを統合することで、「姿勢を可視化しながら、リアルタイムにPIDゲインを調整し、その結果を即座に確認する」 という、理想的な開発サイクルが完成しました。
明日はいよいよ最終日、Day 25。
3Dプリンタで出力した物理筐体にこれら全てのシステムを組み込み、ブラシレスモータを駆動させ、Cubliを大地に立たせ……たい！！
……と、ここで一つ正直なご報告があります。 当初は完全な3次元倒立を目指していましたが、スケジュールの都合上、3軸同時の制御は間に合いそうにありません💦 申し訳ありません！
なので、まずは**「1軸（辺倒立）」**での動作を目指して頑張ります。 正直なところ、その1軸ですらうまく立つかどうか、やってみないと分からない状況ですが……（汗）。 とにかく動くところまで持っていけるよう、ラストスパートで全力の悪あがきをしてみます！ どうか温かい目で見守っていただければ幸いです！
アドベントカレンダー参加中
STM32×AIで「3軸倒立振子」を作る25日間（ひとりアドカレ）Advent Calendar 2025