不定期TOPIC 2003/11/05より

サーボ軌道計算のプログラム製作に精を出しているSISOですが、みなさんいかがお過ごしでしょうか？せっかくなので、この手の計算物のプログラムを作るときに、ちょっと注意しておくといいところをあげてみます。

サーボ軌道計算…というと、やっぱ小数点のついた計算（浮動小数点）をする方が、お手軽に精度とかでたりするんですが、そこはマイコン、パフォーマンスを考えると、結構、きついです。以前、SH7047F（RAMベースでプログラム実行）で、単純なインクリメントのプログラムを実行してみたのですが、long型のインクリメントと比較して、float型のインクリメントでだいたい20倍ぐらい、double型で30倍ぐらいの時間がかかります。コプロセッサとかあるといいんですけどね〜。

そんなわけで、多くの方は、すべて整数型で計算をされているのではないかと思います。整数型で計算する場合、小数点以下の値は無くなってしまいますので、いろいろ気をつける必要があります。例えば、もともと値を１０倍とか１００倍しておく方法があります。また、それ以外にも、計算順序を考慮するのがポイントとしてあります。

簡単な例として、サーボ軌道を直線で補間する例を書きます。サーボ軌道を直線補完するためには、以下のような感じの処理が一般的だと思います。

1. 「（動作終了時のサーボ角度−動作開始時のサーボ角度）／全体の動作カウント数」という式によって、1カウントあたりのサーボ角度を求めます。
2. これに、「軌道を求めたい動作カウント数」を掛けます。そうすることによって、角度の増減量が求められます。
3. さらに、「動作会指示のサーボ角度」を加えることで、最終的な答えを得ることができます。

では、これをプログラムにすると…まずは変数の説明からです。lSrc, lDstにそれぞれサーボの動作開始時の角度、動作終了時の角度が入っており、全体の動作カウント数lTtlClockで、軌道を求めたい動作カウント数をlCurClockとします。

    lAns = ( lDst - lSrc ) / lTtlClock * lCurClock + lSrc;

となります。ところがこれだと、ちょっと変な結果が得られます。lSrc=10、lDst=30、lTtlClock=3、lCurClockを１〜３で変化させると、16、22、28となります。あら、最後が３０になりません。うーん、どうしてでしょう。計算結果の差をみると、最初の位置から計算して、＋６、＋６、＋６、で、挙句の果てに行ってほしい場所まで行ってくれません。なぜでしょう？？？これが小数点以下を切り捨てられる怖さなのです。「( lDst - lSrc) / TtlClock」のところで、ぐぐっと値が小さくなります。ここで切り捨てられた小数点下が、次の「* lCurClock」の結果にぐぐっと影響するわけです。いや〜、こういうテストするときって、ＧＤＬのエミュレート機能って、とても役に立ちます。

ポイントとしては、なるべく、式の中で常時数字が大きく保たれるようにした方がよいです。そこで、ちょっと計算の順番を入れ替えてみます。

    lAns = (lDst - lSrc) * lCurClock / lTtlClock + lSrc;

今度は16、23、30。おお、いいですね。差も、＋６、＋７、＋７と、ゴールに向かってそれなりに均等に計算されています。これは、割り算よりも掛け算を先に行うことで、数値を一旦大きくし、小数点以下切り捨てによる誤差を小さくしているため、計算結果が良くなります。

これぐらいの式ならばすぐに気がつくのですが、これにちょっと複雑な要素を入れて長い式になってくると、なかなか見つけるのが難しくなります。最後になりましたが、オーバーフローにも気をつけましょう。