ROSA 系統開發 16 ─ meArm 程序控制 (Procedure Control)

http://4rdp.blogspot.com/2015/12/rosa-16-mearm-procedure-control.html

圖片來源 http://www.shutterstock.com

本章節主要在說明程序控制，那甚麼是程序控制？無論任何複雜機器人、機器手臂，其動作控制可以分解成許多時間串聯組合的單一小動作。以 meArm 為例，想要用它夾取物品，那需要先基座馬達轉到定位，然後手臂伸過去，夾子再夾起來，這些動作控制跟時間有關，前一個動作尚未完成前，是不能執行下一個動作的，不然就會看到機器人不聽使喚亂動一通，另外，設計機器人程序動作程式時，絕大多數系統非常複雜，有時候想增刪個小動作，可能整篇程式都修改。因此，ROSA 提供一個簡單方法，可以先將機器人動作編輯好，以代碼的型式儲存常數，未來無論是放在程式碼中或是 EEPROM，都可隨時取出依據程序控制動作。





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

#define LAST_SERVO   4
#define SERVO_TABLE  5
byte step_wait;

enum {
  _NONE,
  _GOTO,
  _HOLD,
  _LOOP,
  _NEXT,
  _RETURN,
  _SERVO,
  _SPEED,
  _STOP,
  _WAIT,
};

const byte RUN_N[] PROGMEM = {   // meArm stop

  _STOP,                         //1
};

const byte RUN_K[] PROGMEM = {   // base sweep
  _LOOP,   10, 255, 255, 255,    //0 

  _SERVO,   0, 255, 255, 255,    //1 

  _SERVO, 180, 255, 255, 255,    //2 

  _NEXT,    1, 255, 255, 255,    //3 

  _STOP,                         //4 
};

const byte RUN_5[] PROGMEM = {   // speed fast

  _SPEED, 5, 5, 5, 5,            //0 

  _RETURN,                       //1 
};

const byte RUN_8[] PROGMEM = {   // speed slow
  _SPEED, 1, 1, 1, 1,            //0 

  _RETURN,                       //1 
};

const byte RUN_0[] PROGMEM = {   // meArm reset

  _SERVO, 90,  90,  90,  90,     //0 
  _HOLD,  10,  10,  10,  10,     //1 
  _RETURN,                       //2
};

const byte *ACTION[] = {

  RUN_N,           // none,
  RUN_N, // RUN_1, // 1
  RUN_N, // RUN_2, // 2
  RUN_N, // RUN_3, // 3
  RUN_N, // RUN_4, // 4
  RUN_5,           // 5
  RUN_N, // RUN_6, // 6
  RUN_N, // RUN_7, // 7
  RUN_N, // RUN_8, // 8
  RUN_N, // RUN_9, // 9,  
  RUN_0,           // 0
  RUN_N, // RUN_S, // *
  RUN_N, // RUN_P, // #
  RUN_N, // RUN_U, // up
  RUN_N, // RUN_D, // down
  RUN_N, // RUN_R, // right
  RUN_N, // RUN_L, // left
  RUN_K            // ok
};

void ACTION_SET(void){

  static byte loopn;

  byte i, j;

  switch (pgm_read_byte(ACTION[ir_code] + step_no*SERVO_TABLE)){
    case _SERVO:
      for (i = 1; i < SERVO_TABLE; i++){
j = pgm_read_byte(ACTION[ir_code] 
            + step_no * SERVO_TABLE + i);
if (j == 250)
         servo_hold[i] = 0;         // hold off
else if (j == 251)
         servo_hold[i] = 255;       // hold on
else if (j >=252 && j < 255)
         servo_set[i] = 90;
else if (j < 181)
         servo_set[i] = j;
      }
      break;

    case _HOLD:

      for (i = 1; i < SERVO_TABLE; i++){
j = pgm_read_byte(ACTION[ir_code] 
            + step_no * SERVO_TABLE + i);
if (j != 255) 
          servo_hold[i] = j;
      }
      break;
    case _SPEED:
      for (i = 1; i < SERVO_TABLE; i++){
j = pgm_read_byte(ACTION[ir_code] 
            + step_no * SERVO_TABLE + i);
if (j != 255) 
          servo_speed[i] = j;
      }
      break;
    case _WAIT:
      step_wait = pgm_read_byte(ACTION[ir_code] 
                  + step_no * SERVO_TABLE + 1);
      break;
    case _RETURN:
      ir_code = pre_ir;
      goto ret;
    case _NEXT:
      if (loopn != 255) {
if (loopn == 0) 
          break;
        loopn--;
      }
    case _GOTO:
      j = pgm_read_byte(ACTION[ir_code] 
          + step_no * SERVO_TABLE + 1);
      if (j >= 200){
        ir_code = j - 200;
ret:
step_no = 255;
      } else
        step_no = j - 1;
      break;
    case _LOOP:
      loopn = pgm_read_byte(ACTION[ir_code] 
              + step_no * SERVO_TABLE + 1) - 1;
      if (loopn==254) 
        loopn++;       // 0 or 255 always loop
      break;
    case _STOP:
      ir_code = 0;
  }
}

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

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

  if (servo_no > LAST_SERVO)
    ;
  else 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--;

  if (ir_code != 0) {
    all_ready = 1;
    for (i = 1; i < LAST_SERVO+1; i++){
      if (servo_set[i] != servo_angle[i])
all_ready = 0;
    }
    if (all_ready == 1){
      if (step_wait == 0){
ACTION_SET();
step_no++;
      }
      else
step_wait--;
    }
  }
}

在這程式宣告 _NONE, _GOTO, _HOLD, ... 等常數，是預備未來 ROSA 通訊指令及檔案使用，這些常數現在先放在 FUNC_X[] 中當前導指令，每一行設定可以控制機器人的動作，簡單的說，這可以預先將一些機器人動作寫在 ROSA 程式內，想增減或改變動作時，不必大幅修改控制程式，只要把編修 FUNC_X[] 內的控制碼就可以達到目的。

ROSA 將程序控制與紅外線遙控器碼結合，舉例說明，RUN_K[] 是在處理 OK 按鍵程序控制，第零行設定跑 LOOP 十圈，第一行將 SERVO 1 馬達轉至 0 度角，其它 SERVO 角度設定不變更，當 SERVO 1 馬達轉到 0 度角後，再執行第二行轉到 180 度角，等馬達轉定位後再執行第三行 NEXT 指令，在 LOOP 十圈沒跑完前，都會跳到第一行執行 SERVO 1 馬達轉動， 當跑完十圈後，就會停在第四行 STOP 指令上。

這裡提供簡單範例，按鍵 5 執行 RUN_5[] 是將馬達速度設為一般速度，按鍵 8 執行 RUN_8[] 設定馬達速度為慢速，按鍵 0 執行 RUN_0[] 將 meArm 重置於初始狀態。

*ACTION[] 儲存 RUN_X[] 程序指標，副程式 ACTION_SET 主要是解譯 RUN_X[] 內的程序代碼，副程式 PROCESS_SERVO 內的藍字部分是追加的，它的功能為確定每個 SERVO 馬達都轉到定位才會執行下一個程序，step_no 是 RUN_X[] 的指標，如果正在執行動作程序，那 ir_code 會一直紀錄紅外線遙控器碼，直到該動作程序完成才會被清除為零，或是按其它遙控器按鍵取代原值，pre_ir 主要用來識別按鍵是連續按還是一下一下按。

從這裡範例，希望讓讀者理解，一個複雜程式就是從最前頭的基本功能逐步加入設計，而且系統結構會隨功能需求調整，請各位閱讀 ROSA 系統開發這系列文章，務必由第一章節看過來，才能想法連貫。