MonoBrick を使い倒立振子ロボットを C# に移植したところですが、ジャイロやサーボの回転数のサンプリングレートに問題があります。もともとのコードがだいたい 20 msec 単位(1秒間に50回)ぐらいのサンプリングを行っているものの、途中で C# で Timer クラスを使うと 30 msec ぐらいの精度しかないんですよね。しかも、だんだんと値がずれていきます。
ちょっと考えて、きちんと 20 msec 単位でサンプリングできる TickTimer クラスを作ったので公開しておきます。
TickTimer クラス
System.Diagnostics.Stopwatch と Thread を使って正確に msec 単位の割り込みを発生させます。使い方は、Timer クラスと同じようにコールバック関数を指定して使います。
最初は Task で作ったのですが、mono の Task 生成が遅いらしく Thread に切り替えています。MonoBrick が .NET 4.0 ベースなので async/await が使えないし、まあ Task である必要もないので。
public class TickTimer { TimerCallback _cb; Stopwatch _sw; int _dueTime; int _period; bool _loop = true; Thread _task; public TickTimer(TimerCallback callback, object state, int dueTime, int period) { _cb = callback; _dueTime = dueTime; _period = period; _sw = new Stopwatch(); _task = new Thread(onTimer); _task.Start( state ); } public TickTimer(TimerCallback callback, int period) { _cb = callback; _period = period; _sw = new Stopwatch(); _task = new Thread(onTimer); } public void Start( object state = null ) { _dueTime = 0; _task.Start( state ); } public void Stop() { _loop = false; } private void onTimer(object state) { Thread.Sleep(_dueTime); _sw.Restart(); while (_loop) { long msec = _sw.ElapsedMilliseconds; int rest = _period - (int)(msec % _period); // 200msecだけ余らせてスリープ if (rest > 200) { Thread.Sleep(rest - 200); } // 200msecの間、ちょうどになるまでループで待つ while (true) { if (_sw.ElapsedMilliseconds >= msec + rest) { break; } } if (_cb != null) { _cb(state); } } _sw.Stop(); } }
利用コード
class Program { static void Main(string[] args) { // 1秒毎にタイマーを発生させる // System.Threading.Timer で間隔 20msec を指定すると 30msec 程度になる. // 1.0sec を指定しても完全に1.0にはならない。1msecぐらいずれていく Timer tm = new Timer(timerCB,null,0,1000); Console.WriteLine("Timer start."); Console.ReadKey(); tm.Change(System.Threading.Timeout.Infinite, System.Threading.Timeout.Infinite); // System.Diagnostics.Stopwatch を利用して、正確なタイマーを作る TickTimer tm2 = new TickTimer(timerCB, null, 0, 1000); Console.WriteLine("TickTimer start."); Console.ReadKey(); tm2.Stop(); // 20msecも正確に測れる // TickTimer tm3 = new TickTimer(timerCB, null, 0, 20); TickTimer tm3 = new TickTimer(timerCB, 20); tm3.Start(); Console.WriteLine("TickTimer start at 20msec."); Console.ReadKey(); tm3.Stop(); } static void timerCB(object obj) { var msec = DateTime.Now.Millisecond; Console.WriteLine("msec: {0}", msec); } }
実行すると、こんな感じ。
Timer クラスを使うと 1msec ずつずれていくけど、TickTimer の場合は、1msec 程度ずれても元に戻るので正確な 1000msec や 20msec で割り込みを入れられます。
この割り込みを使って、PID 制御の積分成分を出せば良いはずで、これで平均値を出すときのずれが減るかなと。いまのところ、こんな感じで倒立できています。