【倒立振子part30】タイマー割り込みについて

倒立振子 プログラム システム同定

前回、測定を行うのにサンプリングタイムをメイン文にdelayを使って実行していたが不均一でありそのdelayがカルマンフィルタのサンプリングタイムにも影響を与えているため正確な角度が測定できていないことが判明した。
そのため、メイン文でシステム同定のためのモータ入力と角度の測定を行い、タイマー割り込みで入力する値を読み込み+データの保存を行う。

タイマー割り込みについて

M5AtomMatrixへのタイマー割り込みには下記のブログを参考にさせていただいた。
logikara.blog

分からない単語があったのでそれについてまとめておく

排他制御

簡単に説明すると、「ブッキングしないようにするための制御する」ことらしい。
メイン文の制御とタイマー割り込みでの制御がタイミングによって混在しないようにするためのものらしい。

セマフォ

排他制御する際に割り込みが使えるかどうかを決めるフラグ的なやつ(割り込み制御において)
セマフォが0の時は割り込み制御中で1の時(解放されたとき)はメイン文に移行するというイメージ

プログラム

最終的なタイマー割り込みするプログラムの流れは以下のイメージで作っていこうと思う。

今回はまずタイマー割り込みのテストを兼ねてサンプリングタイムごとに時間測定を行いシリアル通信にて表示させる。
サンプリングタイムを1秒としてプログラムを作成した(参考ブログをそのまま流用して少し変形しただけだけ)

#include <M5Atom.h>

#define samp_time 1000 //[ms]

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;      //秒カウント用

// tim0のタイマー割り込みハンドラ。この処理はsetupより先に書く
// タイマ割込み発生時に実行される処理 (IRAM_ATTRで宣言してRAM上に配置）
void IRAM_ATTR onTimer() {
  portENTER_CRITICAL_ISR(&timerMux);  //排他制御で以下を実行（割込み禁止）
  if(start == 1) {                    //カウント開始フラグが1なら
    count++;                          //1msタイマカウンタ +1
    if (count >= samp_time) {               //1000回＝1秒経過していたら
      sec_up = 1;                     //1秒経過フラグをON (メイン処理でリセットする）
      count = 0;                      //1msタイマカウンタをリセット
    }
  }
  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);

  // タイマ割り込みサービス・ルーチン onTimer を登録
  timerAttachInterrupt(timer, &onTimer, true);
  
  // 割り込みタイミング1ms(1us × 10000)の設定
  timerAlarmWrite(timer, 1000, true);

  // タイマ有効化
  timerAlarmEnable(timer);

}

void loop() {
  M5.update();
  start = !start; //割り込み許可フラグ
  if (xSemaphoreTake(timerSemaphore, 0) == pdTRUE) {
    if (sec_up == 1) {
      //割り込み後に実行する内容
      sec++;
      Serial.println(sec);
      portENTER_CRITICAL(&timerMux);  //排他制御で以下を実行（割込み禁止）
      sec_up = 0;
      portEXIT_CRITICAL(&timerMux); //排他制御終了（割り込み許可）
    }
  }
  //Serial.println(sec);
  delay(5);
}

上手くカウントできたので割り込み制御はうまくいったと思う。

次回

システム同定できるようにプログラムを作っていく。