エンコーダ分解能を考慮してTeensy LCでIPD制御を行うプログラム

DC-motor-IPD-control-ver2.ino

//PWM出力に関しての詳細はhttps://www.pjrc.com/teensy/td_pulse.html
//エンコーダに関しての詳細はhttps://www.pjrc.com/teensy/td_libs_Encoder.html
//タイマー割込みに関しての詳細はhttps://www.pjrc.com/teensy/td_timing_IntervalTimer.html

//Teensyの標準ヘッダファイル
#include <Encoder.h>

//定数の宣言
#define GEAR  (32*33*35*38) / (15*14*13*10)//モータのギア比
#define PULSE 12.0  //エンコーダのパルス数
#define Ts    0.001 //サンプリング周期[s]
#define PWM   9     //PWM出力ピン
#define DIR   10    //モータ回転方向指定ピン
#define volt  5     //モータの電源電圧

//グローバル変数の宣言
float r = 0;//目標値[rad]
float val; //シリアル通信で送られてくる目標値[deg]
float table[618];//角度分解能テーブル

//クラスの宣言
IntervalTimer myTimer;  //タイマー割り込み
Encoder enc(11, 12);    //エンコーダ

void setup() {
  Serial.begin(115200);

  pinMode(PWM,OUTPUT);
  pinMode(DIR,OUTPUT);

  analogWriteFrequency(PWM, 46875);//モータドライバへ出力するPWMの周波数を変更
  analogWriteResolution(10);//PWMの分解能を変更

  //角度分解能のテーブルを作成
  for(int i=0;i<618;i++){
    table[i] = (i * 360) / (PULSE * GEAR);
  }

  myTimer.priority(200);//タイマーの優先度を128から200に変更 
  myTimer.begin(interrupt, Ts*1000000);//タイマー割り込み開始

}

void interrupt() {
  
  float Kp = 40.22, Ki = 672.5, Kd = 0.6012;//制御ゲイン
  static float y_old = 0;//過去の出力
  static float integral = 0;//積分
  float y;//出力[rad]
  float u;//入力[V]
  float e;//偏差
  float duty;//PWMの出力
  float cnt;//エンコーダのカウント

  cnt = enc.read();//エンコーダの値を取得
  y = (cnt * 2 * PI) / (PULSE * GEAR);//現在角度[rad]
  
  e = r - y;//偏差[rad]
  if(abs(e) < 0.58*2*PI/360) e = 0;//分解能以下の場合は偏差を0
  integral += e * Ts;//積分の計算
  u = -Kp*y + Ki*integral - Kd*(y-y_old)/Ts;//I-PD制御
  y_old = y;//出力を保持

  //入力制限とアンチワインドアップ制御
  if      (u >  volt) {u= volt; integral-=e*Ts;}
  else if (u < -volt) {u=-volt; integral-=e*Ts;}

  duty = u * 1023 / volt;//入力電圧をDutyに変換

  //モータへの出力
  if (duty >= 0)
  {
    digitalWrite(DIR, HIGH);
    analogWrite(PWM, (int)duty);
  }
  else {
    digitalWrite(DIR, LOW);
    analogWrite(PWM, (int)-duty);
  }

  //ログデータの出力
  Serial.print(val,4);
  Serial.print(" : ");
  Serial.print(u,4);
  Serial.print(" : ");
  Serial.println(y*360/(2*PI),4);

}

void loop() {

  //目標値の取得
  if(KICK_SERIAL())
  {
    val = SERIAL_VAL();//シリアル通信で送られてきた目標値[deg]

    //分解能を考慮した目標値への変換----------------
    float absVal = abs(val);
    float diff;
    float min_diff = abs(absVal - table[0]);
    int   min_i = 0;
    
    for(int i=1;i<618;i++){
        diff = abs(absVal - table[i]);
        if(min_diff > diff){
            min_diff=diff;
            min_i = i;
        }
    }
    if(val < 0) r = -table[min_i]*2*PI/360;
    else        r =  table[min_i]*2*PI/360;
    //-----------------------------------------   
  
  }
  delay(10);
}

bool KICK_SERIAL() {
  bool flag = false;
  if (Serial.available() > 0)
  {
    flag = true;
    //delay(20);
  }
  return flag;
}

int SERIAL_VAL() {

  byte data_size = Serial.available();

  byte buf[data_size], degree = 1;
  long recv_data = 0, dub = 1;
  bool minus = 0;

  for (byte i = 0 ; i < data_size ; i++)
  {
    buf[i] = Serial.read();

    if (buf[i] >= '0' && buf[i] <= '9') buf[i] -= '0';
    else {
      if (buf[0] == '-') minus = 1;
      else degree = 0;
    }
  }
  if (degree == 1) degree = data_size - minus;

  for (byte i = 0 ; i < degree ; i++)
  {
    recv_data += buf[(data_size - 1) - i] * dub;
    dub *= 10;
  }
  if (minus) recv_data *= -1;

  return recv_data;
}