2016年4月9日 星期六

ROSA 系統開發 33 ─ 進階紅外線遙控

http://4rdp.blogspot.com/2016/04/rosa-33.html?m=0

紅外線遙控器雖然按鍵數量有限,但是仍然有變通方法可以擴充到上百種操作功能,先來看看這是怎麼辦到的?

以 KEYES 遙控器為例,它有數字鍵 0 ~ 9,可以用數字編號來擴展功能,但是平常一按鍵就執行動作,那要如何執行數字編碼的功能?這裡使用 # 鍵來區別,例如依序按下 #2#11##,第一個 # 表示指令模式開始,中間的 # 表示分隔前後指令號碼,最後 ## 結束指令模式並開始執行,這個指令串表示先執行 2 號命令,執行完畢後再繼續執行 11 號命令,至於甚麼是 2 號、 11 號命令,端看程序碼怎樣定義就怎樣執行。

為了 # 指令模式,原來的 # 按鍵功能就無法執行蠻可惜的,因此 ROSA 追加遙控器長按住一秒鐘操作,這樣可以為 # 按鍵長按住一秒設定執行原來 # 鍵蜘蛛站立功能。另外還預留重複鍵操作功能,也就是按鍵一直按住不放,按鍵長按超過一秒後,每 0.2 秒重複執行功能。

為了可以執行一串 # 命令,ROSA 追加了 PIPE 機制,讓佇列命令可以先進先出,每執行完一命令可以接續執行下一個命令,直到所有佇列命令全部執行完畢為止,不過要記得每一個命令。

另外命令程序中還新增 GOSUB 指令,可以執行副程式,也就是可以把重複的動作集結在一起,這樣能夠節省程式碼空間,不過不要忘了副程式的結尾要加 RETURN 或 STOP 指令,因為有副程式,所以 ROSA 系統要加入 STACK 機制,讓副程式執行完成後,可以回到主程式去。

關鍵程式碼如下:

// (C) 2015-2016, Bridan Wang, CC BY-NC-SA 3.0 TW
// The program is for P&B 6x2 Spider
// http://4rdp.blogspot.tw/search/label/ROSA%20(Arduino)

char gIrCommandString[16];
byte gIrCommandIndex = 0;

// 每個 Servo 所屬的動作模式 ---------------------------------------
enum {
_NONE,
_DANCE,
_GOSUB,
_GOTO,
_HOLD,
_LOOP,
_NEXT,
_RETURN,
_SERVO,
_SPEED,
_STOP,
_TEMPO,
_WAIT,
};

const byte RUN_18[] PROGMEM = {    // go sub example
//   0,   1,   2,   3,   4,   5,   6,   7,   8,   9,  10,  11,  12  
_GOSUB,   2, idl, idl, idl, idl, idl, idl, idl, idl, idl, idl, idl,   //0
_STOP,

};

const byte RUN_2[] PROGMEM = {     // spider hello
//   0,   1,   2,   3,   4,   5,   6,   7,   8,   9,  10,  11,  12  
_SERVO, idl, lif, idl, idl, idl, idl, idl, idl, idl, idl, idl, idl,   //0
_LOOP,    3, idl, idl, idl, idl, idl, idl, idl, idl, idl, idl, idl,   //1
_SERVO, fwd, idl, idl, idl, idl, idl, idl, idl, idl, idl, idl, idl,   //2 
_SERVO, mid, idl, idl, idl, idl, idl, idl, idl, idl, idl, idl, idl,   //3 
_NEXT,    3, idl, idl, idl, idl, idl, idl, idl, idl, idl, idl, idl,   //4 
_SERVO, idl, std, idl, idl, idl, idl, idl, idl, idl, idl, idl, idl,   //5 
    _RETURN,                                                              //6
};


/**********************************************************
常數宣告
************************************************************/
#define IR_CARMP3   //  IR_KEYES    //  IR_MBOT   //  IR_AUDIO  //  
#ifdef IR_KEYES
const byte cIR_Code[] PROGMEM = {  // 不加 PROGMEM 的話,會作動錯誤
//  X,  1,  2,  3,  4,  5,  6,  7,  8,  9,  0,  *,  #, up, dn, rt, lf, ok,  X,  X,  X,  X,
 idle, 22, 25, 13, 12, 24, 94,  8, 28, 90, 82, 66, 74, 70, 21, 67, 68, 64,251,252,253,254, 
};
#endif
#ifdef IR_CARMP3
const byte cIR_Code[] PROGMEM = {
//  X,  1,  2,  3,  4,  5,  6,  7,  8,  9,  0,  +,  -, EQ, F-, C+, C-, F+, >>, <<, >|, CH,
 idle, 12, 24, 94,  8, 28, 90, 66, 82, 74, 22, 21,  7,  9, 13, 71, 69, 25, 64, 68, 67, 70,
};
#endif
#ifdef IR_AUDIO
const byte cIR_Code[] PROGMEM = {
//  X,  1,  2,  3,  4,  5,  6,  7,  8,  9,  0, >|, MU, PW, EQ, C+, C-, RP, >>, <<, V-, V+
 idle, 16, 17, 18, 20, 21, 22, 24, 25, 26, 12,  2,  1,  0,  8,  6,  5,  4, 14, 13,  9, 10,
};
#endif
#ifdef IR_MBOT
const byte cIR_Code[] PROGMEM = {
//  X,  1,  2,  3,  4,  5,  6,  7,  8,  9,  0,  A,  F, up, dn, rt, lf,  *,  B,  C,  D,  E
 idle, 12, 24, 94,  8, 28, 90, 66, 82, 74, 22, 69, 13, 64, 25,  9,  7, 21, 70, 71, 68, 67,
};

#endif

const byte cIR_KeyChar[] PROGMEM = {
   '\0','1','2','3','4','5','6','7','8','9','0','*','#','U','D','R','L','K','C','D','E','F',
};

const byte cIR_Hold[] PROGMEM = {
     0,  0,  0,  7,  0,  0,  0,  0,  0,  0,  0,  0, 12,  0,  0,  0,  0,  0,  0,  0,  0,  0,
};

const byte cIR_Repeat[] PROGMEM = {
     0,  0,  0,  9,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
};

const byte *cActions[] = {
  RUN_N, // none,
  RUN_1, // 1
  RUN_2, // 2
  RUN_3, // 3
  RUN_4, // 4
  RUN_5, // 5
  RUN_6, // 6
  RUN_7, // 7
  RUN_8, // 8
  RUN_9, // 9 
  RUN_0, // 0
         // mBot     KEYES     CARMP3    AUDIO
  RUN_S, // A        *         >|        >|
  RUN_P, // F        #         CH        MENU
  RUN_U, // up       up        EQ        POWER
  RUN_D, // down     down      FL-       EQ
  RUN_R, // right    right     CH+       CH+
  RUN_L, // left     left      CH-       CH-
  RUN_K, // *        OK        FL+       RPT
  RUN_18,// B                  >>        >>
  RUN_8, // C                  <<        <<
  RUN_7, // D                  V-        V-
  RUN_9, // E                  V+        V+
};


SoftwareSerial BT(13, 3);      // pin 13 connect to BT's TX   pin 3 connect to BT's RX

TIrCommandMode gIrCommandMode = icNormal;
byte gPreIrCodeID;
byte gIrCodeID;
byte step_no = 0;
byte step_wait;
bool flag_return = false;

byte ir_key = 0;
byte ir_keynum = 0;

byte ir_none = 0;

byte  echo_index = 0;
char  echo[64] = "";

byte  command_IRpipe[16];
byte  command_IRin = 0;
byte  command_IRout = 0;
byte  command_IRstack[10];
byte  command_IRsp = 0;




void ACTION_SET(byte &actionId) {

  static byte loopn;

  byte i, servoValue, k, m;



    tempo = 1;


  //pgm_read_byte() 從一個16bit 的 address 取的 byte 值,
  //Eg:
  //如果要執行 RUN_0(initial) 的第 2 個 Servo動作的第一個參數值(_SERVO)
  //_SERVO, mid, std, idle, idle, idle, idle, idle, idle, idle, idle, idle, idle,   //Servo 2,左前下肢
  //pgm_read_byte(cActions[10] + step_no*SERVO_TABLE) ->  (pgm_read_byte( *RUN_0[] + 2*13),
  //從 RUN_0[] 起始位址,offset 2*13
  //

// Revised by Daniel
  const byte *pCurRunStepStart0 = cActions[actionId] + step_no*SERVO_TABLE; //取得 step_no 的第一個參數所在位址
  byte actionMode = pgm_read_byte(pCurRunStepStart0); //取得 step_no 的第一個參數

#ifndef DisablePcSerial
#ifdef debug_procedure
  ECHO_INT(actionId);
  ECHO_CHARS((char *)" - ");
  ECHO_INT(step_no);
  ECHO_CHARS((char *)" - ");
  ECHO_INT(actionMode);
  REPLY();
#endif
#endif

  switch (actionMode) {
  case _SERVO: // 設定 Servo
         // 逐一設定所有 Servo-------------------
    for (i = 1; i < SERVO_TABLE; i++) {
      // 從 _SERVO 後的參數開始逐一取出 Servo 角度值 ----------------------------
      servoValue = pgm_read_byte(pCurRunStepStart0 + i);
      // 如果設定的角度值 == hof  
      if (servoValue == hof)
        gServo_Hold[i] = 0;     // hold off,hof 取消暫停
      else if (servoValue == hon)
        gServo_Hold[i] = idle;   // hold on,hon 暫停
      else if (servoValue > hon && servoValue < idle) //  252 <= servoValue < idle  -> 將角度值設為 90
        gServo_SetAngle[i] = 90;
      else if (servoValue < 181)        //設定角度值
        gServo_SetAngle[i] = servoValue;
    }
    break;
  case _DANCE:
    for (i = 1; i < SERVO_TABLE; i++) {
      // 從 _SERVO 後的參數開始逐一取出 ----------------------------
      servoValue = pgm_read_byte(pCurRunStepStart0 + i);
      if (servoValue == hof)
        gServo_Hold[i] = 0;         // hold off
      else if (servoValue == hon)
        gServo_Hold[i] = idle;       // hold on
      else if (servoValue > hon && servoValue < idle) {
        gServo_SetAngle[i] = servoValue = 90;
        goto set_speed;
      }
      else if (servoValue < 181) {
        gServo_SetAngle[i] = servoValue;
      set_speed:
        if (servoValue >= gServo_Angle[i])
          k = servoValue - gServo_Angle[i];
        else
          k = gServo_Angle[i] - servoValue;
        m = tempo_time / 2 / SERVO_TABLE;
        if (k % m == 0)
          gServo_SetSpeed[i] = k / m;
        else
          gServo_SetSpeed[i] = k / m + 1;
#ifdef debug
        ECHO_INT(actionId);
        ECHO_CHARS((char *)" = ");
        ECHO_INT(step_no);
        ECHO_CHARS((char *) " speed ");
        ECHO_INT(gServo_SetSpeed[i]);
REPLY();
#endif
      }
    }
    tempo = 0;
    break;
  case _TEMPO:
    // 從 _SERVO 後的參數開始逐一取出 ----------------------------
    tempo_set = pgm_read_byte(pCurRunStepStart0 + 1);
    tempo_time = (unsigned int)10 * tempo_set;
    break;
  case _HOLD:
    for (i = 1; i < SERVO_TABLE; i++) {
      // 從 _SERVO 後的參數開始逐一取出 ----------------------------
      servoValue = pgm_read_byte(pCurRunStepStart0 + i);
      if (servoValue != idle)
        gServo_Hold[i] = servoValue;
    }
    break;
  case _SPEED:
    for (i = 1; i < SERVO_TABLE; i++) {
      // 從 _SERVO 後的參數開始逐一取出 ----------------------------
      servoValue = pgm_read_byte(pCurRunStepStart0 + i);
      if (servoValue != idle)
        gServo_SetSpeed[i] = servoValue;
    }
    break;
  case _WAIT:
    // 從 _SERVO 後的參數開始逐一取出 ----------------------------
    step_wait = pgm_read_byte(pCurRunStepStart0 + 1);
    break;
  case _NEXT:
    if (loopn != 255) {
      if (loopn == 0)
        break;
      loopn--;
    }
    // 從 _SERVO 後的參數開始逐一取出 ----------------------------
    step_no -= pgm_read_byte(pCurRunStepStart0 + 1);
    break;
  case _RETURN:
    flag_return = true;
    actionId = 0;   //gPreIrCodeID;
    goto ret;
  case _GOSUB:
    // 從 _SERVO 後的參數開始逐一取出 ----------------------------
    command_IRstack[command_IRsp++] = actionId;
   command_IRstack[command_IRsp++] = step_no + 1;
  actionId = pgm_read_byte(pCurRunStepStart0 + 1);
    step_no = 255;
    break;
  case _GOTO:
    // 從 _SERVO 後的參數開始逐一取出 ----------------------------
    servoValue = pgm_read_byte(pCurRunStepStart0 + 1);
    if (servoValue >= 200) {
      actionId = servoValue - 200;
    ret:
      step_no = 255;
    }
    else
      step_no = servoValue - 1;
    break;
  case _LOOP:
    // 從 _SERVO 後的參數開始逐一取出 ----------------------------
    loopn = pgm_read_byte(pCurRunStepStart0 + 1) - 1;
    if (loopn == 254)
      loopn++;       // 0 or 255 always loop
    break;
  case _STOP:
    actionId = 0;
step_no = 255;
  }
}


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

  //#ifdef DanielRevised
  //  constrain(servo_no, 0, SERVO_TABLE); 作動錯誤
  //#else
  if (servo_no == 0 || servo_no > SERVO_TABLE) {
    servo_no = SERVO_TABLE;
  }
  //#endif


  // servo_no 超過配置的數量--------------------------------------
  if (servo_no > LAST_SERVO)
    ;
  // servo_no 未達設定角度,需繼續轉動------------------
  else if (gServo_Angle[servo_no] < gServo_SetAngle[servo_no]) {
    gServo_Angle[servo_no] += gServo_SetSpeed[servo_no]; //以設定的速度來累加目前角度
                               // 已達設定角度------------------------------
    if (gServo_Angle[servo_no] > gServo_SetAngle[servo_no])
      gServo_Angle[servo_no] = gServo_SetAngle[servo_no]; //將目前角度,設回設定的角度
    SERVO_SET(servo_no, gServo_Angle[servo_no]); // 驅動 servo_no 舵機
  }
  // 目前的角度,超過設定的角度 ---------------------------------------
  else if (gServo_Angle[servo_no] > gServo_SetAngle[servo_no]) {
    gServo_Angle[servo_no] -= gServo_SetSpeed[servo_no]; // 以設定的速度來累減目前角度
                               // 如果目前角度 > 180 度--------------------------------------
    if (gServo_Angle[servo_no] > 180)
      gServo_Angle[servo_no] = gServo_SetAngle[servo_no]; //將目前角度,設回設定的角度
    SERVO_SET(servo_no, gServo_Angle[servo_no]);
  }
  //如果 servo_no 暫停值 <> 0 --------------------------------------------
  else if (gServo_Hold[servo_no] != 0) {
    // 如果暫停值不等於 idle -----------------
    if (gServo_Hold[servo_no] != idle)
      gServo_Hold[servo_no]--; //暫停值 累減 1 
    SERVO_SET(servo_no, gServo_Angle[servo_no]);
  }
  servo_no--;


  /* 處理紅外線訊號   */

  // 如果不是 無效訊號 -----------------------------------------
  if (gIrCodeID != 0) {

    all_ready = 1;

    // 將所有的 servo 檢查一次是否到位了 ----------------
    for (i = 1; i < LAST_SERVO + 1; i++) {
      // 如果有任一個還沒到位,則 all_ready 設為 0-----------
      if (gServo_SetAngle[i] != gServo_Angle[i])
        all_ready = 0;
    }

    // 如果全部到位----------------------------------------
    if (all_ready == 1) {
      // 如果等待值 == 0 -----------------------
      if (step_wait == 0) {
        if (tempo == 1) {
#ifdef debug
          //ECHO_INT(now);
  //REPLY();
#endif
          ACTION_SET(gIrCodeID);
          step_no++;
          //tempo = 0;
        }
      }
      // 等待值<> 0 ---------------------
      else
        step_wait--;
    }
  } else {
  if (command_IRsp != 0){
  step_no = command_IRstack[--command_IRsp];
gIrCodeID = command_IRstack[--command_IRsp];
  } else if (command_IRin != command_IRout){
  step_no = 0;
  gIrCodeID = command_IRpipe[command_IRout];
  command_IRout = (command_IRout + 1) & 0x0F;
  //ECHO_INT(gIrCodeID);
  //REPLY();
  } else if (flag_return) {
  step_no = 0;
  gIrCodeID = gPreIrCodeID;
  }
  flag_return = false;
  }
}

bool Process_IrCommandString(char chr, bool &blStringCompleted)
{
  byte i,j;
  // #12#34##
  blStringCompleted = false;

  // #
  gIrCommandMode = (chr=='#') ? icCommand : gIrCommandMode;

#ifdef debug_command
    ECHO_CHARS((char *)"gIrCommandMode: "); ECHO_INT(gIrCommandMode);
  REPLY();
#endif

  switch (gIrCommandMode)
  {
    case icNormal:
    {
      return false;
    }
    case icCommand:
    {
        gIrCommandString[gIrCommandIndex++] = chr;
        ECHO_CHARS((char *)"gIrCommandString: "); ECHO_STRING(gIrCommandString);
      REPLY();
      {
        if (gIrCommandString[gIrCommandIndex-2]=='#' && gIrCommandString[gIrCommandIndex-1]=='#') //  ## 指令結束
        {
          gIrCommandMode = icNormal;
          blStringCompleted = (gIrCommandIndex != 0);

        }
      }
      return true;
    }
    case icServos:
    {
      return false;
    }
  }
}

void Get_ActionID(char *st)
{
  byte cmdNum=0;
  byte i;

  //ECHO_STRING(gIrCommandString);
  //REPLY();

  st++;
  for (i=1 ; i < 16 ; i++){   
      if (*st != '#'){
       cmdNum = cmdNum*10 + *(st++) - '0';
       continue;

      } else {
if (*(st-1)=='#') break;
      }
        st++;
    command_IRpipe[command_IRin] = cmdNum;
    command_IRin = (command_IRin + 1) & 0x0F;
    //ECHO_CHARS((char *)"command_IRin ");
    //ECHO_INT(command_IRin);
    //REPLY();
    cmdNum = 0;
  }
}

void PROCESS_10ms(void) {
  byte i;

  if (NULL != CallBack_CheckIR)
  {
    byte keyCode = idle;
    CallBack_CheckIR(keyCode);

    if (keyCode != idle)
    {
      #ifdef debug
        ECHO_CHARS((char *)"keyCode: "); ECHO_INT(keyCode);
        REPLY();
      #endif
      //for (i = gIrCodeNum-1; i > 0; i--)
      for (i = 21; i > 0; i--)
      { // 如果 cIR_Code[i] 的值 == keyCode
        if (keyCode == pgm_read_byte(&cIR_Code[i]))
        {
          ir_none = 5;
          if (ir_keynum < 120){
            ir_keynum++;
            if (ir_keynum==5){ // press    50ms
                            #ifndef disableCommandString
                    bool blGetCompleteString = false;
                    if (Process_IrCommandString(pgm_read_byte(&cIR_KeyChar[i]), blGetCompleteString))
                    {
                      if (blGetCompleteString)
                      {
                        //處理 gIrCommandString----------------------------------
                        Get_ActionID(gIrCommandString);

                        gIrCommandIndex = 0;
                      }
                    }
                    else
                            #endif
                    {
                      gPreIrCodeID = gIrCodeID;
                      gIrCodeID = ir_key = i;
                      step_no = 0;
                    }
            } else if (ir_keynum==100){ // hold   1000ms
                i = pgm_read_byte(&cIR_Hold[i]);
              goto do_it;
              } else if (ir_keynum==120){ // repeat  200ms
                i = pgm_read_byte(&cIR_Repeat[i]);
              ir_keynum = 100;
                        do_it:
              if (i != 0){
                  //gPreIrCodeID = gIrCodeID;
                  gIrCodeID = ir_key = i;
                  step_no = 0;
                gIrCommandIndex = 0;
                gIrCommandMode = icNormal;
              }
            }
          }
          break;
        }
      }
    } else {
      if (ir_none != 0){
        if (--ir_none==0){
          ir_key = ir_keynum = 0;
        }
      }
    }
  }
}

沒有留言:

張貼留言