以 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 個 Servｏ動作的第一個參數值(_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]=='#') // ## 指令結束
{
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
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);
}
}
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;
}
}
}
}
}
沒有留言:
張貼留言