マイクロマウス研修 Part12(岩本)


ーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーー

マイクロマウス研修(岩本)一覧
Part1Part2Part3Part4Part5Part6Part7Part8Part9Part10Prat11、Prat12

番外
Part8.1

ーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーー

こんにちは岩本です。
今回は、サンプルプログラムSTEP6-1~6-2(run_wait・run_ST_PWM)の中身を確認していきたいと思います。
STEP6-1は、180㎜進むためのサンプルです。
MOT_CWCCW_R(L)は、回転方向を決めており、portdef.hでポートの設定がされています。そして、MOT_FORWARDはstatic_parameters.hで0となっています。
MOT_CWCCW_R(L)に、0を出力すると、前進する方向に回転するようになっています。逆に1を出力すると後退する方向に回転するようになります。
43行目でMOT_POWER_ONでモーターを励磁させています。44行目は、いきなり回すと脱調することがあるため、脱調防止の為の待ち時間です。
48行目から52行目で、ブザーとモーターにパルスを出力しています。

 

上図46行目のLEN2STEP(180)の中身ですが、下図のmacro.hで定義されており、180㎜進むためのステップ数を求めています。タイヤの直径(TIRE_DIAMETER)は、parameters.hで定義されています。
PI:COで使われいるタイヤは直径48㎜、モータは1ステップで0.9度ずつ回転するモータ(1回転に400ステップ必要)を使用しています。
そのため、1ステップ辺りの移動量は、 48×PI/400 となり、
180㎜進む時に必要なステップ数は、 180/(48×PI/400)= 400×180/(48×PI) となります。
while文のカウントは、1回で2進むので、2倍されています。なぜ2倍しているかは後々解説します。

 

 

STEP6-2は、PWMでモータを回します。
今回メインプログラム部分は、part9で説明したモード選択しているだけなので、飛ばします。
モード選択の内容は下図のようになり、モータを励磁させた後122行目から123行目で最高速度・加速度を指定しています。
124行目でstraightが直進走行用の関数で、括弧内の180が移動距離を表しています。

 

straight()はrun.cで定義されています。
24行目で、PWMを出力して前進を開始しています。
34行目では、残りの走行距離と減速に必要な距離を比較して、減速が必要な距離になったら処理が開始されます。
この式は、関数で書いていると分かりにくいかもしれませんが、x=(V²-V0²)/(2a)(x:移動距離、V:速度、V0:初速度、a:加速度)の式です。
なお、STEP2LENは、LEN2STEPとは逆にステップを距離に変換しています。
最後に38行目で目標ステップ数に到達したらPWMを停止しています。

 

加減速は、interrupt.cのint_cmt0で定義されています。
23行目でspeedに加速度を足していっています。
25~31行目は、最高・最低速度の制限
36・37行目で求めた速度をTGRCのバッファレジスタに代入し、速度を変えてます。

 

step_r・step_lは、interrupt.cでカウントアップされています。
なお、int_motは、intprg.cで定義されており、
int_mot_rは、MTU3のTGRBがコンペアマッチすると割り込みが発生します。
int_mot_lは、MTU4のTGRBがコンペアマッチすると割り込みが発生します。

 

今回はここまでです。


Posted in Pi:Co Classic3 製作記