台形速度制御を実装してみました〜

前のエントリの動画では、PD制御で直進制御をしたわけですが、スタート時に車輪がスリップして少し左に曲がっています（その後制御で元に戻っていますが）。
前のエントリの制御は、モータの正転ON!（実際にはパワー30%）といきなりやっているのでスリップします。
という訳でだんだん加速していって最高速度で等速、目的地の手前から減速というごく普通の走りにするため台形速度制御を追加しました。
ドライバAPIのインターフェースは、「加速度4km/h・s、最高速度4km/hの直進5m」のような感じの命令で、

//前へ進む
// forward : 進む距離（単位：mm）、負のとき後退
// speed : 最大速度（単位：m/h）、後退のときでも正にすること
// accel : 加速度（単位：m/h・s）、後退のときでも正にすること
void GoForward(int16 forward,int16 speed,int16 accel)

とした。

上図、青が目標速度、オレンジが実際の速度。実際の速度を目標速度にあうようにPD制御する。
速度が台形なので、移動距離、最高速度、加速度が指定されれば台形の形が自動的に求まる。
ちなみに指定距離が短いときは、台形ではなく三角形になり、最高速度に達しない。

最初の実装は失敗

最初の実装は、命令が来たら最初に台形の形を求める。加速区間と減速区間は、時刻tから目標速度を決めた。そしてロータリエンコーダで加速度区間の面積分の距離を進んだら等速区間へモード切替、等速区間の面積分さらに進んだら減速区間へというように実装した。
実際に試したところ、試走トライによっては、目的地に達する前に目標速度が0になってしまってロボットが目標地点手前で止まってしまった。

次の実装は成功（たぶん）

そこで減速区間の目標速度の決め方を時刻から決めるのではなく、残り距離（三角形の面積）から目標速度を決めるようにした。これで目標速度が常に調整され手前で止まりそうになっても目標速度が０にならずに目的地に到着する。

とはいえ、まだノートPCを積んでログを取るという環境が作れていないので正確にロータリーエンコーダの値が目標地点ぴったりかどうかはまだ分からない。ログが取れるようになったら、さらに改良が必要かもしれない。（ずれがあるようなら積分制御をたしてPID制御としないといけないかな）
とりあえず、加速時のスリップはほぼなくなったように見えるし、時速4km/hを保つこともできた。

次は、超信地旋回で90度曲がれ。みたいな制御かな。これこそPID制御がいりそうな気がするな。