こんにちは。koraです。
前回、HM-StarterKitのエンコーダとジャイロセンサを使えるようにしましたので、今回はタイマ割り込み機能を使ってこれらの値を読み込みます。
タイマ割り込みの設定
一定周期でエンコーダとジャイロの値を読み取り、車体の状態を更新する割り込み関数を実装します。タイマ割り込み自体の初期設定は、以前作成した、init.cファイルのinit_cmt関数でされています。今回使用するのはCMT0~2のうちのCMT2です。また、割り込んだ後の処理を追加するため、以下のファイルを編集します。
- intprg.c
- interrupt.c
intprg.c
他のソースファイル(interrupt.c)にある関数を参照するため、externを使ってプロトタイプ宣言します。
extern int_cmt2();
そして以下の行を、
void Excep_CMT2_CMI2(void){ }
次のように書き換えます。
void Excep_CMT2_CMI2(void){ int_cmt2(); }
interrupt.c
2つのファイルをインクルードするよう指定します。
#include “parameters.h” #include "spi.h"
そして、サンプルプログラムStep7のinterrupt.cから、int_cmt2関数をコピー&ペーストします。この関数でエンコーダの値の取得、車体の速度更新、距離積分、およびジャイロの値の取得、車体の角度の積分が行われます。
グローバル変数の設定
割り込みで読み込まれた値を、グローバル変数を介してメインプログラムに渡します。そのためのグローバル変数を用意します。ここでは次のファイルを編集します。
- glob_var.h
- static_parameters.h
- parameters.h
glob_var.h
サンプルプログラムを参考にエンコーダとジャイロのグローバル変数を作りますが、ついでに車体速度、距離、タイマ、制御用などのグローバル変数もつくります。なお、ここで使用している「GLOBAL」については以前のものと同じです。
//車体速度系のグローバル変数 GLOBAL float fast_speed; //最短走行時の最高速度 [m/s] GLOBAL float tar_ang_vel; //目標角速度 [rad/s] GLOBAL float tar_degree; //目標角度 [deg] GLOBAL float max_degree; //旋回時の最大角度 [deg] GLOBAL float start_degree; //走行進入時の車体角度 [deg] GLOBAL float ang_vel; //現在角速度 [rad/s] GLOBAL float p_ang_vel; //過去角速度 [rad/s] GLOBAL float max_ang_vel; //最高角速度 [rad/s] GLOBAL float ang_acc; //角加速度 [rad/ss] GLOBAL float accel; //加速度 [m/ss] GLOBAL float max_speed; //最高速度 [m/s] GLOBAL float speed_r; //現在の右タイヤ速度 [m/s] GLOBAL float speed_l; //現在の左タイヤ速度 [m/s] GLOBAL float speed_old_r; //右タイヤの過去の速度 [m/s] GLOBAL float speed_new_r; //右タイヤの最新の速度 [m/s] GLOBAL float speed_old_l; //左タイヤの過去の速度 [m/s] GLOBAL float speed_new_l; //左タイヤの最新の速度 [m/s] GLOBAL float speed; //現在車体速度 [m/s] GLOBAL float p_speed; //過去の車体速度 [m/s] GLOBAL float tar_speed; //目標車体速度 [m/s] GLOBAL float end_speed; //終端車体速度 [m/s] GLOBAL float V_r; //右モータの出力電圧 [V] GLOBAL float V_l; //左モータの出力電圧 [V] //エンコーダ角度系のグローバル変数 GLOBAL unsigned int angle; //現在の車軸角度 [deg] GLOBAL unsigned int locate_l; //現在の車軸位置 [無次元] GLOBAL unsigned int locate_r; //現在の車軸位置 [無次元] GLOBAL unsigned int before_locate_r; //過去の車軸位置 [無次元] GLOBAL unsigned int before_locate_l; //過去の車軸位置 [無次元] GLOBAL int diff_pulse_r; //車軸位置の微分値(車軸の回転速度[pulse/ms]) GLOBAL int diff_pulse_l; //車軸位置の微分値(車軸の回転速度[pulse/ms]) //タイマ系グローバル変数 GLOBAL unsigned int timer; //1mSごとにカウントアップされる変数. //ジャイロ系のグローバル変数 GLOBAL float gyro_x; //ヨー軸ジャイロの現在の値 [無次元] GLOBAL float gyro_x_new; //ヨー軸ジャイロの最新の値 [無次元] GLOBAL float gyro_ref; //よー軸ジャイロのリファレンス値[無次元] GLOBAL float degree; //現在の車体角度 [degree] //車体移動距離系のグローバル変数 GLOBAL float len_mouse; //マウスの移動距離 [mm] GLOBAL float len_target; //マウスの目標移動距離 [mm] //制御用グローバル変数 GLOBAL float I_tar_speed; //目標速度のI成分 GLOBAL float I_speed; //実速度のI成分 GLOBAL float I_tar_ang_vel; //目標角速度のI成分 GLOBAL float I_ang_vel; //実角速度のI成分
static_parameters.h
static_parameters.hには定数になるパラメータを記述します。
#define PI (3.141592653589793) //円周率
parameters.h
新しくファイルを作成します。parameters.hには値を調整するパラメータを記述します。
#ifndef _PARAMETER #include "static_parameters.h" //物理的なパラメータ #define TIRE_DIAMETER (12.8) //タイヤの直径 [mm] #define TIRE_RADIUS (TIRE_DIAMETER/2.0) //タイヤの半径 [mm] #define MMPP (TIRE_DIAMETER*PI)/(ENC_RES_MAX) //エンコーダ1パルスあたりに進む距離[mm](TIRE_DIAMETER*PI)/(ENC_MAX) #define ENC_RES_MAX (1024) #define ENC_RES_HALF (512) #define _PARAMETER #endif
ジャイロとエンコーダの初期設定
ジャイロセンサの値から角速度を求めるには、車体が静止した状態の値を差し引く必要があります。そのため、静止状態の値を取得して保存する関数が必要です。次のファイルを編集します。
- init.h
- init.c
init.h
関数のプロトタイプ宣言を追加します。
void gyro_get_ref(void);
init.c
関数の定義を追加します。
/***************************************************************************************** ジャイロのリファレンス取得 *****************************************************************************************/ void gyro_get_ref(void){ long i = 0; float gyro_ref_temp = 0; gyro_ref = 0; //ジャイロのリファレンス取得 for(i = 0; i < 2500; i++){ gyro_ref_temp += (float)gyro_x_new; wait_ms(1); } gyro_ref = (gyro_ref_temp/2500.0); degree = 0; wait_ms(100); }
また、init_all関数にジャイロの初期設定を追加します。
init i =0; preprocess_spi_gyro_2byte(0x0681); //ジャイロリセット for(i = 0; i < 100*1000*10; i++); preprocess_spi_gyro_2byte(0x0601); //Low Power Mode OFF for(i = 0; i < 100*1000*10; i++); preprocess_spi_gyro_2byte(0x7F20); //User Bank2に変更 for(i = 0; i < 100*1000*10; i++); preprocess_spi_gyro_2byte(0x0107); //Range を最大2000dpsへ変更 for(i = 0; i < 100*1000*10; i++); preprocess_spi_gyro_2byte(0x7F00); //User Bank0に変更 for(i = 0; i < 100*1000*10; i++); preprocess_spi_gyro_2byte(0x0621); //ジャイロスタート
なお、タイマ割り込みによるセンサ値の取得は、センサの初期設定が終了した後に開始したいので、init_cmt関数はここで追加したコードの下に移動します。
wait_ms関数の設定
今後、指定時間待機するwait_msという関数をよく使います。サンプルプログラムStep7から、misc.cをコピーしてプロジェクトに追加します。また、wait_ms関数が機能するように、以下の編集を行います。
- interrupt.c
- init.c
interrupt.c
wait_ms関数で使用する変数(timer)は、1msごとに呼び出されるint_cmt0関数内でカウントすることにします。interrupt.cに戻って、int_cmt0関数内に次の一行を追加します。
timer++;
init.c
変数の初期化も必要なので、init.cのinit_all関数にtimerをリセットする行を追加します。
timer = 0;
テスト
正常にセンサの値が取得され、車体の状態が更新されているか確認します。main関数を次のように書き換えます。
my_hm_starterkit.c
void main(void) { int_all(); unsigned long i = 0; int a, b; for(i=0; i< 100*1000*10; i++); for(i=0; i< 100*1000*100; i++); gyro_get_ref(); while(1){ for(i = 0; i< 100*1000*10; i++); for(i = 0; i< 100*1000*100; i++); a = (int)(speed*1000); b = (int)(degree) SCI_printf("\x1b[2J"); //クリアスクリーン[CLS] SCI_printf("\x1b[0;0H"); //カーソルを0,0に移動 SCI_printf("speed,%d\n\r",a); SCI_printf("degree,%d\n\r,b); } }
TeraTermと接続して実行させると、車体の速度と角度が表示されるはずです。
ちなみに、gyro_get_ref関数はジャイロの値を2500回取得しているため、少し時間がかかります。今回のプログラムだと最初の値が表示されるまで5秒ほど必要です。
次回
エンコーダ、ジャイロセンサ、タイマ割り込みを組み合わせ、マウスの速度・回転を把握できるようになりました。次は、迷路探索に使用する光センサの値を取得できるようにしたいと思います。