沼津高専 電子制御工学科
MIRSMG3D 走行制御モジュール
MIRSMG3D-FMOD-0001
改訂記録
版数 作成日 作成者 承認 改訂内容
A01 2009.3.18 細川 牛丸 初版
A02 2010.7.12 牛丸 牛丸 ソースコード(motor.c, motor_therad.c, motor_func.c）の変更

目次







1.はじめに


本ドキュメントは、MIRSMG3Dの標準プログラムで使用している走行制御モジュールについてまとめたドキュメントである。

2.動作の特徴


走行制御モジュールの特徴は、次の通りである。
  • 直進制御、その場回転制御、周回制御の3つからなる。
  • 距離(角度)と時間を指定して制御を行う。
  • 左右のタイヤの回転距離の誤差を修正しながら走行する。
  • 目標値に近づいたら速度を数段階に分けて落とす。
  • 目標値を過ぎてしまった場合、目標値まで戻る。

3.動作概要


Fig.1に走行制御モジュールのスローチャートを示す。


　　　　　　　Fig.1 走行制御モジュールのフローチャート

Fig.1は直進制御の場合のフローであり、その他の制御でも同様な流れで制御を行っている。

4.モジュールの詳細


4.1.速度・距離算出

    走行制御は直進制御の場合は距離、その場回転制御の場合は回転角度、周回制御の場合はターゲット中心から移動目標までの角度と、制御に要する時間を指定して行う。一方、実際にモータの制御を行うPICには、目標速度[rad/100msec]を与える必要があり、PICから得られる情報は現在のモータの回転速度[rad/100msec]とロータリーエンコーダのパルス数である。
    そこで、走行制御モジュールで速度の算出と、移動距離の算出を行う必要がある。

    ●距離と時間からの速度の算出
      (目標速度[rad/100msec]) = {100×(距離[mm])÷{(タイヤ円周[mm])×(ギア比)÷(エンコーダの解像度)}×(時間[msec])}

    ●回転角度と時間からの速度の算出
      (目標速度[rad/100msec]) = {100×(回転角度[deg])×(円周率)÷180×(MIRS半径)÷{(タイヤ円周[mm])×(ギア比)÷(エンコーダの解像度)}×(時間[msec])}

    ●周回角度と時間からの速度の算出
      (目標速度[rad/100msec]) = (周回角度[deg])×(周回半径[mm])×(円周率)÷180×((周回半径[mm])±(MIRS半径[mm]))/(周回半径[mm])
      ※±は進行方向に依存する。
    ●パルス数から移動距離の算出
      (移動距離[mm]) = (パルス数)×100×(距離[mm])÷{(タイヤ円周[mm])×(ギア比)÷(エンコーダの解像度)

4.2.左右タイヤの誤差修正

    走行制御モジュールでは左右のタイヤの移動距離を常に計算し、それらに誤差が生じた場合、左右のタイヤの回転速度を変えることで誤差の修正を行っている。
    左右タイヤの移動距離の誤差が10[mm]以上になった場合、移動距離が多いタイヤの速度を-5[rad/100msec]、少ないタイヤを+5[rad/100msec]することで、左右タイヤの移動距離を合わせるようにしている。

4.3.目標値付近での減速

    目標値に近づいた場合、段階を踏んで減速を行い停止する。
    目標値まで100[mm]の時点で速度を半分に落とし、更に、目標値まで50[mm]の時点で速度を1/4に落としている。その後、目標値でタイヤを停止させる。

4.4.目標値を行き過ぎた場合

    目標値を行き過ぎた場合、目標値まで戻る動作を行う。
    目標値を20[mm]以上行き過ぎた場合、元の速度の1/4の速度で目標値まで戻る。この動作は左右のタイヤで独立して行う。

5.ソースコード


以下にソースコードをリンクする。



6.使用関数


使用する関数は以下の通りである。
関数の詳細は「MIRSMG3D-FREF-0001 関数レファレンス」を参照。






関連文書