直進

加速処理を見直し。

今までエンコーダのせいで速度、加速度の分解能が低くて、
20msに1回だけ加速(速度にして10とか15とか)していたのに対して、
加速度をfloatで持つようにして他の制御と同じく1ms周期で更新するようにした。
あと減速開始位置の計算方法も修正。

これでだいぶ直進の品質が上がった気がする。