トルク上限を追加し発熱を抑制する

[nupenupe]

倒立時に股が徐々に開脚することにより、トルクが増大しモータが発熱する問題に対してトルクの上限を設ける形で温度上昇を抑制しました。

Goモータでは明示的なトルク上限を設けることはできません。
また倒立時のトルクの支配的な項はP制御のトルクであるため、こちらに擬似的な制限を設けることにしました。
具体的には、偏差にPゲインをかけたものがトルクとなるため、算出されたトルクがしきい値以上になった場合、Pゲインをしきい値を偏差で除したものとすることで、擬似的なトルク上限を実現しています。
この実装ではPゲインからのトルクに制限をかけているため、Dゲインやトルク指令などには影響を及ぼさない実装となっています。
実験結果ではトルク上限近傍でトルクが頭打ちになっているため、機能していることは確認済みです。

トルクの上限はtorque_limitで管理しています。設定値は9.0 Nmにしています。
この値は約300秒程度倒立ができ、温度上昇がゆるやかといった点から選んでいます。
必要に応じて変更していただいて問題ありません。
トルク上限を大きくすると温度上昇が早くなりますが、姿勢を維持する時間が長くなるトレードオフになっています。