あけましておめでとうございます。koraです。
2019年最初の更新です。
年末に自律走行に取り掛かっていたところだったので、今回はその続きで自律走行を実装します。
ブザーの実装
まずは、ブザーを鳴らす関数を作ります。実際にマウスを走らせると、外からはマウスが何を実行しようとしているのか分かりにくくなってしまうので、ブザーでプログラムの進行状況を知らせるようにします。
interface.h
新しくinterface.hというファイルを作って、次のプロトタイプ宣言を追加します。
void BEEP(void);
interface.c
ブザーを鳴らす関数の実体を作ります。新しくinterface.cというファイルを作って、サンプルプログラムStep7のBEEP関数とBEEP_MULTIをコピーします。
#include "iodefine.h" void BEEP(void){ int i,j; for(j=0 ; j<100 ; j++){ PORTB.PODR.BIT.B5 = 1; for(i = 0; i < 1000; i++); PORTB.PODR.BIT.B5 = 0; for(i = 0; i < 1000; i++); } } void BEEP_MULT(short n){ int i,j,k; for(k = 0; k < n+1; k++){ for(j=0 ; j<1000*0.1 ; j++){ PORTB.PODR.BIT.B5 = 1; for(i = 0; i < 100*10*1; i++); PORTB.PODR.BIT.B5 = 0; for(i = 0; i < 100*10*1; i++); } wait_ms(100); } }
自律走行の実装
interrupt.c
machine.hをインクルードするように追加して、サンプルプログラムStep7からint_cmt0関数をコピーします。
#include "machine.h"
int_cmt0関数を書くと長くなってしまうので、ここでは省略します。中身については前回の記事で解説しています。
glob_var.h
直進か回転かを切り替えるグローバル変数を追加します。TURN_DIRは回転モードで使用するグローバル変数です。
GLOBAL int run_mode; //フラグ系のグローバル変数 GLOBAL char TURN_DIR; //ターン方向フラグ
static_parameters.h
定数を追加します。
//方位 #define RIGHT (0) #define LEFT (1) #define FRONT (2) #define REAR (3) #define HALF_SECTION (45) //半区画の距離 #define SECTION (90) //一区画の距離 #define STRAIGHT_MODE 0 //直進時のモード #define TURN_MODE 1 //超信地旋回時のモード #define SLA_MODE 2 //スラロームモード #define NON_CON_MODE 3 //非制御モード #define TEST_MODE 4 //テストモード(割り込み用モータ制御を切るモード) #define F_WALL_MODE 5
parameters.h
将来的に調整する定数を追加します。
//フィードバックゲインパラメータ //Pゲイン 最初に調整する 実速度が目標速度を中心として軽く振動する程度に調整 //Iゲイン 最後に調整する 積分値が合うようにする程度。 //Dゲイン 二番目に調整する。 P制御によって発生した振動を抑えられる程度に調整 //車体中心における並進方向速度に関するフィードバックゲイン #define SPEED_KP (20.0) //Pゲイン #define SPEED_KI (0.20) //Iゲイン #define SPEED_KD (0.0) //Dゲイン //車体中心における回転方向速度に関するフィードバックゲイン #define OMEGA_KP (20.0) //Pゲイン #define OMEGA_KI (0.4) //Iゲイン #define OMEGA_KD (0.0) //Dゲイン //走行パラメータ #define SEARCH_SPEED (0.3) //探索の速度 [m/s] #define SEARCH_ACCEL (1.0) //探索の加速度 [m/s^2] #define FAST_SPEED (1.0) //探索の速度 [m/s] #define FAST_ACCEL (2.0) //探索の加速度 [m/s^2] #define MIN_SPEED (0.1) //最低速度 [m/s] #define TURN_ACCEL (PI*2) //超信地旋回の加速度 [rad/s^2] #define TURN_SPEED (PI) //超信地旋回の最高速度 [rad/s] #define TURN_MIN_SPEED (PI/10.0) //超信地旋回の最低速度 [rad/s] #define WAIT_TIME 500 //各動作後の待機時間 [ms]
run.h
Step7からrun.hファイルをコピーしてプロジェクトに追加します。
run.c
Step7からrun.cファイルを持ってきてプロジェクトに追加します。
直進走行のテスト
my_hm_starterkit.c
まず、今回追加したヘッダーファイルをインクルードします。
#include "run.h" #include "interface.h"
次に待機用の関数を宣言します。wait_ms関数については以前の記事で解説しています。
extern void wait_ms(int wtime);
main関数の中身を作成します。
init_all(); wait_ms(1000); BEEP(); gyro_get_ref(); BEEP(); len_mouse = 0; straight(SECTION,SEARCH_ACCEL,SEARCH_SPEED,0); wait_ms(1000); MOT_POWER_OFF; BEEP();
- init_all()で初期設定した後、wait_ms(1000)で1秒間待機し、gyro_get_ref()でジャイロセンサの初期値を取得します。
- 次に、マウスが進んだ距離を表すグローバル変数len_mouseをリセットし、straight(SECTION,SEARCH_ACCEL,SEARCH_SPEED,0)で直進します。引数で指定している値の意味は以下の通りです。
- 最後にwait_ms(1000)で1秒間待機し、MOT_POWER_OFFでモータドライバをOFFにして終了します。
以上のプログラムを書き込んで、実際に走行した結果がこちらです。
次回
今回、マウスが走行できるようになりました。しかし、実際に迷路で走行するためには、動作確認・センサの調整・探索・最短走行など、複数のモードを人間が切り替える必要があります。HM-StarterKitはLEDで選択中のモードを表示できるようになっていますので、次回はこれを使用できるようにします。