ROSA 系統開發 13 ─ SERVO 多工同步控制

http://4rdp.blogspot.com/2015/11/rosa-13-servo.html

前文談過單一伺服馬達驅動，本期續談多顆伺服馬達多工同步控制方法，

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

byte servo_set[] = { 0, 90, 90, 90, 90};

byte servo_speed[] = { 0, 5, 1, 5, 5};
byte servo_hold[] = { 0, 0, 0, 0, 0};

void PROCESS_SERVO(void) {
  static byte servo_no;
  byte i;

  if (servo_no == 0 || servo_no > 4) {
    servo_no = 4;
  }

  if (servo_set[servo_no] > servo_angle[servo_no]){
    servo_angle[servo_no] += servo_speed[servo_no];
    if (servo_set[servo_no] < servo_angle[servo_no])
      servo_angle[servo_no] = servo_set[servo_no];
    SERVO_SET(servo_no);
  }
  else if (servo_set[servo_no] < servo_angle[servo_no]){
    servo_angle[servo_no] -= servo_speed[servo_no];
    if (servo_angle[servo_no] > 180)
      servo_angle[servo_no] = servo_set[servo_no];
    SERVO_SET(servo_no);
  }
  else if (servo_hold[servo_no] != 0){
    if (servo_hold[servo_no]!=255) 
      servo_hold[servo_no]--;
    SERVO_SET(servo_no);
  }
  servo_no--;
}

unsigned long  now;

void SYS_TIMER() {
  static byte  tm_10ms = 0;
  static byte  tm_100ms = 0;
  static byte  tm_1sec = 0;

while (millis() - now >= 5) {
now += 5;
tm_10ms++;
tm_100ms++;
tm_1sec++;
    PROCESS_SERVO();
}
if (tm_1sec >= 200) {
tm_1sec -= 200;
PROCESS_1sec();
}
if (tm_100ms >= 20) {
tm_100ms -= 20;
PROCESS_100ms();
}
if (tm_10ms >= 2) {
tm_10ms -= 2;
PROCESS_10ms();
}
}

void PROCESS_1sec(void){   // print analog value for debugging
  Serial.print("A0 - ");
  Serial.println(A_IN(A0));
  Serial.print("A1 - ");
  Serial.println(A_IN(A1));
  Serial.print("A2 - ");
  Serial.println(D_IN(A2));

  Serial.print("A3 - ");
  Serial.println(A_IN(A3));
  Serial.print("A4 - ");
  Serial.println(A_IN(A4));
  Serial.print("A5 - ");
  Serial.println(D_IN(A5));
}

void PROCESS_100ms(void){
}

void PROCESS_10ms(void){
  if (A_IN(A4) < 200){   // right X
    if (servo_set[2] > 120) servo_set[2]--;
  }
  else if (A_IN(A4) >= 800){
    if (servo_set[2] < 160) servo_set[2]++;
    else servo_hold[2] = 255;
  } else
    servo_hold[2] = 0;

  if (A_IN(A0) < 200){   // left Y
    if (servo_set[4] > 60) servo_set[4]--;
  }
  else if (A_IN(A0) >= 800) {
    if (servo_set[4] < 150) servo_set[4]++;
  }
  if (A_IN(A1) < 200){   // left X
    if (servo_set[1] != 0) servo_set[1]--;
  }
  else if (A_IN(A1) >= 800){
    if (servo_set[1] < 180) servo_set[1]++;
  }
  if (A_IN(A3) < 200){   // right Y
    if (servo_set[3] > 60) servo_set[3]--;
  }
  else if (A_IN(A3) >= 800) {
    if (servo_set[3] < 150) servo_set[3]++;
  }
}



前文已提過 SG90 SERVO 控制與脈波寬度有關，但與 period 無關，meArm 使用四顆馬達，為了伺服馬達多工同步，因此系統設計以 5 ms 分時控制，各別馬達控制週期皆為 20 ms，這樣的規格設計接近 Servo.h 的控制週期，這樣設計也可以降低 meArm 多顆馬達同時動作電力消耗。

在此馬達編號規劃如下圖

左邊搖桿，水平方向控制 D4 馬達，讓手臂左右迴轉，垂直方向控制 D7 馬達，讓手臂上下運動。右邊搖桿，水平方向控制 D5 馬達，讓夾子開合，垂直方向控制 D6 馬達，讓手臂前後運動。

PROCESS_10ms() 讀取搖桿數值轉換成馬達角度設定 servo_set[] 陣列，這陣列紀錄馬達角度設定值，如果想 SG90 伺服馬達平順轉動，要用角度微調的方式控制，角度加減程式放在 10ms 控制速度較快也比較平順，100ms 速度則較慢也有點頓頓的。

PROCESS_SERVO()才是伺服馬達最底層驅動程式，這程式碼示範每 5ms 驅動一顆馬達，servo_speed[] 陣列儲存馬達速度，數值範圍建議在 1 ~ 10，值越大馬達轉越快，D5 馬達因為可控角度範圍很小，因此設定 servo_speed[2] = 1，以方便微調，servo_hold[] 是讓馬達轉到定位後，持續通電維持支撐力，hold = 255 表示一直通電，hold = 0 則是不通電，其它數值表示持續通電時間 = (hold 數值)x(4 顆馬達)x(5 ms)。


注意，馬達角度控制範圍，應以實際機台組合後測試再調整程式碼。