ArduinoでPID制御する（位置/速度制御）

(2020.09.14 がっつり記事を書き直しました)

# PID制御の説明

DCモーターの位置や速度の制御をPIDで行います。
位置制御の場合、DCモータの角度をエンコーダで読み取り、
取得した現在の角度と、目標の角度の差を偏差と呼びますが、

偏差にある一定の値（Kp）をかけたことを比例(P : proportional)と呼びます。
さらに、計算するごとに偏差を足していくことを積分(I : integral)と呼び、
一回前に計算した偏差と、現在計算している偏差の差を微分(D : differential)と呼びます。

PID制御では、これら比例, 積分, 微分に対して、それぞれにある一定の値(Kp, Ki, Kd)をかけて、足し合わせた値をモーターへの入力とします。
つまり、Kp*P + Ki*I + Kd*Dです。

まあ、あとは実際にプログラムを見るのが手っ取り早いかと思います。

# 用意するもの

• Arduino
• DCモータ
• モータドライバ 今回は（）を使います。

# まずは位置のP制御

以下のようなプログラムがP制御を行います。
以下の部分、MotorDriverとかEncoderとかいうクラスは存在しないのでエラーがでることに注意してください。

MotorDriver motor_driver();
Encoder encoder();

// 比例ゲインKp
float Kp = 0.6;

// 目標角度[rad]
float target = 3.14;

MotorDriver motor_driver();
Encoder encoder();

void setup()
{
  motor_driver.init();
  encoder.init();
}

void loop()
{
  // エンコーダから現在の角度を取得
  float current_rad = encoder.getEncoderRadians();
  float err = target - current_rad;
  float P = Kp * err;
  motor_driver.setMotorSpeed( P );
}

# 位置のPID制御

float Kp = 0.6;       // 比例ゲインKp
float Ki = 0.1;       // 比例ゲインKi
float Kd = 0.1;       // 比例ゲインKd
float target = 3.14;  // 目標角度[rad]

MotorDriver motor_driver();
Encoder encoder();

void setup()
{
  motor_driver.init();
  encoder.init();
}

void loop()
{
  static float integral = 0;
  static float last_err = 0;
  static unsigned long last_micros = 0;
  
  float current_rad = encoder.getEncoderRadians();    // エンコーダから現在の角度を取得
  float err = target - current_rad;                   // 偏差を計算
  float P = Kp * err;                                 // Pを計算
  unsigned long current_micros = micros();            // 現在の時間を取得
  float dt = ((float)(current_micros - last_micros))
              / 1000000.0;                            // 経過時間を計算
  integral += err * dt;                               // 偏差の積分を計算
  float I = Ki * integral;                            // Iを計算
  float diff = (err - last_err) / dt;                 // 偏差の微分を計算
  float D = Kd * diff;                                // Dを計算
  motor_driver.setMotorSpeed( P+I+D );                // PIDでモータを制御
  last_err = err;                                     // 現在の偏差を保存
  last_micros = current_micros;                       // 現在の時間を保存
}