Windows10 IoT Coreでサーボモーターを動かしたかった

タイトルどおりです。あまり正確には動きませんでした。

とりあえずここまででPWMっぽいものは正しく出力できた感じがあるのでPWMが出力できればサーボモーターが動かせるはずです。

PWM信号を出力する

サーボモーターをどうやってPWMで動かすかはこちらのサイトが参考になります。

そんな感じで前回つくったPwmWriterクラスと

public class PwmWriter
{
    private bool _isWriting;

    public bool IsWriting
    {
        get
        {
            return _isWriting;
        }
    }

    private GpioPin _pin;

    public GpioPin Pin
    {
        get
        {
            return _pin;
        }

        set
        {
            _pin = value;
        }
    }

    private TimeSpan _period;

    public TimeSpan Period
    {
        get
        {
            return _period;
        }

        set
        {
            _period = value;
        }
    }

    private float _dutyRate;

    public float DutyRate
    {
        get
        {
            return _dutyRate;
        }

        set
        {
            _dutyRate = value;
        }
    }

    private Stopwatch _watch;

    /// <summary>
    /// コンストラクタ
    /// </summary>
    /// <param name="pin">出力するGPIOピン</param>
    /// <param name="periodMillisec">PWMの1周期の長さ</param>
    /// <param name="dutyRate">デューティー比</param>
    public PwmWriter(GpioPin pin, float periodMillisec, float dutyRate)
    {
        this._isWriting = false;
        this._pin = pin;
        this._period = TimeSpan.FromMilliseconds(periodMillisec);
        this._dutyRate = dutyRate;
        this._watch = new Stopwatch();
    }

    /// <summary>
    /// PWM出力
    /// </summary>
    public void WriteAsync()
    {
        //1周期の長さとduty比からOn時間を計算
        TimeSpan onTime = TimeSpan.FromTicks((long)(_period.Ticks * _dutyRate));
        bool isHighWrited = false;
        bool isLowWrited = false;
        this._isWriting = true;
        
        //時間を計測するためにストップウォッチスタート
        this._watch.Start();
        //非同期Taskを開始
        Task.Run(() =>
        {
            //書き込みフラグが立ってる間はループし続ける
            while (this.IsWriting)
            {
                //経過時間を取得
                long elapsed = _watch.ElapsedTicks;

                if (elapsed < onTime.Ticks && !isHighWrited)
                {
                    //On時間ならHighをWrite
                    _pin.Write(GpioPinValue.High);
                    isHighWrited = true;
                }
                else if (elapsed >= onTime.Ticks && elapsed < _period.Ticks && !isLowWrited)
                {
                    //Off時間ならLowをWrite
                    _pin.Write(GpioPinValue.Low);
                    isLowWrited = true;
                }
                else if (elapsed >= _period.Ticks)
                {
                    //1周期終わったらストップウォッチをリスタート
                    this._watch.Restart();
                    isHighWrited = false;
                    isLowWrited = false;
                }
            }
        });
    }

    /// <summary>
    /// PWM出力を止める
    /// </summary>
    public void Stop()
    {
        _isWriting = false;
    }


}

それを利用して今回つくったServoControllerクラスを利用してサーボを回してみます。

public class ServoController
{
    int _minAngle = -90;
    int _maxAngle = 90;
    float _minMillisec = 0.5f;
    float _maxMillisec = 2.4f;
    float _period = 20;
    private PwmWriter _pwm;

    public GpioPin Pin
    {
        get
        {
            return this._pwm.Pin;
        }
        set
        {
            this._pwm.Pin = value;
        }
    }
    public ServoController(GpioPin pin)
    {
        this._pwm = new PwmWriter(pin,_period,0.0f);
    }

    public void Write(int angle)
    {
        if(angle > _maxAngle || angle < _minAngle)
        {
            throw new Exception("回転角度は-90~90で指定してください");
        }
        else
        {
            float mapMillisec = map(angle,_minAngle,_maxAngle,_minMillisec,_maxMillisec);
            float maxDuty = _maxMillisec / _period;
            float minDuty = _minMillisec / _period;
            float mapDuty = map(mapMillisec,_minMillisec,_maxMillisec,minDuty,maxDuty);
            _pwm.DutyRate = mapDuty;
            Debug.WriteLine("min="+minDuty+"max="+maxDuty+"val="+mapDuty);
            _pwm.WriteAsync();
        }

    }

    private float map(float x, float in_min, float in_max, float out_min, float out_max)
    {
        return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
    }

    public void Stop()
    {
        this._pwm.Stop();
    }
}

今回はSG90という安価で手に入るサーボモーターをつかっているのでデータシートを見てみると、回転角は-90~90、OnTimeは0.5millisec~2.4Millisec、1周期は20millisecなのでそんな感じでプログラムを書きました。

あとはこんな感じでWrite

public sealed partial class MainPage : Page
{
    GpioController gpio;
    GpioPin pin;

    public MainPage()
    {
        
        this.InitializeComponent();
        
        //GPIO初期化
        gpio = GpioController.GetDefault();
        //26番ピンを取得
        var pinId = 26;
        pin = gpio.OpenPin(pinId);
        //26番ピンを出力指定
        pin.SetDriveMode(GpioPinDriveMode.Output);

        ServoController servo = new ServoController(pin);
        servo.Write(70);
        
    }

    
}

0度

Media preview

60度(サーボの向き間違えた)

Media preview

-60度(サーボの向き間違えた)

Media preview

ここまではそこそこいい感じだったんだけど60度以上が正しく回転してくれない..

なんでなんですかね。PWMもTickで正しく計算してるはずなんですけど。

まあ公式にサーボのサンプルがあるのでちゃんと動かしたい人はそちらをどうぞ

https://github.com/ms-iot/samples/tree/develop/ServoMotorBasics

Leave a Reply

Your email address will not be published. Required fields are marked *

This site uses Akismet to reduce spam. Learn how your comment data is processed.