【倒立振子part32】タイマー割り込みでモータの角速度を取得する

前回、角度を取得したが今回はモータを回転させて角度を取得する。

プログラム

基本的には前回とほぼ同じ

#include <M5Atom.h>
#include "Kalman.h"

#define samp_time 50 //[ms]
#define data 10
#define ENC_A 22
#define ENC_B 19
#define stepsPerRotate 100
#define pi 3.14
#define PWM_pin 33
#define STOP_pin 23
#define Dirc_pin 32         //HIGHでCW,LOWでCCW

//タイマー割り込み用設定
hw_timer_t *timer = NULL; //タイマー0の割り込みtimerで定義
volatile SemaphoreHandle_t timerSemaphore;  //セマフォの宣言（割込み発生の確認用）
portMUX_TYPE timerMux = portMUX_INITIALIZER_UNLOCKED; //排他制御の利用を宣言

//タイマー割り込み用変数
volatile int count = 0;   //1msタイマカウンタ用
volatile int sec_up = 0;  //1秒経過フラグ用
// グローバル変数宣言
int start = 0;    //カウント開始フラグ
int sec = 0;      //秒カウント用
int sec_disp = 0;      //秒カウント用
float array_data[data];
int serial_frag = 0;  //シリアル通信用のフラグ
long count_time=0;

float dt, preTime,theta,pretheta,omega;
volatile int  enc_count = 1;
volatile const int CHATTERING_TIMER_ENCODER = 0 ;  // チャタリング回避タイマー 
int duty,a=0;


// tim0のタイマー割り込みハンドラ。この処理はsetupより先に書く
// タイマ割込み発生時に実行される処理 (IRAM_ATTRで宣言してRAM上に配置）
void IRAM_ATTR onTimer() {
  portENTER_CRITICAL_ISR(&timerMux);  //排他制御で以下を実行（割込み禁止）
  if(start == 1) {                    //カウント開始フラグが1なら
    sec_up = 1;                     //1秒経過フラグをON (メイン処理でリセットする）
  }
  portEXIT_CRITICAL_ISR(&timerMux);   //排他制御終了（割り込み許可）
  xSemaphoreGiveFromISR(timerSemaphore, NULL);  //セマフォを開放
}

//==============================================================
//初期設定関数
//==============================================================
void setup() {
  M5.begin(false, false, true);
  Serial.begin(115200);

  //タイマー割り込み用初期設定
  timerSemaphore = xSemaphoreCreateBinary();  //バイナリセマフォを作成(0か1のバイナリ)
  timer = timerBegin(0, 80, true);  // タイマ作成
  timerAttachInterrupt(timer, &onTimer, true);  // タイマ割り込みサービス・ルーチン onTimer を登録
  timerAlarmWrite(timer, (samp_time*1000), true);  // 割り込みタイミング1ms(1us × 10000)の設定
  timerAlarmEnable(timer);  // タイマ有効化

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

  pinMode(ENC_A, INPUT);
  pinMode(ENC_B, INPUT);
  pinMode(STOP_pin, OUTPUT);
  pinMode(Dirc_pin, OUTPUT);
  // ボタン押すまでカウントは進めない
  start = 0;
  // モータ制御の初期化
  digitalWrite(STOP_pin, LOW);
  //digitalWrite(Dirc_pin, LOW);
  ledcWrite(0, 255);  // モータ停止
  attachInterrupt(ENC_A, ENC_READ_A, CHANGE);
  attachInterrupt(ENC_B, ENC_READ_B, CHANGE);

}

//==============================================================
//メイン関数
//==============================================================
void loop() {
  M5.update();
  ENC_theta();
  count_time = micros();
    //オフセット再計算
  if (M5.Btn.isPressed()){
    start = !start;
    delay(1000);
    // モータ制御の初期化
    digitalWrite(STOP_pin, HIGH);
    ledcWrite(0, 255);  // モータ停止
  }

  digitalWrite(STOP_pin,HIGH);

  if (xSemaphoreTake(timerSemaphore, 0) == pdTRUE) {  //セマフォが解放されたら
    M5.dis.drawpix(20, 0x00ff00);
    if (sec_up == 1) {
      //割り込み後に実行する内容
      duty = sec*25;
      array_data[sec] =  theta;
      ledcWrite(0, duty);
      portENTER_CRITICAL(&timerMux);  //排他制御で以下を実行（割込み禁止）
      sec_up = 0;
      if(sec < 4){
        digitalWrite(Dirc_pin,LOW);
      }
      else{
        digitalWrite(Dirc_pin,HIGH);
      }
      Serial.print(count_time);
      Serial.print(",");
      Serial.print(sec);
      Serial.print(",");
      Serial.print(duty);
      Serial.print(",");    
      Serial.println(array_data[sec]);
      portEXIT_CRITICAL(&timerMux); //排他制御終了（割り込み許可）
      sec ++;
      if(sec == (data)){  //配列に0からdata-1までのdata個のデータが入ったとき
        start = !start;     //タイマー割り込みのカウントを止める
        sec = 0;            //配列を0にして再測定できるようにする
        digitalWrite(STOP_pin,LOW);
        ledcWrite(0, 255);
      }
    }
  }
  delay(5);
}

//==============================================================
//エンコーダ関数
//==============================================================
void ENC_READ_A(){
  enc_count += digitalRead(ENC_A) == digitalRead(ENC_B) ? -1 : 1;
  delayMicroseconds(CHATTERING_TIMER_ENCODER * 1000) ;
  //delay(1);
}

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

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;
}

解説

前回は1msごとにタイマー割り込みでカウントを増やしてサンプリングタイムごとにフラグを上げていたが、結局ズレが生じて正確な角度が取得できていなかった。
よく考えてみるとサンプリングタイム毎に割り込めばよかったので38行目から45行目に変更した。

void IRAM_ATTR onTimer() {
  portENTER_CRITICAL_ISR(&timerMux);  //排他制御で以下を実行（割込み禁止）
  if(start == 1) {                    //カウント開始フラグが1なら
    sec_up = 1;                     //1秒経過フラグをON (メイン処理でリセットする）
  }
  portEXIT_CRITICAL_ISR(&timerMux);   //排他制御終了（割り込み許可）
  xSemaphoreGiveFromISR(timerSemaphore, NULL);  //セマフォを開放
}

またボタンを押したときにモータが回転するように初期設定関数の69行目から73行目を追加した。

  start = 0;
  // モータ制御の初期化
  digitalWrite(STOP_pin, LOW);
  //digitalWrite(Dirc_pin, LOW);
  ledcWrite(0, 255);  // モータ停止

次回

次回はモータの入力を配列で読み取ってシステム同定まで進める。