Teensy LCでDCモータの周波数応答を測定するプログラム

DC-motor-frequency-response.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    //モータ回転方向指定ピン

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

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

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

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

  delay(5000);

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

void interrupt()
{
  float encCnt;         //エンコーダのカウント
  float omega;          //角速度[rad/s]
  int u_pwm;            //PWMのDuty比
  static float u = 0;   //入力
  static float t = 0;   //時間
  static float f = 0.1; //入力の周波数

  encCnt = enc.read();//カウント読み取り
  omega = (encCnt * 2 * PI) / (PULSE * GEAR * Ts);//角速度計算
  enc.write(0);//カウントリセット

  //ログデータの出力
  Serial.print(u*5,4);
  Serial.print(" : ");
  Serial.println(omega,4);

  t += Ts;//時間の更新
  
  u = sin(2 * PI * f * t);//入力の計算
  u_pwm = int(u * 1023);  //入力をPWMのDuty比に変換

  //モータドライバへの出力
  if (u_pwm >= 0)
  {
    digitalWrite(DIR, HIGH);
    analogWrite(PWM, u_pwm);
  }
  else {
    digitalWrite(DIR, LOW);
    analogWrite(PWM, -u_pwm);
  }

  //入力周波数の更新
  if (t > 1/f) {
    t = 0;
    if      (f < 1)  f += 0.1;
    else if (f < 10) f += 1;
    else if (f < 100)f += 10;
  }

  //入力周波数が100Hz以上になったら終了
  if (f >= 100) {
      analogWrite(PWM, 0);
      myTimer.end();
  }
}

void loop() {}