R&D daily

目標速度計算に必要な平方根計算を組んでみる。
処理時間をざっくり計測してみると、6～9usecと出た。

それなりに時間がかかってはいるようだ．．．
ループを展開したりすればもう少し速くなるかもしれないけれど、毎制御周期で１回しか使わないはずなので、現状これでよしとする。