Netduino--同時控制多個伺服馬達

netduino 有四個PWM接腳，分別是 pin5、6、9、10

若能同時控制這四個接腳輸出訊號，就可以同時控制四個伺服馬達

(PS.如果還要控制更多馬達，可能就要搭配74HC595來設計程式)

下圖為實驗線路圖

 

以下是完整程式

using System;
using System.Threading;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
using SecretLabs.NETMF.Hardware;
using SecretLabs.NETMF.Hardware.Netduino;

namespace PWM_Server_Test02
{
    public class Program
    {
        const int TimeSlot = 10;
        const int ServoNum = 4;
        const int DefaultMinPos = 750;
        const int DefaultMaxPos = 2250;
        const int DefaultPos = 1500;

        struct s_stat
        {
            public int curr_pos, cmd_pos, step_pos, max_pos, min_pos;
            public bool stopflag;
            public PWM servo;
        }

        static s_stat[] servo_stat = new s_stat[ServoNum];  // curr_pos, cmd_pos, step_pos
        static Thread cth;

        //檢查伺服器位置是否超過極限邊界值
        static int CheckRange(int X, int min_pos, int max_pos)
        {
            return X < min_pos ? min_pos : (X > max_pos ? max_pos : X);
        }

        //初始化資料
        private static void InitialData()
        {
            servo_stat[0].servo = new PWM(Pins.GPIO_PIN_D5);  //指定 channel 0 馬達 就是接腳 pin5 所連的馬達
            servo_stat[1].servo = new PWM(Pins.GPIO_PIN_D6);  //指定 channel 1 馬達 就是接腳 pin6 所連的馬達
            servo_stat[2].servo = new PWM(Pins.GPIO_PIN_D9);  //指定 channel 2 馬達 就是接腳 pin9 所連的馬達
            servo_stat[3].servo = new PWM(Pins.GPIO_PIN_D10); //指定 channel 3 馬達 就是接腳 pin10 所連的馬達

            for (int i = 0; i < ServoNum; i++)
            {
                servo_stat[i].curr_pos = DefaultPos;   //curr_pos, cmd_pos, step_pos
                servo_stat[i].cmd_pos = DefaultPos;
                servo_stat[i].step_pos = 0;
                servo_stat[i].max_pos = DefaultMaxPos;
                servo_stat[i].min_pos = DefaultMinPos;
                servo_stat[i].stopflag = true;

                servo_stat[i].servo.SetPulse((uint)20000, 1500);  //回歸原點
            }

            // 建立多緒負責執行驅動核心
            cth = new Thread(new ThreadStart(control_core)); 
            cth.Start();
        }

        //多緒程式 驅動核心
        static void control_core()
        {
            int _step_pos;
            while (true)
            {
                for (int i = 0; i < ServoNum; i++)
                {
                    if (!servo_stat[i].stopflag)
                    {
                        if (servo_stat[i].curr_pos != servo_stat[i].cmd_pos)  //判斷目前位置  是否已經到達期望位置
                        {   //若尚未到達，則計算還剩多遠
                            _step_pos = servo_stat[i].cmd_pos - servo_stat[i].curr_pos;
                           //與原先設定的 step 距離相比，取較小者
                            if (System.Math.Abs(servo_stat[i].step_pos) < System.Math.Abs(_step_pos))
                                _step_pos = servo_stat[i].step_pos;
                            //計算下一步要前進的位置
                            servo_stat[i].curr_pos += _step_pos;
                      //檢查是否超出範圍
                      servo_stat[i].curr_pos = CheckRange(servo_stat[i].curr_pos, servo_stat[i].min_pos, servo_stat[i].max_pos);
                            servo_stat[i].servo.SetPulse((uint)20000, (uint)servo_stat[i].curr_pos);
                        }
                        else
                            servo_stat[i].stopflag = true;   //設定停止旗標
                    }

                }
                Thread.Sleep(TimeSlot);
            }
        }

        //設定伺服器位置
        public static void ServoMove(int channel, int pos, int speed)  //伺服器編號, 指定位置, 轉速
        {
            //檢查是否 out of range
            pos = CheckRange(pos, servo_stat[channel].min_pos, servo_stat[channel].max_pos);

            servo_stat[channel].cmd_pos = pos;
            int delta_pos = servo_stat[channel].cmd_pos - servo_stat[channel].curr_pos;
            int _step_pos = delta_pos / (speed / TimeSlot);
            servo_stat[channel].step_pos = _step_pos;
            servo_stat[channel].curr_pos += _step_pos;
        }

        //主測試程式
        public static void Main()
        {
            // write your code here
            InitialData();

            while (true)
            {
                ServoMove(0, 2000, 20);  //設定 channel 0 的 servo 快速轉動(速度20)至位置2000
                ServoMove(1, 1000, 40);
                ServoMove(2, 2000, 100); 
                ServoMove(3, 1000, 200);  //設定 channel 3 的 servo 慢速轉動(速度20)至位置1000
                Thread.Sleep(2000);

                ServoMove(0, 1000, 20); 
                ServoMove(1, 2000, 40);
                ServoMove(2, 1000, 100); 
                ServoMove(3, 2000, 200);
                Thread.Sleep(2000);
            }
        }
    }
}

以下是同時控制伺服器的影片

 

說實話，我覺得有點怪怪的，不過看起來勉強可以用啦