ROSA 系統開發 12 ─ SERVO 驅動

http://4rdp.blogspot.com/2015/11/rosa-12-servo.html?m=0

影片來自 MeArm.Joytick

接下來 ROSA 將以 meArm 機械手臂為控制標的，加入伺服馬達驅動控制，

// (C) 2015, Bridan Wang, CC BY-NC-SA 3.0 TW
// http://4rdp.blogspot.tw/search/label/ROSA%20(Arduino)

// SERVO  SG90  pulse = 560 ~ 2000 us (0 ~ 180)
#define MAXus  2000
#define MINus  560
#define GAIN   (MAXus-MINus)/180.0
byte servo_pin[] = { 0, 4, 5, 6, 7};
byte servo_revse[] = { 0, 0, 1, 0, 0};
byte servo_angle[] = { 0, 0, 0, 0, 0};

void SERVO_SET(byte p){   // for SG90
  byte a = servo_angle[p];
  if (a >= 181) return;
  if (servo_revse[p]==1) a = 180 - a;
  D_OUT(servo_pin[p], HIGH);
  delayMicroseconds(GAIN*a+MINus);
  digitalWrite(servo_pin[p], LOW);
}

void PROCESS_10ms(){     // sweep 0 ~90 degree

  static byte dir = 0;
  
  if (dir==0){
    if (++servo_angle[1] >= 90)
      dir = 1;
  } else {
    if (--servo_angle[1] >= 180)
      dir = servo_angle[1] = 0;
  }
  SERVO_SET(1);   // servo 1

}

開發 ROSA 複雜系統，這裡並未使用大眾習慣泛用的 Servo.h 函數庫，主要原因是避免未來加入其它功能造成 Timer 或中斷衝突問題。經試驗 SG90 的驅動脈波寬度，轉 0° 為 560 us，轉 180° 為 2000 us，因此用這為基準來設計，不同型馬達的脈波寬度數值可能有些差異，請自行調整。

SERVO_SET 為伺服馬達最底層驅動副程式，為了未來方便變更伺服馬達編號次序，程式使用 servo_pin 陣列變數儲存 Arduino pin 編號，上例 servo_pin[1] = 4，也就是說編號 1 號的伺服馬達使用 Arduino pin D4，一般習慣號碼從 1 開始，所以 0 號馬達沒有被使用。變數 servo_revse 是用來設定馬達轉向，數值 0 為順時針轉向，1 為逆時針轉向。變數 servo_angle 則儲存伺服馬達目前角度資料。

這裡的例子安裝一個 SG90 馬達在 Arduino D4 pin，然後以 10 ms 為一個控制週期，讓馬達在 0°~90° 間來回擺動，而 Servo.h 是採 20 ms 控制週期，其實週期不要過短於 3 ms 通常都可以控制的。

有興趣的讀者可以嘗試下列幾個試驗：
一、擺動控制由 PROCESS_10ms 改成 PROCESS_100ms，轉速就會變慢。
二、修改 servo_pin[] = { 0, 5, 4, 6, 7}; 變成 D5 輸出。
三、修改 servo_revse[] = { 0, 1, 1, 0, 0}; 變成 90°~180° 擺動。