ミニ④：PID＋30秒キープ
角度が見える → 向きが合う → P → PD → I → 30秒 の順で進める。
学習ステップ
| Step | 内容 | 状態 |
|---|---|---|
| 0 | 用語・PWM = u | 済 |
| 1 | 角度（pitch・CW=+） | 済 |
| 2 | 相補フィルタ | 済 |
| 3 | 向き合わせ | 済（PWM180） |
| 4 | P 制御 | 済（Kp=45→60） |
| 5 | PD 制御 | 部分合格（Kp60／Kd2.2） |
| 6 | I 項 | 部分合格（60/2.2/1.0・printf OFF） |
| 7 | 30秒・ゲイン確定 | 合格（2026-08-12）→ 判定完了（2026-08-13） |
Step 0: 用語・PWM = u
物理ではトルク τ ≈ I·α。コードでは毎周期 PWM を u で上書きする。
PWM += u にすると、前の指令が残ってモータが暴走する。
この約束だけ先に決めて、あとの P／PD／I を同じ型で足していく。
使用したプログラム: src_balance_atom/main.cpp （Step0〜3 当時）
Step 1: 角度（pitch・CW=+）
倒す前に、ATOM の IMU で「今何度傾いているか」を出す。
加速度から atan2 で pitch を計算し、画面と Serial に出した。
符号は 時計回り（CW）を＋、反時計回り（CCW）を− に固定した。
軸を間違えると、あとでモータが「倒れる向き」に回ってしまう。
使用したプログラム:
src_balance_atom/main.cpp
（updateImu の加速度角）
- 軸は pitch。CW → P＋／CCW → P−
Step 2: 相補フィルタ
加速度だけの角度（Pa）はガタガタする。ジャイロだけを足していくと、だんだんずれる。
そこで 相補フィルタ を updateImu() に入れた。
毎周期、ジャイロで進めた角を 98%、加速度角を 2% 混ぜて滑らかな Pf にする。
式のイメージ: Pf = 0.98 × (Pf + ジャイロ×dt) + 0.02 × Pa。
ジャイロは Y 軸、gyroPitchSign = +1。
使用したプログラム: src_balance_atom/main.cpp （相補フィルタ追加時） ｜ 解説 IMU の基礎
- α = 0.98 で Pf が使える。符号も Step1 と一致
Step 3: 向き合わせ
PID はまだ入れない。傾いたとき「戻す向き」に短パルスを出すだけのプログラムにした。
倒れる側に回ったら m で motorDirSign を反転する。
パルスが弱いと方向が分からないので、PWM 180・150ms で確認した。
使用したプログラム: src_balance_atom/main.cpp （短パルス・BtnA で ARM/SAFE）
- PWM_PULSE 180（120 では方向が分かりにくかった）
- motorDirSign +1 で戻す向き OK
- BtnA = ARM/SAFE ／
z= ゼロ ／m= 向き反転
Step 4: P 制御
誤差 e = 今の角 − ゼロ点 に比例した指令を出す。
毎周期 u = Kp · e を計算し、その絶対値を PWM に上書きする（足し込みしない）。
向きは Step3 の motorDirSign をそのまま使う。
使用したプログラム: src_balance_atom/main.cpp （P 制御・Kp 調整）
- Kp 45 ／
|e|≈3°で PWM < 180 - CW／CCW 行き来あり（戻せるが止まりにくい）→ PD へ
Step 5: PD 制御
P だけだと行き来が止まらない。そこでジャイロの角速度 ω をブレーキに使う。
毎周期 u = Kp · e + Kd · ω。傾く速さに比例して逆向きの PWM を足す。
Serial の [ ] と , . で Kp／Kd をその場で変えた。
使用したプログラム: src_balance_atom/main.cpp （PD・Kd 項）
- Kp 60／Kd=0 で
|e|≈7°シーソー可 - Kd 2.2 で約1分保持の瞬間あり
eはマイナス偏り・FWD 多め → 左右対称とはまだ言えない → I へ
Step 6: I 項
PD でも角が片側に残る。そこで誤差を時間で足した積分を使う。
毎周期 u = Kp·e + Kd·ω + Ki·∫e。PWM は上書きのまま。
モータが飽和しているときは積分を止める（anti-windup）。
Serial の - = で Ki を変える。
使用したプログラム: src_balance_atom/main.cpp （PID +I・定期 printf 既定 OFF）
- Kp 60／Kd 2.2／Ki 1.0／単一 loop
- 定期
[ST]OFF で 60秒以上・ぴくぴくなし - printf ありだと 40秒＋周期的なぴくぴく（再現済）
- ホイール片側回転は残る → Step7
AIチューター調査
倒立が下手に見えても、中身は別物が3つ重なっていた。 I で立てる話と、ループを盗む話と、ホイールが片側だけ回る話は分けて考える。
現象
| # | 見た目 | いつ出たか |
|---|---|---|
| ① | 約1秒ごとのぴくぴく | 制御ループで [ST] の Serial.printf を出したとき。止めると消える（A/B 再現） |
| ② | 10秒持たない・全体が崩れる | 画面と Serial を Core 0、IMU＋PID を Core 1 にしたとき |
| ③ | 安定してもホイールが片側だけ回る | printf OFF で 60秒安定しても残る。Step5 の角度偏りと同じ系統 |
「ハードが劣化したのでは」も疑ったが、同じ筐体でログの有無だけ変えて①が再現したので、 ぴくぴくの主因は配線劣化ではない。
測定結果
倒立はせず、モータを止めたままループ時間だけ測った
（env loop_bench_atoms3）。目標周期 5ms、各条件 5秒。
| 条件 | 平均 | 最大（仕事） | 5ms 超過 | 読み |
|---|---|---|---|---|
| P0 素の制御 | 656 µs | 888 µs | 0 | IMU＋計算は十分速い |
P1 [ST] 1Hz |
668 µs | 1264 µs | 4（printf 4回） | 1回あたり約 +0.6ms |
| P2 画面全消し 10Hz | 1218 µs | 11.2 ms | 49 | 100ms ごとに約 11ms 止まる |
P4 [ST] 2Hz |
673 µs | 1253 µs | 9（printf 9回） | 超過回数が周期に追従 |
予想と違った。いちばん大きい穴は
printf ではなかった。
素の制御は 0.65ms。ログ1行は約 0.6ms 足すだけ。
画面の全消し（fillScreen）が約 11ms で、5ms 周期を2回分盗む。
ただしログの回数と「キック」の回数はぴったり揃う。だから1秒おきのぴくぴくのリズムは、やはり Serial 側。
推定原因
- ① ぴくぴく — ATOM の Serial は USB CDC。同じループで待つと、短い遅れでも1秒おきの外乱になる。 Kd がそれを拾って周期的なぴくぴくに見える。 「USB が何十ms も固まる」より、小さい周期外乱だった可能性が高い。
- ② 二重コア — 計算を分けること自体は正しい。壊れたのは IMU と画面を別コアから同時に触ったこと。 ミニ④では使わない。将来の LCD チューナでは、無線だけを別コア・共有は数値のコピー、なら使える。
- ③ 片側回転 — ゼロが頂点から少しずれると、P も I も同じ向きの力を出し続ける。 I で持ち時間は伸びるが、片側加速は残る。遅延とは別の話。Step7 でトリム確認。
遅延ベンチ:
src_loop_bench/main.cpp
（env loop_bench_atoms3）
｜ 正本の詳細: docs/hardware/rw-pendulum-loop-jitter.md
Step 7: 30秒キープ
L1 のゴールは「離して 30秒」。Step6 のゲインはそのままに、次を足した。
- ARM 中は画面を止める（
fillScreenが約 11〜21ms 止まるため） |e|<8°が 30秒続いたら[HOLD] 30s OKを1回出す9/0で目標角を ±0.2° ずらす（片側回転の手当て）- 仕事時間が 1.2ms を超えたら
[JIT]（今後の改定で倒す級の遅れが出たときの保険）
使用したプログラム: src_balance_atom/main.cpp （30秒保持・ARM 中 LCD オフ・トリム） ｜ 現行: main の最新
- ゲイン確定 60 / 2.2 / 1.0。ARM 中は画面オフ。
[HOLD] 30s OK再現 - best 106.8s / 114.6s（トリム無し）
- 片側回転は残る。トリムで回転は抑えられ、向きは反転する。ただし常に一方向
- この FW では
t/vでも倒すほどの遅延は出ない。理由: ARM 中は画面を止めたので、残るのは printf の +0.4ms 級だけ（0.20〜0.64ms）。以前のぴくぴくは画面の穴（11〜21ms）が乗っていた - WARN 1.2ms は、今後プログラムを変えて倒すほどの遅れが出たときの保険として残す
- 管理人判定 GO → L1 完走（⑤／⑥は Stretch）
Stretch ④: LiPo 完全自立
USB ケーブルの張力がゼロ点をずらすので、LiPo のみ・Serial なしで倒立できるかを確認した。 起動時に直立させて自動ゼロ → BtnA で ARM。WiFi は起動しない（常時 ON だと倒立が崩れた）。
LiPo 自立デモ（YouTube） — USB 非接続・机上 60秒以上
- USB 非接続、Serial なしで ARM
- 机上 60秒以上
- ミニ④’（片側回転・段A/B・機械 CoG）記録済み → 下記
- 本フェーズはここでいったんクローズ
ミニ④’：片側回転状況調査
ミニ④（机上30秒）合格後も残ったホイール片側回転を、ソフト診断・疑似 bias・機械重心の順で切り分けた。 L1 合格条件外の Stretch 調査。正本 rw-pendulum-wheel-bias.md。
使用したプログラム:
src_balance_atom/main.cpp
（env balance_atoms3。trim 9/0、bias b・Kbias 1/2）
用語
| 用語 | 意味 | 操作 |
|---|---|---|
| trim（トリム） | 倒立の目標角をゼロ点から少しずらす量（度）。 重心やゼロ点が幾何の直立とずれるとき、PID の目標を機械側に合わせて片側加速を弱める。 向きの反転・回転の抑制はできるが、「左右行き来」にはならない。 | Serial 9 / 0（±0.2°） |
| Kbias（Kb） |
段Bの疑似ホイール速度除去の強さ。
モータ指令 u をゆっくり積分した wheelBias を引き戻す係数
（u_cmd = u − Kbias · wheelBias）。
ホイール角速度センサは無いので近似。上げすぎると倒立と喧嘩しやすい。
|
b ON/OFF、1 / 2（±0.01） |
実施内容
| 段 | 内容 | 操作・条件 |
|---|---|---|
| A | 診断マトリクス | trim 0 / 最小 / Ki=0。ゲイン固定 60／2.2／1.0。t/v OFF |
| B | センサ無し bias 除去 | b ON、1/2 で Kbias（0.02→0.05）。指令 u の積分を引き戻す |
| 機械 | 糸吊りで CoG 確認 | TOP 穴に糸。マット縦線＝鉛直。右おもり 1.8g → 1.2g |
左右バランス調整（機械）
モーター固定用ボルトの重さを変え、機械的な重心軸を合わせた。
結果
| 段 | 結果 | メモ |
|---|---|---|
| A | 段B へ | trim≈1.8° で |u| 最小。Ki=0 でも片側 → I 主因ではない。トリムは向き反転のみ |
| B | 限界・未合格 | Kb=0.04〜0.05 で一時回転ゼロ可。やがて ゼロ↔一方向。係数追い込み打ち切り |
| 機械 | CoG≈中立 | 1.2g 固定。trim=0 で 30秒+。e≈0 でも u が一符号に張り付き |
- 静的な左右偏りは機械調整でほぼ潰せる（右 1.2g・trim=0 で倒立可）
- それでもホイールは一方向に溜まる → 外側自由度不足（ωw→0 の外ループが無い）
- ソフト疑似 bias（段B）では両方向行き来まで届かない → 段C（エンコーダ）待ち
次にやること
ミニ④ 判定 GO。机上30秒キープ＝L1 完走を認める。片側回転は残件だが合格条件外とする。
現時点の完成は Stretch ④ LiPo 完全自立（USB 非接続・Serial なしで ARM・60秒以上）とする。ミニ④’で段A/B・機械 CoG まで記録済み。本フェーズはここでいったんクローズする。
学び: USB ログ取得と係数調整がボトルネック → 開発環境（実験ツール）整備が次の前提。
次: LCD TX に着手。Stretch 順: LCD TX → エンコーダ → 自律立ち上がり → 260 vs 130 比較。各段は管理人 GO。
正本:
rw-pendulum-balance.md
｜ コード:
src_balance_atom/main.cpp
前: ミニ③ 小型化・電流
｜ 次: プロジェクトまとめ（動画）
｜ プロジェクト概要