モーターとエンコーダで角度PID制御を作ろう!
ステップ1:基本構成を決める
まず、必要なものは?
✅ モーター制御 → Motor
✅ エンコーダ読み取り → Encoder
✅ PID制御 → Pid
✅ システムタイマ管理 → SYSTEM
使うクラスたちはこれ!
#include "stm32f4xx.h"
#include "sken_library/include.h"
// 必要なオブジェクトを作る
Motor motor;
Encoder encoder;
Encoder_data e_data;
Pid pid_control;
ステップ2:初期化を書こう
次に、初期化をまとめよう!
int main(void) {
sken_system.init(); // システム初期化
// モーター初期化
motor.init(Apin, B8, TIMER10, CH1); // Aピン(回転方向1)
motor.init(Bpin, B9, TIMER11, CH1); // Bピン(回転方向2)
// エンコーダ初期化
encoder.init(A0, A1, TIMER2); // エンコーダ(A相, B相)
// PIDゲイン設定
pid_control.setGain(5.0, 0.0, 0.5, 10); // (P, I, D, 微分フィルタ時定数)
while (1) {
// ここに制御を書く!
}
}
✅ ここまでで、モーターとエンコーダが使える状態になった
ステップ3:目標角度と制御を作ろう!
次に、角度制御の本体を書こう!
double target_angle = 90.0; // 目標角度 [度]
double control_output = 0; // 制御出力
// 1msごとに呼ばれる制御関数
void control_loop() {
encoder.interrupt(&e_data); // エンコーダ情報を更新
// 目標角度と現在角度の誤差からPID制御
control_output = pid_control.control(target_angle, e_data.deg, 1);
}
ステップ4:制御出力をモーターに反映しよう!
メインループに、モーター制御を書こう!
int main(void) {
sken_system.init();
motor.init(Apin, B8, TIMER10, CH1); // Aピン(回転方向1)
motor.init(Bpin, B9, TIMER11, CH1); // Bピン(回転方向2)
encoder.init(A0, A1, TIMER2);
pid_control.setGain(5.0, 0.0, 0.5, 10); // PID設定
sken_system.addTimerInterruptFunc(control_loop, 0, 1); // 制御ループ登録
while (1) {
motor.write(control_output); // PID出力をそのままモータに流す
}
}
✅ これで 「目標角度まで回す制御」 が完成!
全体完成版まとめ
#include "stm32f4xx.h"
#include "sken_library/include.h"
// 必要なオブジェクト
Motor motor;
Encoder encoder;
Encoder_data e_data;
Pid pid_control;
double target_angle = 90.0; // 目標角度 [度]
double control_output = 0; // 制御出力
// 1msごとに呼ばれる制御関数
void control_loop() {
encoder.interrupt(&e_data); // エンコーダ更新
control_output = pid_control.control(target_angle, e_data.deg, 1); // 目標角度と現在角度で制御
}
int main(void) {
sken_system.init();
motor.init(Apin, B8, TIMER10, CH1); // Aピン(回転方向1)
motor.init(Bpin, B9, TIMER11, CH1); // Bピン(回転方向2)
encoder.init(A0, A1, TIMER2); // エンコーダ初期化
pid_control.setGain(5.0, 0.0, 0.5, 10); // PIDゲイン設定
sken_system.addTimerInterruptFunc(control_loop, 0, 1); // 1msごとに制御実行
while (1) {
motor.write(control_output); // PID出力でモーターを動かす
}
}
注意
エンコーダーの回転方向とモーターの回転方向をデバックで確認する必要がある. しない場合,発散することがある
Note
著者:Shion Noguchi