[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 である必要もないので。

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

20151106_01

カテゴリー: 開発, C# パーマリンク