Skip to content

モーターとエンコーダで角度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