2015年12月5日 星期六

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

https://4rdp.blogspot.com/2015/12/rosa-16-mearm-procedure-control.html?m=0

圖片來源 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 系統開發這系列文章,務必由第一章節看過來,才能想法連貫。

沒有留言:

張貼留言