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);
}
}
}
}
以下是同時控制伺服器的影片
說實話,我覺得有點怪怪的,不過看起來勉強可以用啦