← プロジェクト概要に戻る ミニ⑤ Stretch · Step1 OK · Step2 ゲート A/B OK Step1 増幅 OK ゲート A OK ゲート B OK（hold≈20 s）

面置きから揺れを増幅し、掴んで倒立する

ミニ⑤は面置きから揺れを増幅（Step1）し、直立付近でCATCH→PID（Step2）へ渡す段です。
Step1 は PUMP（キック ↔ Ka·sign(ω)·cos(e)）。 Step2 は CATCH 条件で既存 PID にラッチ。

2026年9月11日 ｜ slug rw-pendulum-standup ｜ env balance_atoms3 ＋ TX Tuner ｜ Step1: KwUI=2.0 ｜ Step2 本番: KwUI=2.7・motorDirSign=+1

ここまでの判定（2026-09-11）
Step1 合格 — 面→大揺れ→直立通過（Kw=2.0）。 グラフ ｜ ビデオ
ゲート A 合格 — PID 手置き（符号は motorDirSign=+1）。
ゲート B 合格 — PUMP→CATCH→PID で hold≈20 s（KwUI=2.7）。 Step2 グラフ

Chocofon（B）

面から揺れて、最後は立った！ 偽 CATCH もあったけど、2回目で掴めた。

AIチューター

増幅と掴みで符号を分けた（PID=+1、PUMP は式内で位相合わせ）。次は安定の質を上げる段。

管理人

Step2 は合格でクローズ。偽 CATCH 改善や hold 延長は Step3〜。

キック ↔ Ka·sign(ω)·cos(e)

PUMP はふたつの則を切り替える制御です。 どちらもリアクションホイールへの指令 u（PWM 相当）を出します。

状態条件（実装）指令ねらい
キック |ω − bias| < 8 °/s ±180 を約 280 ms ごとに左右反転 面置きで止まっているとき、最初の動きを作る
cos 則 上記以外（動いている） u = Ka · sign(ω−bias) · cos(e) · PUMP_LAW_SIGN · motorDirSign 角速度の向きに合わせて揺れを増幅する

なぜ sin では増幅できないか

寄せ（SIN）:  u = Ks · sin(e)          ← e だけを見る（寄せ）
増幅（PUMP）: u = Ka · sign(ω) · cos(e) ← ω の位相に乗る
  • 面置きでは e≈±54〜57° でほぼ一定 → sin(e) はほぼ一定の押し。左右に振れない
  • 増幅には「今どちらへ動いているか（ω）」が必要。だから sign(ω)
  • cos(e) は直立付近（e≈0）で効きを残し、大きく傾いたところで形を整える

記号の意味（初心者向け）

記号意味
e姿勢誤差（度）。ZERO で直立を 0 にしたあとの傾き
ωピッチ角速度（°/s）。CSV の omega
biasARM／PUMP 入室時の ω。面置きで生値が 15 前後でも「止まっている」と判定するため
Ka増幅ゲイン。TX の Kw×40（例: Kw 2.0 → Ka 80）
motorDirSign既定 +1（PID/ゲート A）。Serial m は逃げ用
PUMP_LAW_SIGN-1（増幅位相。PID と m を切り替えない）
切り替えのイメージ
止まっている → キックで軽く揺する → 一度動き出したら cos 則が「その方向へ」エネルギーを足す → 振幅が育ち、やがて直立を超える。エネルギーが足りない／多いは Kw（Ka） で調整。

実験方法

1. 書き込み

cd PlatformIO/PlatformIO_Projects/rw_pendulum_torque
pio run -e balance_atoms3 -t upload

2. ATOM Serial（必須）

キー内容
pPID → PUMP → SIN → SIN_D → PID（LCD に md PUMP）
mmotorDirSign トグル（通常不要。既定 +1）
5 / 6PUMP 中は Ka −/+（TX の Kw でも可）

3. TX Tuner 手順（推奨レシピ）

  1. LINK 確認 → Serial v（csv=ON）
  2. ZERO — 筐体を垂直倒立にしてターゲットを取る
  3. 面に寝かせる（e≈54〜57° 付近）
  4. エンコーダ #0＝Kw を 2.0 前後 → APPLY（CSV の kw が 2.00 になる）
  5. ARM — キック／増幅スタート（OUTER は触らない＝OFF）
  6. 振幅を観察。危険なら即 E-STOP／SAFE
  7. 記録後 v で csv=OFF。倒立に戻すときは ATOM p → PID

4. 見る欄（CSV）

t_ms, armed, e, omega, u, pwm, ww, kw
# EVENT,... APPLY / ARM / ESTOP
  • kw … APPLY 後に狙った値か（PUMP 中は Ka 換算の UI 値）
  • u … ±180 付近＝キック。それ以外は cos 則
  • e … 面→振幅拡大→0 通過→反対面、が増幅の証拠
安全
Ka が大きいと一回転する。最初は Kw≈2.0、机上・手の届く範囲で。E-STOP をすぐ押せる位置に。

Step1 結果 — CSV グラフ（Kw=2.0）

2026-09-11・PUMP・Serial m 後・Kw=2.0（Ka≈80）。 JC4827 の CSV（244 行）から作成。横軸は csv=ON からの秒。

PUMP Kw=2.0 の姿勢誤差 e の時間推移。面から揺れが増え直立を通過する
e（姿勢誤差） — 面（≈+56°）→ 振幅拡大 → 約 14 s で直立付近 → 反対面へ。 ESTOP 後は ≈−42° に落ち着く
e・omega・u の3段グラフ。ARM・キック・直立通過・ESTOP の位置を示す
上: e / 中: ω / 下: u。縦の点線は APPLY・ARM・動き出し・直立通過・ESTOP。 u の ±180 はキック

タイムライン（この試行）

おおよそ出来事読み取り
~1.5 sAPPLYkw 3.50 → 2.00
~3.5 sARMarmed=1。当面はキック待ち
~4.1 s動き出しu が非ゼロ、e が面から離れ始める
~14 s直立通過e≈0（ARM から約 10 s）
~22 sESTOPu=0。反対面付近へ収束

ゲインの学び

Kw（Ka）結果
~3.5（≈140）増幅は強いが一回転しやすい → ESTOP
2.0（≈80）面→大揺れ→直立通過。Step1 の目安帯
符号が逆揺れが増えない／押し戻される → Serial m

生 CSV（リポジトリ）: PlatformIO/.../rw_pendulum_torque/logs/kw20_pump_20260911.csv

Step1 結果 — ビデオ

同じ試行系の揺れ（面置きから増幅）。グラフの e の振れと見比べるとわかりやすいです。

PUMP 増幅デモ（YouTube）

角度の前提

項目値メモ
ターゲット e=0垂直倒立TX の ZERO
面置きおおよそ ±54〜57°ルーロー面
PID 安定帯（参考）±2°ミニ④ 評価
CATCH（Step2）|e| < 12° かつ |ω| < Wth初期 Wth=100 °/s（仮）

Step2 — 評価手順と実機判定

Step2 で知りたいのは増幅ではなく、直立付近で掴めるか／掴んだあと即倒れないか。 CATCH FW 実装済。実機はゲート A→B で評価し、両方合格（詳細グラフは 下）。

合格線（この段で止める）

項目内容
合格CATCH→PID に入り、すぐ倒れず 目安 3〜5 s 姿勢が保たれる
不合格の典型掴んだ直後に暴れる／一回転／すぐ SAFE
まだやらない（→ Step3〜）偽 CATCH 対策の本改修、hold 10 s、OUTER ON

CATCH 条件

if |e| < 12° && |omega| < Wth:
    → 通常 PID（既存 balance）※一度入ったらラッチ
else:
    → PUMP 継続（失敗しても SAFE にしない）
項目合意メモ
|e|12°骨格どおり。偽 CATCH 対策は Step3 候補
Wth 初期100 °/sStep1 CSV から
失敗時PUMP 継続SAFE に落とさない
OUTEROFF切替〜安定まで
motorDirSign+1PID 確定。PUMP は PUMP_LAW_SIGN=-1

ゲート A — 掴み単体

PUMP を外し、PID だけが健全か見る。

ゲート A 実機（2026-09-11） — 合格。 motorDirSign=+1（CSV hold≈11 s／目視≈20 s）。 誤って -1 のままでは即 ESTOP。

ゲート B — 本番（PUMP→CATCH→PID）

ゲート B 実機（2026-09-11・合格本） — 合格。
条件: KwUI=2.7（Ka≈108）・符号訂正後 FW・OUTER OFF。
偽 CATCH 1 回（e≈10.5°）→ abort → 本 CATCH（e≈0.1°）→ PID で hold≈20 s（手動 ESTOP）。 グラフへ
ゲート B・1 本目（参考・未達） — 符号が PID と不一致のとき CATCH 直後に暴れて ESTOP。符号訂正のきっかけ。

評価サマリ

項目結果
Step1 PUMPOK — KwUI=2.0、面→直立通過
ゲート A PIDOK — motorDirSign=+1
ゲート B CATCHOK — KwUI=2.7、hold≈20 s
学び（符号）PID=+1／増幅は式内 PUMP_LAW_SIGN=-1（m 切替不要）
学び（CATCH）頂点手前の一瞬 latch がありうる（|e|<8 や dwell は Step3 候補）

Step2 結果 — CSV グラフ（KwUI=2.7）

2026-09-11・ゲート B 合格本。PUMP→CATCH→PID。 緑帯は CATCH（PID）区間。点線は |e|=12（現行）と |e|=8（候補）。

ゲート B 全体: e・omega・u・CATCH。ARM 後に増幅し、CATCH 後に直立キープ、ESTOP まで約20秒
全体 — ARM → PUMP 増幅 → CATCH（緑）→ hold≈20 s → 手動 ESTOP。 約 7 s 付近に短い偽 CATCH（u スパイク）あり
CATCH 付近ズーム: 偽 CATCH（e=10.5）のあと本 CATCH（e≈0）で安定
CATCH ズーム — 偽 CATCH（e=10.5°・ω=47）は即 abort。 本 CATCH（e≈0.1°）から e が ±数度に収まる

タイムライン（この試行）

おおよそ出来事読み取り
~0 scsv ON面置き e≈55°、kw=2.70
~2.4 sARMPUMP 開始（キック→cos 増幅）
~6.9 s偽 CATCHe≈10.5°・ω≈47 → u 急増 → abort
~8.8 s本 CATCHe≈0.1° → PID ラッチ（out=2）
~29.4 sESTOPhold=20。手動停止

生 CSV（リポジトリ）: PlatformIO/.../rw_pendulum_torque/logs/gateb_kw27_20260911.csv

次 — Step3〜（安定の質）

管理人:
Step2（掴んで数秒〜20 s）は合格。次は偽 CATCH を減らす／hold を伸ばす／OUTER など。 いまの CATCH 条件（|e|<12・Wth=100）はそのままで記録済み。
  • 候補: CATCH の |e| 締め（例 8°）／連続 ms／Wth／切替時 u 制限
  • 候補: hold 目標の引き上げ、OUTER ON の再開
  • 逃げ: E-STOP／SAFE。戻すときは p → PID

正本: rw-pendulum-standup.md ｜ FW: src_balance_atom/main.cpp ｜ 評価: JC4827 評価 CSV ｜ 前: Stretch ①C