[C#] ミリ秒単位のタイマーを作成する

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 制御の積分成分を出せば良いはずで、これで平均値を出すときのずれが減るかなと。いまのところ、こんな感じで倒立できています。