2016年2月13日 星期六

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

https://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 的控制程序,今天只放一段前四拍妖怪手錶舞點頭的動作,等舞全部編齊再一次公佈,敬請期待。

沒有留言:

張貼留言