[IoT] 在樹莓派中,使用C#驅動伺服馬達

在前一篇[IoT] 在樹莓派中,使用C#驅動步進馬達中,說明了如何透過C#驅動步進馬達
在本篇文章,會說明如何驅動伺服馬達

要驅動伺服馬達的方式有兩種,一種是透過GPIO發送訊息的頻率,進行伺服馬達轉動的動作,另一種則是直接發送轉動的角度定位至GPIO,讓伺服馬達作旋轉,兩種方式的寫法不一樣,硬體的環境設定也不一樣
這裡會針對兩種方式都作說明

這篇文章所用的伺服馬達是MG90S,而在線路的配置上,連接馬達的線路
紅色:輸入電壓
棕色:接地
橘色:訊號輸入

所以硬體的接線上,我們把輸入電壓接在Pin2(5v),接地接在Pin6(GND),訊號輸入接在Pin12(GPIO 18)

硬體這樣接就算完成了,如果擔心電流或是電壓爆衝的問題,可以加上一個電容作緩衝

接著,我們把Visual Studio打開,並在專案中加入一個類別庫,ServoMotor.cs

將下面的程式碼放入至ServoMotor.cs的類別庫中

using Windows.Devices.Gpio;
using Windows.System.Threading;
using System.Diagnostics;
using System.Threading;
using Windows.Foundation;

public class ServoMotor
{
    public GpioPin pin;
    public GpioPinValue pinValue;

    private IAsyncAction workItemThread;
    public GpioController gpio;

    public ServoMotor(int servoPin)
    {
        gpio = GpioController.GetDefault();
        pin = gpio.OpenPin(servoPin);
        pinValue = GpioPinValue.High;
        pin.Write(pinValue);
        pin.SetDriveMode(GpioPinDriveMode.Output);
    }

    public void PWM_R(int intAngle)
    {
        var stopwatch = Stopwatch.StartNew();
        intAngle = (intAngle * 300 / 90);

        workItemThread = Windows.System.Threading.ThreadPool.RunAsync(
                (source) =>
                {
                    // setup, ensure pins initialized
                    ManualResetEvent mre = new ManualResetEvent(false);
                    mre.WaitOne(1000);

                    ulong pulseTicks = ((ulong)(Stopwatch.Frequency) / 1000) * 2;
                    ulong delta;
                    var startTime = stopwatch.ElapsedMilliseconds;
                    while (stopwatch.ElapsedMilliseconds - startTime <= intAngle)
                    {
                        pin.Write(GpioPinValue.High);
                        ulong starttick = (ulong)(stopwatch.ElapsedTicks);
                        while (true)
                        {
                            delta = (ulong)(stopwatch.ElapsedTicks) - starttick;
                            if (delta > pulseTicks) break;
                        }
                        pin.Write(GpioPinValue.Low);
                        starttick = (ulong)(stopwatch.ElapsedTicks);
                        while (true)
                        {
                            delta = (ulong)(stopwatch.ElapsedTicks) - starttick;
                            if (delta > pulseTicks * 10) break;
                        }
                    }
                }, WorkItemPriority.High);
    }

    public void PWM_L(int intAngle)
    {
        intAngle = (intAngle * 300 / 90);

        var stopwatch = Stopwatch.StartNew();
        workItemThread = Windows.System.Threading.ThreadPool.RunAsync(
                (source) =>
                {
                    // setup, ensure pins initialized
                    ManualResetEvent mre = new ManualResetEvent(false);
                    mre.WaitOne(1000);

                    ulong pulseTicks = ((ulong)(Stopwatch.Frequency) / 1000) * 2;
                    ulong delta;
                    var startTime = stopwatch.ElapsedMilliseconds;
                    while (stopwatch.ElapsedMilliseconds - startTime <= intAngle)
                    {
                        pin.Write(GpioPinValue.High);
                        ulong starttick = (ulong)(stopwatch.ElapsedTicks);
                        while (true)
                        {
                            delta = starttick - (ulong)(stopwatch.ElapsedTicks);
                            if (delta > pulseTicks) break;
                        }
                        pin.Write(GpioPinValue.Low);
                        starttick = (ulong)(stopwatch.ElapsedTicks);
                        while (true)
                        {
                            delta = (ulong)(stopwatch.ElapsedTicks) - starttick;
                            if (delta > pulseTicks * 10) break;
                        }
                    }
                }, WorkItemPriority.High);
    }
}

這段程式碼,主要是在呼叫順時鐘旋轉與逆時鐘旋轉的方法,並在初始化ServoMotor的時候,傳入發送訊號的GPIO Pin腳

接著在MainPage.xaml裡,加上下面的程式碼

<TextBlock x:Name="txtServoMotor" HorizontalAlignment="Left" Margin="3,16,0,0" TextWrapping="Wrap" Text="ServoMotor" VerticalAlignment="Top"/>
<Button x:Name="btnReverse" Content="Reverse" HorizontalAlignment="Left" Margin="136,10,0,0" VerticalAlignment="Top" Click="btnReverse_Click"/>
<TextBox x:Name="txtAngle" HorizontalAlignment="Left" Margin="212,10,0,0" TextWrapping="Wrap" Text="90" VerticalAlignment="Top"/>
<Button x:Name="btnFoward" Content="Foward" HorizontalAlignment="Left" Margin="281,10,0,0" VerticalAlignment="Top" Click="btnFoward_Click"/>
<Button x:Name="btnStop" Content="Reset" HorizontalAlignment="Left" Margin="212,47,0,0" VerticalAlignment="Top" Click="btnStop_Click" Width="64"/>

這段內容主要是在畫面上放入旋轉角度、順時鐘旋轉以及逆時鐘旋轉的按鈕,放完後會出現下面的畫面

接下來再將下面的程式碼放入至MainPage.xaml.cs之中

ServoMotor objServo = null;

public MainPage()
{
    this.InitializeComponent();
    objServo = new ServoMotor(18);
}

private void btnReverse_Click(object sender, RoutedEventArgs e)
{
    objServo.PWM_R(int.Parse(txtAngle.Text));
}

private void btnFoward_Click(object sender, RoutedEventArgs e)
{
    objServo.PWM_L(int.Parse(txtAngle.Text));
}

程式碼的動作很簡單,就是在初始化的時候指定GPIO的腳位以及在旋轉的時候指定旋轉角度而已

接著將寫好的程式碼佈署到樹莓派中

執行剛佈署好的程式

透過遠端桌面,看到可以轉動伺服馬達的按鈕,這時已經可以實際進行伺服馬達旋轉的動作了

執行的結果如下

文章一開始有提到,要驅動伺服馬達有兩種方式,上面提到的是透過送出訊號的頻率,讓伺服馬達作旋轉的動作,第二種方式是直接輸出旋轉的角度定位讓伺服馬達旋轉。
要用這種方式的話,需要作一些硬體設定的變更,首先先連進樹莓派的管理畫面,並點入到[Devices]的畫面中

在[Devices]的設定中,將[Default Controller Driver]的下拉選單中,將Driver變更為[Direct Memory Mapped Driver]模式,並按[Update Driver],重新啟動樹霉派

然後回到Visual Studio裡,加入一個ServoMotorAngle.cs的類別庫檔案

將下面的程式碼,加入至ServoMotorAngle.cs的類別庫檔案之中

using Windows.Devices;
using Windows.Devices.Pwm;
using Microsoft.IoT.Lightning.Providers;

public class ServoMotorAngle : IDisposable
{
    public ServoMotorAngle(int servoPin)
    {
        if (LightningProvider.IsLightningEnabled)
        {
            LowLevelDevicesController.DefaultProvider = LightningProvider.GetAggregateProvider();
        }

        ServoPin = servoPin;
    }

    public int Frequency { get; set; } = 50;

    public double MaximumDutyCycle { get; set; } = 0.1;

    public double MinimumDutyCycle { get; set; } = 0.05;

    public int ServoPin { get; set; }

    public int SignalDuration { get; set; }

    private PwmPin ServoGpioPin { get; set; }

    public async Task Connect()
    {
        var pwmControllers = await PwmController.GetControllersAsync(LightningPwmProvider.GetPwmProvider());

        if (pwmControllers != null)
        {
            var pwmController = pwmControllers[1];
            pwmController.SetDesiredFrequency(Frequency);
            ServoGpioPin = pwmController.OpenPin(ServoPin);
        }
    }

    public void Dispose()
    {
        ServoGpioPin?.Stop();
    }

    public void Go()
    {
        ServoGpioPin.Start();
        Task.Delay(SignalDuration).Wait();
        ServoGpioPin.Stop();
    }

    public void SetPosition(int degree)
    {
        ServoGpioPin?.Stop();
        var pulseWidthPerDegree = (MaximumDutyCycle - MinimumDutyCycle) / 180;
        var dutyCycle = MinimumDutyCycle + pulseWidthPerDegree * degree;
        ServoGpioPin.SetActiveDutyCyclePercentage(dutyCycle);
    }

    public void AllowTimeToMove(int pauseInMs)
    {
        this.SignalDuration = pauseInMs;
    }
}

這段程式碼,主要目的就是當呼叫SetPosition時,可以傳入要定位的角度,並在執行Go之後進行旋轉

接著打開類別庫專案的Nuget套件管理員,加入[Microsoft.IoT.Lightning]套件的參考

接下來打開UWP的專案,使用XML編輯器點開[Package.appxmanifest],將下面的內容作一些修改,首先是在最上方的部份,讓內容變成這樣

<Package
  xmlns="http://schemas.microsoft.com/appx/manifest/foundation/windows10"
  xmlns:mp="http://schemas.microsoft.com/appx/2014/phone/manifest"
  xmlns:uap="http://schemas.microsoft.com/appx/manifest/uap/windows10"
  xmlns:iot="http://schemas.microsoft.com/appx/manifest/iot/windows10" <!-- 加上這一行 -->
IgnorableNamespaces="uap mp iot">

在最下方的內容,更改為這樣

<Capabilities>
  <Capability Name="internetClient" />
  <!-- 加上下面這兩行 -->
  <iot:Capability Name="lowLevelDevices" />
  <DeviceCapability Name="109b86ad-f53d-4b76-aa5f-821e2ddf2141" />
</Capabilities>

打開MainPage.xaml的畫面,切換到原始碼內容的部份,變更最上方的部份內容

<Page
    x:Class="maduka_RaspberryPi.App.MainPage"
    xmlns="http://schemas.microsoft.com/winfx/2006/xaml/presentation"
    xmlns:x="http://schemas.microsoft.com/winfx/2006/xaml"
    xmlns:local="using:maduka_RaspberryPi.App"
    xmlns:d="http://schemas.microsoft.com/expression/blend/2008"
    xmlns:mc="http://schemas.openxmlformats.org/markup-compatibility/2006"
    xmlns:iot="http://schemas.microsoft.com/appx/manifest/iot/windows10" <!-- 加上這一段 -->
    mc:Ignorable="d">

點開UWP專案,並點選右鍵選擇[參考],接著點選[加入參考],在參考項目中,選擇[Universal Windows] => [擴充功能],並將[Windows IoT Extensions for the UWP]打勾,加入至UWP專案的參考之中

接著把下面的控制項放入到畫面裡

<TextBlock x:Name="txtServoMotorAngle" HorizontalAlignment="Left" Margin="3,200,0,0" TextWrapping="Wrap" Text="ServoMotorAngle" VerticalAlignment="Top"/>
<Button x:Name="btnReverseAngle" Content="0" HorizontalAlignment="Left" Margin="136,200,0,0" VerticalAlignment="Top" Click="btn0Angle_Click"/>
<Button x:Name="btnFowardAngle" Content="90" HorizontalAlignment="Left" Margin="212,200,0,0" VerticalAlignment="Top" Click="btn90Angle_Click"/>
<Button x:Name="btnStopAngle" Content="180" HorizontalAlignment="Left" Margin="282,200,0,0" VerticalAlignment="Top" Click="btn180Angle_Click"/>

可以看到畫面加上了一個文字方塊與三個按鈕這三個按鈕代表,當按下按鈕的動作時,會直接將伺服馬達轉到指定的角度上

最後,將下面的程式碼放入到MainPage.xaml.cs裡

private async void btn0Angle_Click(object sender, RoutedEventArgs e)
{
    using (var servo = new ServoMotorAngle(18))
    {
        await servo.Connect();

        servo.SetPosition(0);
        servo.AllowTimeToMove(1000);
        servo.Go();
    }
}

private async void btn90Angle_Click(object sender, RoutedEventArgs e)
{
    // 讓馬達回到正中央,90度的地方
    using (var servo = new ServoMotorAngle(18))
    {
        await servo.Connect();

        servo.SetPosition(90);
        servo.AllowTimeToMove(1000);
        servo.Go();
    }
}

private async void btn180Angle_Click(object sender, RoutedEventArgs e)
{
    using (var servo = new ServoMotorAngle(18))
    {
        await servo.Connect();
        servo.SetPosition(180);
        servo.AllowTimeToMove(1000);
        servo.Go();
    }
}

這樣就完成了直接指定伺服馬達轉動角度的功能了

[Inbox Driver] 與 [Direct Memory Mapped Driver] 這兩種模式無法同時使用
切換至 [Direct Memory Mapped Driver] 模式後,也無法單純透過GPIO的高低訊號差作訊號的輸出,只能指定輸出訊號的相位模式
所以選擇要使用哪一種模式,也要看其他腳位的應用,再進行決定

參考資料
https://engineering.tamu.edu/media/4247823/ds-servo-mg90s.pdf​
Any Servo Library for Raspberry Pi 2
A servo library in C# for Raspberry Pi 3 – Part #1, implementing PWM
aspberry Pi software PWM Servo Windows IoT C#
Windows 10 IoT Core - Controlling Servo Motor

GitHub檔案下載
https://github.com/madukapai/maduka-RaspberryPi