【倒立振子part8】エンコーダから角度、角速度の算出

前回エンコーダの値を測定し何回転したのかを測定した。
今回はそれを元に角度、角速度の算出を行う。

プログラム

#include "M5Atom.h"
 
#define ENC_A 22
#define ENC_B 19
#define stepsPerRotate 100
#define pi 3.14
#define PWM_pin 25

float dt, preTime,theta,pretheta,omega;
volatile int  enc_count = 1;
volatile const int CHATTERING_TIMER_ENCODER = 0 ;  // チャタリング回避タイマー 
 
void setup() {
  M5.begin(false, false, true);
  Serial.begin(115200);

  ledcSetup(0, 20000, 8);
  ledcAttachPin(PWM_pin, 0);

  pinMode(ENC_A, INPUT);
  pinMode(ENC_B, INPUT);
 
  attachInterrupt(ENC_A, ENC_READ_A, CHANGE);
  attachInterrupt(ENC_B, ENC_READ_B, CHANGE);

}
 
void loop() {
  ENC_theta();
  ledcWrite(0, 125);  //duty比50% = 255*0.5
}

void ENC_READ_A(){
  enc_count += digitalRead(ENC_A) == digitalRead(ENC_B) ? -1 : 1;
  delayMicroseconds(CHATTERING_TIMER_ENCODER * 1000) ;
}

void ENC_READ_B(){
  enc_count += digitalRead(ENC_A) == digitalRead(ENC_B) ? 1 : -1;
  delayMicroseconds(CHATTERING_TIMER_ENCODER * 1000) ;
}

void ENC_theta(){
  dt = (micros() - preTime) / 1000000;
  preTime = micros(); 
  theta = 2 * pi * enc_count / (4 * stepsPerRotate);
  omega = (theta - pretheta) * (60/2/pi) / dt ;
  pretheta = theta;

  Serial.println(omega);
  delay(10);
}

4逓倍でカウントしているため、変数enc_countが400になった際に1周している判定となる。
角度はカウント数を400で割ってでかけることで算出する。(単位は[rad])
角速度はとして計算できる。
ここで[d\theta]は現在角度-前回割り込み角度として導出、は現在時間-前回割り込み時間として算出できる。

整合性の確認

プログラムの整合性を確認するために実測とプログラムでのシリアル通信の結果を比較を行う。
角度については一周したときの角度、角速度は軸を回転計で確認を行う。
回転計は
https://www.amazon.co.jp/gp/product/B00XZYHEL6/ref=ppx_yo_dt_b_search_asin_title?ie=UTF8&psc=1
を使用する。
シリアル通信での測定結果：1485～1500rpm
回転計での測定結果：1495rpm
でありほぼ同等なので問題ないと判断する。

次回

今回のプログラムを使用してブラシレスDCモータの同定試験を行う。