台形制御

　ステッピングモータで台形制御を試している。とりあえず動くようになってきたのでメモ。

　横軸が時間(秒)で、縦軸は右が位置、左が速度。
　位置は25600ステップで1回転分。速度は約67.11で1step/secとなる。ただし速度は200stepで1回転(ドライバの仕様)なので、約13422step/secで1rev/secとなる。2000step/secでリミットしてるので、1rev/6.7secくらい。0.5回転の移動に3秒ほどなので、だいたい計算通り。

　とりあえず秒20回制御してる。printfで位置や速度を出力しているのでこのレートだが、実際はもうちょっと細かく制御したい。


　台形制御というのは、速度が台形になる制御であり、位置が台形になる制御ではないのであしからず(位置が台形になるのはON/OFF制御)。

　指令値と実測値の差が少ない領域(速度が0に近い時)の操作を作ってないので、そのあたりでは発振してしまっているが、とりあえず台形制御にはなっている。


　台形制御は、低速域を除けば、「加速するか、減速するか、それが問題だ」みたいな制御になる。「この速度だと目標を追い越すぞ」となれば減速し、それ以外では常に加速し続ける。ただしあまりに加速してしまうとモーターの限界を超えてしまうので、そこはリミットする。
　ざっくり言えば、制御的には三角制御を行っているが、山の高さをある程度に制限することにより、結果的に台形になる、みたいな感じだろうか。

　減速するかどうかの判定は、
　現在位置 + (速度 * (abs(速度) / 加速度) / アップデートレート) > 目標位置
　がtrueの場合は速度から加速度を引き、falseの場合は速度に加速度を足す。
　こうすると、現在位置と目標位置の関係にかかわらず、うまく制御できる。
　あとは速度のリミットや、低速時の挙動を処理すれば、うまく動く気がする。

　まぁ、他にももっと計算コストの低い、あるいは簡単な実装がありそうな気がする。


　とりあえず、台形制御が動きそうな雰囲気はしてきたので、この方向でもうちょっと進めてみる。
　でも今回のは速度をPID制御したほうが向いてる気がするなぁ。。。