====== パラメータファイル ======
#2015-11-16 FJY キネマティクスパラメータ調整方法を修正\\
#2018-02-15 END キネマティクスパラメータ調整方法を追記\\
===== モータ制御の暴走への対処 =====

モータの個体差・バッテリー電圧の変化等により、freeモード時にモータが回転し続ける場合があります。これは、モータの逆起電力補償や摩擦補償が過剰にかかっている状態ですので、以下のパラメータを調整して下さい。

^MOTOR_VC| モータの回転数定数を、増やす|
^TORQUE_NEWTON| モータ軸から見た摩擦係数、減らす|
^TORQUE_VISCOS|モータ軸から見た粘性摩擦係数、減らす |

===== ダイナミクスパラメータの決定 =====

YP-Spurでは、制御性を高めるため、左右のタイヤ間の相互作用を含むダイナミクスを考慮したフィードフォワード制御を使用しています。

^MASS| ロボットの質量|
^MOTOR_M_INERTIA| モータのロータ慣性モーメント|
^TIRE_M_INERTIA| タイヤの慣性モーメント|
^MOMENT_INERTIA| ロボットの旋回中心を軸とする慣性モーメント| 


  - はかりや体重計でバッテリ、制御用コンピュータなどを全て搭載した際の総重量を求める
  - モータのデータシートからロータ慣性モーメントを確認する。コアレスタイプのモータの場合は、ロータ慣性モーメントは十分小さいとして0に近似してもよい。ただし、特にギア比が高い場合は、モータ軸から見たギアの慣性モーメントも含めること 
  - タイヤの慣性モーメントを計算で求める。円柱近似、直径a、タイヤの重さMとした場合$$ \mbox{MOTOR_M_INERTIA} = \frac{1}{2} M a^2 $$
  - ロボットの慣性モーメントを計算で求める。3D-CADで設計している場合は、CAD上でロボットの旋回中心を軸とする慣性モーメントを求める。もしくは、近似的に計算で求める直方体近似、ロボットの幅w[m]、奥行きl[m]とした場合$$ \mbox{MOMENT_INERTIA} = \frac{1}{12} \mbox{MASS} ( l^2 + m^2 ) $$

===== パラメータの調整方法 =====
==== 速度制御パラメータ ====

YP-Spurでは、シンプルなフィードバック系を用いながらも制御性を高めるため、フィードフォワード計算によってモータの逆起電力補償、摩擦補償を行っています。以下のパラメータを決定する必要があります。

^GAIN_KP| 比例(P)制御パラメータ|
^GAIN_KI| 積分(I)制御パラメータ|
^INTEGRAL_MAX| PI制御における積分項の最大値|
^TORQUE_MAX| モータの最大トルク|
^TORQUE_NEWTON| モータ軸から見た摩擦係数|
^TORQUE_VISCOS| モータ軸から見た粘性摩擦係数|


  - 上記のパラメータを全て0にし、TORQUE_MAXは適当に大きめの値(1程度)に設定する
  - 手でタイヤを空中で回転させ、手を離した際に回転が止まらない場合には、止まるようになるまでMOTOR_VCを大きくする
  - 手でタイヤを空中で回転させ、手を離した際に回転が止まる範囲で、TORQUE_NEWTONを少しずつ大きくする
  - 同様に、手でタイヤを空中で回転させ、手を離した際に回転が止まる範囲で、TORQUE_VISCOSを少しずつ大きくする
  - 手でタイヤを空中で回転させようとしたときに、振動が発生しない範囲で、GAIN_KPを少しずつ大きくする (例えば10から100程度) $ \frac{1}{\mbox{GAIN_KP}} $ が制御の時定数を表す 
  - 地面の凹凸にタイヤがはまったときに、どの程度の距離なら頑張って抜け出してほしいか想像し、その距離に応じたタイヤの回転量をINTEGRAL_MAXとする (例えば0.1から0.5程度)
  - 手でタイヤを空中で回転させようとしたときに、振動が発生しない範囲で、GAIN_KIを少しずつ大きくする$ \frac{1}{\sqrt{\mbox{GAIN_KI}}} $が制御の時定数を表す 
  - ロボットを地面に置き、サーボがかかっている状態でロボットを押したときに、ロボットが押し負ける力がちょうど良くなるように、TORQUE_MAXを小さくする (例えば1から0.1程度) 

==== キネマティクスパラメータ ====

ここでは、オドメトリのキネマティクスパラメータである以下のパラメータの調整方法の例をあげます。

^RADIUS_R | 右タイヤ半径 |
^RADIUS_L | 左タイヤ半径 |
^TREAD | トレッド |

これらのパラメータはロボットの個体差や、タイヤのゴムの変形などで、微細に変化するため、 オドメトリにより精度のよく自己位置を推定したい場合に調整が必要となります。

   - 設計値または実測値から、初期パラメータを決める。
   - 左右タイヤ半径の比
   - ロボットに直進(spur_line_FS(0,0,0)等)を指示し、任意の距離(10〜30[m]程度)まっすぐ走行することを確認する。
       * 右にそれる場合、右タイヤ半径の値を小さくする(左タイヤ半径の値を大きく)
       * 左にそれる場合、左タイヤ半径の値を小さくする(右タイヤ半径の値を大きく) 
   - 左右タイヤ半径
   - ロボットに直進、任意の距離(10〜30[m]程度)で停止(spur_stop_line_FS(30,0,0)等)を指示し、実際の停止位置がオドメトリの値と一致しているか確認する。
       * メジャーなどで計測した走行距離がオドメトリの値よりより短い場合、左右の比を保ったまま両輪のタイヤ半径の値を大きくする
       * メジャーなどで計測した走行距離がオドメトリの値よりより長い場合、左右の比を保ったまま両輪のタイヤ半径の値を小さくする 
       * メジャーなどで計測した距離と、オドメトリの値の比を計算し、その値をタイヤ半径に反映させる。（※上記２つの手法ではなく、これを推奨）
   - トレッドの調整
   - ロボットに10回転等を指示し、実際に停止した向きがオドメトリの値と一致しているか確認する。
       * 分度器などで計測した旋回角度がオドメトリの値より小さい場合、トレッドの値を大きくする
       * 分度器などで計測した旋回角度がオドメトリの値より大きい場合、トレッドの値を小さくする 
       * 分度器などで計測した旋回角度と、オドメトリの値の比を計算し、その比をトレッドに反映させる。（※上記２つの手法ではなく、これを推奨）

===== 各パラメータの説明 =====
==== バージョン管理 ====

^ VERSION | パラメータファイルバージョン |

==== モータパラメータ ====

^ COUNT_REV | [Counts/rev] | モータ１回転あたりのエンコーダのパルス数(4逓倍) |
^ VOLT | [V] | 電源電圧 |
^ GEAR | [in/out] | ギア比 |
^ MOTOR_R | [Ω] | モータの内部抵抗 | 
^ MOTOR_TC | [Nm/A] | モータのトルク定数 |
^ MOTOR_VC | [rpm/V] | モータの回転数定数 |

==== キネマティクスパラメータ ====

^ RADIUS_R | [m] | 右タイヤ半径 |
^ RADIUS_L | [m] | 左タイヤ半径 |
^ TREAD | [m] | トレッド |

==== ダイナミクスパラメータ ====

^ MASS | [kg] | ロボットの質量 |
^ MOMENT_INERTIA | [kgm2] | ロボットの旋回中心を軸とする慣性モーメント |
^ MOTOR_M_INERTIA | [kgm2] | モータのロータ慣性モーメント |
^ TIRE_M_INERTIA | [kgm2] | タイヤの慣性モーメント |
^ TORQUE_NEWTON | [Nm] | モータ軸から見た摩擦係数 |
^ TORQUE_VISCOS | [Nm/(rad/s)] | モータ軸から見た粘性摩擦係数 |

==== 走行制御パラメータ ====
^ CONTROL_CYCLE | [s] | 軌跡制御周期  |
^ MAX_VEL | [m/s] | 最大走行速度  |
^ MAX_W | [rad/s] | 最大角速度  |
^ MAX_ACC_V | [m/ss] | 最大加速度 | 
^ MAX_ACC_W | [rad/ss] | 最大角加速度 | 
^ MAX_CENTRI_ACC | [m/ss] | 最大遠心加速度 | 
^ L_C1 | [m/s / rad/s] | 軌跡制御における減速係数 |
^ L_K1 | [rad/ss / m] | 目標軌跡までの距離に対するフィードバック係数 | 
^ L_K2 | [rad/ss / rad] | 目標軌跡との相対角度に対するフィードバック係数 | 
^ L_K3 | [rad/ss / rad/s] | 角速度に対するフィードバック係数 | 
^ L_DIST | [m] | 距離偏差のクリップ値 | 
==== モータ速度制御パラメータ ====

^ CYCLE | [s] | 速度制御周期 |
^ GAIN_KP | [1/s] | PI制御パラメータ(Kp) | 
^ GAIN_KI | [1/ss] | PI制御パラメータ(Ki) |
^ INTEGRAL_MAX | [rev] | PI制御における積分項の最大値 | 
^ TORQUE_MAX | [Nm] | モータの最大トルク |

    
 ==== ボディパラメータ ====

^ SIZE_FRONT | [m] | ロボットの代表点(自己位置が示す点)からロボットの前面までの距離 |
^ SIZE_REAR | [m] | Robot size of rear ロボットの代表点(自己位置が示す点)からロボットの背面までの距離 | 
^ SIZE_LEFT | [m] | Robot size of left ロボットの代表点(自己位置が示す点)からロボットの左側面までの距離 | 
^ SIZE_RIGHT | [m] | Robot size of right ロボットの代表点(自己位置が示す点)からロボットの右側面までの距離 | 

==== その他 ====

^ TORQUE_UNIT | [Integer Nm/Nm] | PC-MCU間の通信における固定小数点(トルク) | 
^ VOLT_UNIT | [Integer V/V] | PC-MCU間の通信における固定小数点(電圧) |
^ METER_UNIT | [Integer m/m] | PC-MCU間の通信における固定小数点(距離) |
^ ANPERE_UNIT | [Integer A/A] | PC-MCU間の通信における固定小数点(電流) |
^ AVEL_UNIT | [Integer rad/s / rad/s] | PC-MCU間の通信における固定小数点(角速度) | 
^ PWM_MAX | [Counts] | PWM周期 |