ATOM Matrix の MPU6886 で Y 軸傾斜角をカルマンフィルタ推定し、角度と角速度の PD 制御で RobStride 05 を速度モード駆動する Arduino スケッチです。
詳細はコチラ https://homemadegarbage.com/codex01
- MCU: M5Stack ATOM Matrix
- Motor: RobStride 05, ID
0x7e - CAN transceiver: SN65HVD230 / VP230
- CAN speed: 1 Mbps
- CAN TX: GPIO32
- CAN RX: GPIO26
- スケッチを書き込む
- PC またはスマートフォンを Wi-Fi
ATOM-RS05-IDに接続する - パスワード
pass1234を入力する - ブラウザで
http://192.168.4.1/を開く
ATOM-RS05-IDとpass1234はサンプル値です。必要に応じてスケッチ内のAP_SSID/AP_PASSを変更してください。
M5AtomKalmanFilterby TKJ Electronics: https://github.com/TKJElectronics/KalmanFilter
- 速度指令:
speed = -(Kp * angleY + Kd * gyroY - Kw * wheelSpeed) gyroY: カルマンフィルタのバイアス補正後レートKp/Kd/Kw: ブラウザで0.0から1.0まで0.05刻みで指定wheelSpeed: RobStride 05 の0x701B(mechVel) から読んだホイール回転速度- 速度指令の上限:
+/-50 rad/s - フィードバック結果は符号反転して速度指令にします
- モータ Enable 条件:
|angleY| <= 1.0 degかつ IMU 正常 - モータ Disable 条件:
|angleY| >= 30.0 degまたは IMU 異常 1.0 degから30.0 degの間では直前の Enable 状態を保持します- CAN 指令更新周期:
500 us, 2000 Hz - 起動後
1000 ms待ってから RobStride 05 の速度モード設定を送信します
回転方向が逆の場合は、Kp / Kd / Kw の符号を反転してください。
- 表示範囲:
-20 degから+20 deg - 緑:
+/-1 deg以内 - 赤:
+/-15 deg以上 - 青: その他
- LED 輝度:
M5.dis.setBrightness(20)
GET /api/state: 角度、角速度、ホイール速度、Kp/Kd/Kw、速度指令、CAN/IMU状態を返しますGET /api/cmd?kp=0.6:Kpを設定しますGET /api/cmd?kd=1.0:Kdを設定しますGET /api/cmd?kw=0.8:Kwを設定します