本記事にはアフィリエイト広告が含まれます。

今年(2026年)に入ってから倒立振子ばかり作っております。

俺の倒立メソッドを他のモータでも

この過程でどんなモータでもどんな構成でも使える倒立メソッドを確立することができました (気のせいかもしれないが)。
モータに関してはDCモータ、サーボモータ、BLDC、QDDで検証し、
構成についてはエンコーダや足の有無などで確認しました。

このメソッドを皆さんにも検証いただくべくいろんな条件で、それこそ3Way 4wayで楽しめて構成も簡単なハードを模索していたのですが ….無理でした 🙄
やはりハードウェアはハードよ
どうしても条件変わるとややこしいはんだ付けが必要になったりIOピン足りなくなったりで。。。

そこでまずは連続回転サーボによる倒立振子で制御メソッドを楽しんでいただければと思います。
構成やソースコードを記載しますので是非お試しください。
フィードバックあると嬉しいです。

Atomic Motion Base

ここではコントローラとしてM5Stack社のATOM Matrixと Atomic Motion Base を採用します。

Atomic Motion Base はバッテリ搭載でサーボ (4ch) とDCモータ (2ch)を駆動できるATOMシリーズ用のユニットです。

今回はAtomic Motion Base に連続回転サーボを接続して倒立振子を構成します。

連続回転サーボ FS90R

サーボとしてFS90Rを使用して車輪倒立振子を製作します。

↑こちらの２個セットを購入してタイヤも付属のものを使用。

FS90Rはパルス幅1500usecで停止状態となり、そこを中心にパルス幅を増減させて正転/逆転させます。
増減の量でスピードを調整 (範囲 900～2100usec)

機体構成

Atomic Motion Base に ATOM Matrixを載せてサーボ FS90R を2個つなげます。

上図のとおり前方を定めてサーボR、LをAtomic Motion BaseのサーボポートS3、S1にそれぞれ接続します。

FS90Rは雑ですがATOM Matrixに両面テープで貼り付けただけです。

ATOM MatrixにはLEDで機体角度インジケータが表示されます。

X軸、Y軸の傾斜が表示されますが制御にはY軸のみ使用しています。

ソースコード

コードの環境や必要ライブラリは以下の通りです。
　Arduino IDEバージョン：ver. 1.8.19
　ESP32ライブラリ：ver. 2.0.13
　M5Atomライブラリ：ver. 0.1.3
　Kalman Filter Library ：ver. 1.0.2
　PS4Controllerライブラリ：ver. 1.1.0
　M5AtomicMotion：ver. 1.0.0

PS4Controllerライブラリのサイズが大きいのでESP32のフラッシュ領域設定は
“No OTA (Large APP)”にしてください。

servoPendulum/FS90R/FS90R.ino at main · homemadegarbage/servoPendulum

GitHub

ATOM MatrixをアクセスポイントとしてWiFiで接続してブラウザからゲイン等の設定が可能です。
走行はPS4コントローラで操作できます。

制御回り

    //モータ制御
    M = Kp / 10.0 * (kalAngleY + theta0_hat)
      + Kd / 500.0 * kalAngleDotY
      + Kw / 1000.0 * (vel - velCmdFiler);
    
    Tturn = Kyaw / 100.0 * (theta_Zdot - omegaCmd);
    ML1 = M - Tturn;
    MR1 = M + Tturn;

    ML1 = constrain(ML1, -1.0, 1.0);
    MR1 = constrain(MR1, -1.0, 1.0);


    //モータ回転速度
    vel = (ML1 + MR1) * 0.5;
    theta0_hat += Ki * 100.0 * (vel - velCmdFiler) * dt;
    theta0_hat *= 0.99;

//モータ制御

M = Kp / 10.0 * (kalAngleY + theta0_hat)

+ Kd / 500.0 * kalAngleDotY

+ Kw / 1000.0 * (vel - velCmdFiler);

Tturn = Kyaw / 100.0 * (theta_Zdot - omegaCmd);

ML1 = M - Tturn;

MR1 = M + Tturn;

ML1 = constrain(ML1, -1.0, 1.0);

MR1 = constrain(MR1, -1.0, 1.0);

//モータ回転速度

vel = (ML1 + MR1) * 0.5;

theta0_hat += Ki * 100.0 * (vel - velCmdFiler) * dt;

theta0_hat *= 0.99;

機体角度 (kalAngleY )、機体角速度(kalAngleDotY)、ホイール回転速度をフィードバックして倒立させます。

旋回動作(Tturn )はYaw軸角速度とのP制御

ホイール回転速度はエンコーダがないのでサーボへの速度指令値で代用しています。
さらにωを積算して目標倒立角度を調整する工夫も施しています (theta0_hat)。

動作

機体を起きあげてY軸の角度(ピッチ角)が±1.0°以内なると倒立制御が開始
±40.0°以上傾くとモータは停止

連続回転サーボ FS90R で倒立振子

制御はいつもの
m = Kp・θ ＋ Kd・θ̇＋ Kw・ω

エンコーダがないのでホイール回転速度ωは速度指令値mで代用
さらにωを積算して目標倒立角度を調整する工夫も施して
機体オフセットなどに対応 pic.twitter.com/wWQCAPBC32

— HomeMadeGarbage (@H0meMadeGarbage) June 6, 2026

ブラウザアプリ

ATOM MatrixをアクセスポイントとしてWiFi接続してブラウザからパラメータ調整が可能です。

機体を起動して、スマホもしくはPCのWiFi接続設定で以下に接続
　SSID： AtomMotionServo
　パスワード：password
ブラウザで ”192.168.55.90” にアクセス

FS90Rは停止点の調整をネジで調整可能ですが、アプリのOffsetL、OffsetRでも微調整可能です。

PS4コントローラ

本倒立振子はPS4コントローラで操作します。

　
ATOM Matrixの起動時に Bluetooth Mac Addressをシリアル出力させています。
アドレスを控えてコントローラと接続します。

PS4コントローラとESP32のペアリング方法の詳細は以下を参照ください。

・ESP32 (M5StickC) で PS4コントローラを使ってみた : Skyzoo ヨッシーの備忘録

Skyzoo ヨッシーの備忘録

ロボのBluetooth Mac Addressを書き込んだコントローラのホームボタン⌂を押すとペアリングされます。

PS4ペアリング後にはアプリによるWiFi接続ができませんので、両方実施したい場合はWiFiアプリ接続後にBluetoothコントローラペアリングしてください。

左ジョイスティックの上下で前後進、左右で旋回します。

シリアルサーボ SCS0009

シリアルサーボ SCS0009 を用いた車輪倒立振子も紹介させてください。

あまり知られてないのですがSCS0009 には連続回転モードがございます。

SCS0009 の連続回転モードは Address 9（Min Position Limit）と Address 11（Max Position Limit）を両方 0 にすることで設定され、回転速度は Time parameter で指定します。

ユーティリティソフトでAddress 9、Address 11を0に設定

　
更にここでは左サーボのID = 1、右を ID = 2として（Address 5）、
サーボとのシリアル通信ボーレートを1Mbpsとしました（Address 6 Baud Rate = 0）。

制御コードでは以下の記事の角度指定コードを Time parameterを設定するように改造してSCS0009 の連続回転モードを楽しめるようにしました。

送信のみで受信はできませんが半2重通信回路なしで手軽に駆動できます。

機体構成

Atomic Motion Base に ATOM Matrixを載せてサーボ SCS0009（両面テープ固定）を2個つなげます。

SCS0009は付属の分岐コネクタを加工してAtomic Motion Base のPORT.Bに接続しました。
サーボ信号線 GPIO = 33

ホイールにはタミヤのタイヤを使用

SCS0009とタイヤは3Dプリント品のコネクタで接続

サーボ付属の丸ホーンとコネクタを2×6mmタッピングネジ(2本)でねじ止め
タイヤはあらかじめ外側3つの穴をM3タッピングでねじ切りしておきM3×5mmネジ(3本)で固定します。

コネクタの3Dプリント用STLデータは↓コチラ

servoPendulum/wheelFix.stl at main · homemadegarbage/servoPendulum

GitHub

ソースコード

コードの環境や必要ライブラリは以下の通りです。
　Arduino IDEバージョン：ver. 1.8.19
　ESP32ライブラリ：ver. 2.0.13
　M5Atomライブラリ：ver. 0.1.3
　Kalman Filter Library ：ver. 1.0.2
　PS4Controllerライブラリ：ver. 1.1.0

PS4Controllerライブラリのサイズが大きいのでESP32のフラッシュ領域設定は
“No OTA (Large APP)”にしてください。