ROSA 系統開發 25 ─ 舞蹈節奏控制 (Tempo & Dance)

http://4rdp.blogspot.com/2016/02/rosa-25-tempo-dance.html

妖怪手錶體操舞

過年放假時間較多，想幫 P&B Spider 加一些有趣的動作，所以想試看看能不能讓機器蜘蛛跳個舞，最近很紅的舞曲是日本任天堂的妖怪手錶，看過影片以及聽了音樂後，分析它的節奏是一秒鐘一拍，回頭看 ROSA 的動作編輯，還欠缺一個時間同步，少了它，動作與音樂會對不起來，解決辦法之一就是把時間填滿，例如某個動作需半拍，而馬達旋轉花了 0.4 秒，那就要插入 0.1 秒等待指令，可是這是做苦工的方法，一旦馬達轉速或角度調整後，等待的時間也要一同修改，這樣逐一修改是很費時的。

因此，我決定在 ROSA 的動作程序裡追加 TEMPO 與 DANCE 兩個指令，這樣做的優點可以減少程序記憶空間耗用，如果每行動作程序都加一個時間欄，一百個動作就會增加 100 bytes 記憶體耗用，而增加指令雖增加處理程式，但耗用程序記憶體並不多。TEMPO 指令設定音樂一拍所需的時間，時間單位為 10 ms，數值範圍 0 ~ 255，DANCE 指令基本上同 SERVO，但它會自動調速，讓每顆伺服馬達動作盡量均速，不會讓馬達很快轉到定位後就停在那裏，空耗時間，例如一號馬達半拍轉 50 度角與二號馬達半拍轉 20 度角，兩者速度就是不一樣，那麼看看程式如何設計可以達成這些功能。

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

byte servo_pin[] =
{ 0, 4, 5, 6, 7, 9, 10, 11, 12, 14, 15, 16, 17 };
byte servo_revse[] =
{ 0, 0, 1, 1, 0, 0,  1,  1,  0,  0,  1,  1,  0 };  // 6x2-2

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

byte tempo;
byte tempo_set;
unsigned int tempo_time;


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

  while (millis() - now ＞= 2) {
    now += 2;
    tm_10ms++;
    tm_100ms++;
    if (tempo_time ＞= 2) tempo_time -= 2;
    if (tempo_time == 0) {
      tempo_time = (unsigned int)10 * tempo_set;
      tempo = 1;
    }
    PROCESS_SERVO();
  }
  if (tm_10ms ＞= 5) {
    tm_10ms -= 5;
    PROCESS_10ms();
  }
  if (tm_100ms ＞= 50) {
    tm_100ms -= 50;
    PROCESS_100ms();
    tm_1sec++;
  }
  if (tm_1sec ＞= 10) {
    tm_1sec -= 10;
    PROCESS_1sec();
  }
}


void ACTION_SET(void){
  static byte loopn;
  byte i, j, k, m;

  //Serial.print(ir_code);
  //Serial.print(" - ");
  //Serial.print(step_no);
  //Serial.print(" - ");
  //Serial.println(pgm_read_byte(ACTION[ir_code] 
                   + step_no * SERVO_TABLE));

  tempo = 1;   
  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 _DANCE:
      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] = j = 90; 

          goto set_speed; 

        } else if (j ＜ 181){ 

          servo_set[i] = j; 

set_speed: 

          if (j ＞= servo_angle[i]) 

            k = j - servo_angle[i]; 

          else 

            k = servo_angle[i]-j; 

          m = tempo_time / 2 / SERVO_TABLE; 

          if (k % m == 0) 

            servo_speed[i] = k / m; 

          else 

            servo_speed[i] = k / m + 1; 

        }       
      }
      tempo = 0;
      break;
    case _TEMPO:
      tempo_set = pgm_read_byte(ACTION[ir_code]
          + step_no * SERVO_TABLE + 1);
      tempo_time = (unsigned int) 10 * tempo_set;
      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){
if (tempo==1){
          ACTION_SET();
         step_no++;
}
      } else
step_wait--;
    }
  }
}

const byte RUN_5[] PROGMEM = {
//   0,   1,   2,   3,   4,   5,   6,   7,   8,   9,  10,  11,  12  
_TEMPO,  50, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,   //0
_DANCE,  45, zeo,  45, zeo, fwd, std, fwd, std, mid, std, mid, std,   //1
_DANCE, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,   //2
_LOOP,    3, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,   //3
_DANCE, 255, lif, 255, lif, 255, 255, 255, 255, 255, 255, 255, 255,   //4
_DANCE, 255, zeo, 255, zeo, 255, 255, 255, 255, 255, 255, 255, 255,   //5
_NEXT,    4, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,   //6
_GOTO,  212,                              // goto RUN_P

};

新程式主要修改的地方以紅字標示，本文用第二種方式組裝，雖然音樂是一拍一秒，但是有半拍動作，因此 TEMPO 應該設定成半拍時間 (500 ms)，DANCE 除了設定伺服馬達角度外，同時也會自動設定馬達的速度。此外，tempo 是串起節拍動作的重要變數，節拍時間 (tempo_time) 計數到達後，tempo 會被設為 1，表示程序可以接續執行，反之，執行 DANCE 指令，它會等節拍時間到才會自動執行下一個程序。

RUN_5 是紅外線遙控按鍵 5 的控制程序，今天只放一段前四拍妖怪手錶舞點頭的動作，等舞全部編齊再一次公佈，敬請期待。