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 である必要もないので。
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 | 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(); } } |
利用コード
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 | 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 制御の積分成分を出せば良いはずで、これで平均値を出すときのずれが減るかなと。いまのところ、こんな感じで倒立できています。