ｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰ
岩本ブログ一覧
ｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰｰ
こんにちは岩本です。
今回は、サンプルプログラム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がコンペアマッチすると割り込みが発生します。
今回はここまでです。