活用手機加速度感測器與藍芽控制

批評或詆毀一個人很簡單，但請想想自己相對又為這個社會貢獻了些什麼呢? 比如說，至少只要到行政院主計總處/中華民國統計資訊網便可以得到非常詳盡的歷年國內各產業生產數據，比許多收費的產業分析機構要強多了! 不禁想多嘉獎幾次，這可能是國內做得最好最有效率的官網了，他們的Q&A非常快，很認真! 再者如，經濟部中小企業處，提供了許多創業者有用的輔導與資源。老實說，任何人來做，也不見得比現在強! 希望這個社會，能多一點鼓勵，多一點主動的貢獻。(謾罵聲之餘)

又離題了! …
若持續使用之前的命令式協定來操控(BTCom)如汽車或飛行器等難免撞牆，所以我們今天要使用到手機內建的加速度儀(Acceleration Sensor)，透過藍芽，來迅速產生對應需要較敏捷操控訊號的裝置(仍然是命令式協定，但配合加速度儀自動產生取代人工輸入)。因此，今天重點擺在如何「活用」手機加速度儀的App程式撰寫。

很多人問，天底下程式語言千百種，究竟要學哪一個? 說實話，歐吉尚是C/C++的基本教義派，問我，當然是回答學C囉! (歐吉尚也學過Basic、Java、Pascal、Perl、Tc/Tk以及一堆UNIX Shell Script) 但! 當Maker不同! 世界實在運轉得太快了! 我們必須習慣利用現有的資源，然後很快速的組成符合功能的原型(Prototype)。因此，我們選擇MIT App Inventor 2! (好啦，其實是我很懶! Fundamental的工作就交給職業的就好了，我們要的是天馬行空而有趣的應用!)

拜MEMS(micro electronic mechanical system)技術突飛猛進所賜，目前的智慧型手機幾乎都內建多樣的感測器，包括位置儀(Location Sensor，如GPS)、加速度儀(Acceleration Sensor)與方位儀(或陀螺儀; Orientation Sensor)等。關於MEMS的製造過程、原理與應用讀者可以參考下面影片。

Source: MEMS Industry Group


目前業界最常使用的加速度儀，其內部製程為電容式感測結構。藉由加速度運動造成內部如梳子狀交錯的電容極板移動，使得極板之間的雜散電容量產生等量變化的原理。將此電容變化量信號放大再交ADC(類比式轉數位式)輸出，便可以得知空間中不同方向位移的程度。有興趣的讀者可以上網參考其工作原理動畫。  

 
Source: BOSCH



一些前置作業(知識)



在這Project之前有一段小插曲，原本心血來潮想將之前的L293D改裝成具有美美的藍芽端子，因此自製了HC-06的端子線，並且把焊出來(給為插座式接口)。但TX/RX信號實在太Sensitive了，轉接兩次端子的傳輸線始終無法正確接收藍芽訊息，最後因此作罷。(改回原本醜醜的Bulmoduino板)

  





用手機加速度儀透過藍芽遙控四驅車
四驅車不是重點，今天我們的重點是要會用手機加速度儀透過藍芽傳輸控制訊號!
若問為何買四驅車? 作弊嗎? 不是，因為直接買四驅車底盤(含馬達、齒輪與輪胎)比到電子材料行拿四顆減速馬達還便宜。那為何要四顆馬達? 因為，將來的飛行器也將會是四顆馬達(轉速較快的無刷馬達)。同理，直接買四軸飛行器回來拆，應該會比你個別買四顆馬達便宜。(經濟規模的關係，除非你大量採購)
  





BOM表 (把虛擬通路也一起放進來Survey)

項目(不含運費)
台灣(台幣)
淘寶(人民幣)
Arduino UNO R3
290~450
12~40
Bluetooth HC-06
210~350
17~30
L293D Board (H-橋式電機驅動器)
180~250
12~21
4顆減速馬達(普通)含輪胎與底盤
350~550
60~80





四驅車組裝測試(開發手機App前):
在更改手機App從原本的鍵盤命令控制(BTCom)到利用手機加速度儀的自動命令產生控制器前，我們需要先測試將來想要輸出的命令格式(Protocol)是否能正確控制機電組，因此可以沿用命令列式的BTCom，並調整輸出格式的長相與符合馬達輸出規格的參數值等到適當的設定為止。例如，每顆馬達的PWM控制都非線性，同樣的設定轉速也都不同，這需要花時間(加速感應器值範圍需另外調整)。

仍然沿用之前的DIY實作Blumoduino控制板的程式，手機傳給Arduino的指令格式也不變，提供9個指令，可個別控制四顆輪子前進、後退、轉速與停止。

<Command><逗號><轉速0~255>
例如:
frf,150    右前輪forward,轉速150
flf,150    左前輪forward,轉速150
frb,150    右前輪backward,轉速150
flb,150    左前輪backward,轉速150
brf,150    右後輪forward,轉速150
blf,150    左後輪forward,轉速150
brb,150    右後輪backward,轉速150
blb,150    左後輪backward,轉速150
x,       停止


為簡化程式碼我們直使用AF_Motor函式庫，想知道更多細節的人請參考上一篇DIY實作Blumoduino控制板。程式部分取消了伺服馬達機電，有需要的朋友可以使用Pin9與Pin10(LD293接腳已經焊死了)當PWM輸出控制。

藍芽通訊的部份，因為L293D沒有多餘的Pin腳可供軟體模擬，所以直接與實體串列埠(TX/RX)共用。因此必須先將軟體燒錄完之後，才能將藍芽模組接上!


//#include <Servo.h>
#include <AFMotor.h>

#define MAX_UARTCMDLEN 128
#define SERVO1_PWM 10 // for L293D motor shield
#define SERVO2_PWM 9  // for L293D motor shield

// up to 128 bytes UART command received from the Android system
byte uartCmdBuff[MAX_UARTCMDLEN];
int uartCmdLen = 0; // received BT command length

// DC減速馬達 Available PWM frequency:
//     1KHz, 2KHz, 8KHz and 64KHz for motors 1,2; 
//     1KHz, 8KHz and 64KHz for motors 3,4
// The valid values for speed are between 0 and 255.
AF_DCMotor motorFR(1,MOTOR12_8KHZ); // FR
AF_DCMotor motorFL(2,MOTOR12_8KHZ); // FL
AF_DCMotor motorBR(4,MOTOR34_8KHZ); // BR
AF_DCMotor motorBL(3,MOTOR34_8KHZ); // BL


void setup() {
    Serial.begin(9600); // connect to the HC_06, after downloading the software
    motorFR.run(RELEASE);
    motorFL.run(RELEASE);
    motorBR.run(RELEASE);
    motorBL.run(RELEASE);
}

void loop() {
    if ( listenSerialCmd()>0 )
        executeSerialCmd();
}

void executeSerialCmd() {
    char cmd[MAX_UARTCMDLEN];
    char* p = NULL;
    int value = 0;
           
    // parse UART command
    sprintf(cmd,"%s",uartCmdBuff);
    if ( (p=strchr(cmd,','))==NULL )
        return;
    *p = '\0';
   
    // get speed
    value = atoi(p+1);
   
    // for debugging
    switch ( cmdCode(cmd) ) {
        case 0: // front right DC motor forward
            motorFR.run(RELEASE);
            motorFR.setSpeed(value);
            motorFR.run(FORWARD);
            break;
        case 1: // front right DC motor backward
            motorFR.run(RELEASE);
            motorFR.setSpeed(value);
            motorFR.run(BACKWARD);
            break;   
       case 2: // front left DC motor forward
            motorFL.run(RELEASE);
            motorFL.setSpeed(value);
            motorFL.run(BACKWARD);
            break;
        case 3: // front left DC motor backward
            motorFL.run(RELEASE);
            motorFL.setSpeed(value);
            motorFL.run(FORWARD);
            break;    
       
        case 4: // back right DC motor forward
            motorBR.run(RELEASE);
            motorBR.setSpeed(value);
            motorBR.run(BACKWARD);
            break;
        case 5: // back right DC motor backward
            motorBR.run(RELEASE);
            motorBR.setSpeed(value);
            motorBR.run(FORWARD);
            break;   
       case 6: // back left DC motor forward
            motorBL.run(RELEASE);
            motorBL.setSpeed(value);
            motorBL.run(FORWARD);
            break;
        case 7: // back left DC motor backward
            motorBL.run(RELEASE);
            motorBL.setSpeed(value);
            motorBL.run(BACKWARD);
            break;    
        case 8:
        default:
            motorFR.run(RELEASE);
            motorFL.run(RELEASE);
            motorBR.run(RELEASE);
            motorBL.run(RELEASE);
            break;
    }
}

// for debugging
int cmdCode(char* cmd) {
    int ii;
    char* cmdHash[] = {
        "frf",  // 0
        "frb",  // 1
        "flf",  // 2
        "flb",  // 3
        "brf",  // 4
        "brb",  // 5
        "blf",  // 6
        "blb"   // 7
    };
    for ( ii=0; ii<8; ii++ ) {
        if ( strcmp(cmdHash[ii],cmd)==0 ) {
            return ii;
        }
    }
    return 8;
}

int listenSerialCmd() {
    char tmp; 
    uartCmdLen = 0;
    memset(uartCmdBuff,0,MAX_UARTCMDLEN);
    while( Serial.available()>0 ) {
        if( (tmp=Serial.read())=='0' ){
            uartCmdLen = 0;
        }
        uartCmdBuff[(uartCmdLen++)%MAX_UARTCMDLEN] = tmp;
        delay(5); // wait RX signal
    }
    return uartCmdLen;
}






Android App手機程式規劃
手機程式從之前的App藍芽控制程式(BTCom)，擴展成讀取手機內建的三軸加速度感測器的值，並自動產生透過藍芽對應馬達驅動的指令。當然，這比前面測試用鍵盤敲指令要快多了! 操控也比較直覺。

新版的藍芽控制指令由偵測加速度儀變化產生，以「accel」命令開頭緊接著x、y與z三軸的加速值(目前z軸不使用)，格式如下:

<Command>,<xAccel -10~10>,<yAccel -10~10>,<zAccel>
例如:
accel,0,-5,9   x、y與z三軸的加速值分別為0,-5,9 (手機往前傾)
accel,0,5,9    x、y與z三軸的加速值分別為0,5,9 (手機往後傾)
accel,-5,0,9   x、y與z三軸的加速值分別為-5,0,9 (手機往右傾)
accel,5,0,9    x、y與z三軸的加速值分別為5,0,9 (手機往左傾)


懶的寫程式的朋友可以使用下面的QR Code下載執行檔，安裝方式請參閱前幾回的Android與Arduino的藍芽通訊。


BTAccelCom – Designer View
App程式名稱之為BTAccelCom，長相規劃如下圖，除了之前可用鍵盤敲指令的BTCom Layout之外，我們顯示目前三軸加速度感測器變量的Label、一個用來表示目前手機水平的Canvas(用來顯示游標)與根據目前加速度感測器變量所產生的藍芽指令。其中還包括四個不可視物件，分別為三軸加速度感測器(主角)、藍芽模組、聲音播放模組與時鐘模組。

聲音模組是判斷當游標到達螢幕邊界時，用來發出警示聲。而時鐘模組則是用來限制每次偵測三軸加速度儀變量的時間間隔(本程式設定為500ms偵測一次)，因為感測器的反應時間實在是太快了(以Millisecond的解析度)! 若沒加以限制，藍芽命令的Queue很快就塞爆了! 此外，馬達反應根本跟不上，直接擺爛給你看!
 



BTAccelCom – Block View
App程式我們宣告一個儲存藍芽指令的字串、用來限制感測器存取的時鐘旗標以及記錄三軸加速度儀變量的全域變數。


跟之前的BTCom程式一樣，我們希望在藍芽裝置配對成功前將所有功能暫時取消(false)，一旦取得藍芽裝置便回復所有功能(true)，直到Disconnect按鍵被觸發。因為同樣的設定會重複被使用許多次，所以我們將這些動作獨立成一個Procedure Call，稱之為AccelCtrlView。


程式中另一個會不斷被重複執行的程序UpdateCanvas，是用來「顯示游標的位置」，其根據三軸加速度的變量，不斷地在Canvas上畫出目前的游標的位置，因此也將其Proceduralized! 以筆者的手機而言，三軸加速度的變量範圍約為-10~10，而螢幕大小約320x320像素，因此取加速度儀變量的倍數(16x)來畫圖。讀者應根據自己的手機，實際了解其內建的加速度儀範圍並做調整。


第三個Procedure UpdateAccelValue用來判斷三軸加速度值是否有變化，以節省不必要的藍芽指令產生，其中也判斷游標是否超出所設定的範圍並發出警示聲。產生的藍芽指令會以<accel,x,y,z>的格式先存入BTCom字串變數中，待時鐘旗號變化時(產生指令的最小時間間隔)，再交由藍芽模組輸出。


偵測三軸加速度感測器變化的程式如下圖，其中浮點數的加速度值被Normalized成整數之後再呼叫前述之UpdateAccelValue判斷是否產生新的藍芽指令，若是則呼叫藍芽模組傳送之。


最後是藍芽模組的部份，在藍芽裝置配對成功前，透過List元件列出所有藍芽信號範圍內(約15公尺)所有裝置，此時將所有功能暫時取消(false)。一旦取得藍芽裝置後(使用者選取)便回復所有功能(true)，直到Disconnect按鍵被觸發，此時傳送讓所有馬達都停止的指令<accel,0,0,0>。




Arduino Uno端系統與程式規劃
Arduino系統端的程式則是將上述的「加速度值」各別對應為四顆輪子的轉向與轉速。比較麻煩的是，經過BTCom的測試，你會發現四個輪子對PWM信號都非線性，而且對應的值也都非等量，因此需花時間調整參數。像我就很懷疑自己可能買到瑕疵品，其中一顆(測試成品的右後輪)馬達轉速明顯較其他三顆慢，還需要加100左右的Bias才能稍微平衡。

本程式部分，以「y軸加速度值」為優先判斷順序，以決定車子的方向是前進或後退(偷懶就對了)。其次才讀取「x軸加速度值」，用來增減四顆輪子的轉速，以輔助車子轉向。Z軸我們就暫不考慮了，由於重力的關係，會一直讀到一個g值。

Android手機的三軸加速度值被Normalized在-10到+10之間，而馬達PWM信號強度則是0到255，因此對應加速度值的增量變化我們可以放大25倍左右。(若想讓馬達反應更大些，值可以加大，本程式用50)



//#include <Servo.h>
#include <AFMotor.h>

#define MAX_UARTCMDLEN 128
//Servo servo; // 伺服器馬達, 紅色(VDD), 棕色(GND), 橙色(Signal)
#define SERVO1_PWM 10 // for L293D motor shield
#define SERVO2_PWM 9  // for L293D motor shield

// up to 128 bytes UART command received from the Android system
byte uartCmdBuff[MAX_UARTCMDLEN];
int uartCmdLen = 0; // received BT command length

// DC減速馬達 Available PWM frequency:
//     1KHz, 2KHz, 8KHz and 64KHz for motors 1,2; 
//     1KHz, 8KHz and 64KHz for motors 3,4
// The valid values for speed are between 0 and 255.
AF_DCMotor motorFR(1,MOTOR12_8KHZ); // FR
AF_DCMotor motorFL(2,MOTOR12_8KHZ); // FL
AF_DCMotor motorBR(4,MOTOR34_8KHZ); // BR
AF_DCMotor motorBL(3,MOTOR34_8KHZ); // BL

int motorDir = FORWARD;
int motorStep = 50;
int motorSpeed[4] = {0,0,0,0}; // FR, FL, BL, BR
int gAccel[3] = {0,0,0}; // accel x,y,z values


void setup() {
    Serial.begin(9600); // connect to the HC_06, after downloading the software
    motorFR.run(RELEASE);
    motorFL.run(RELEASE);
    motorBR.run(RELEASE);
    motorBL.run(RELEASE);
}

void loop() {
    if ( listenSerialCmd()>0 )
        executeSerialCmd();
}

void executeSerialCmd() {
    char cmd[MAX_UARTCMDLEN];
    char* token = NULL;
    char* p = NULL;
    int accel[3] = {0,0,0}; // accel x,y,z values
           
    // parse UART command
    sprintf(cmd,"%s",uartCmdBuff);
    if ( (p=strchr(cmd,','))==NULL )
        return;
    *p = '';
   
    // check if the accel command, the format is <accel,x,y,z,>
    if ( strcmp(cmd,"accel") )
        return;
       
    // get speed
    for ( int ii=0; ii<3; ii++ ) {
        token = p+1;
        if ( (p=strchr(token,','))==NULL )
            break;
        *p = '';
        accel[ii] = atoi(token);
    }
   
    if ( !(accel[0]!=gAccel[0] || accel[1]!=gAccel[1]) )
        return; // no change (ignore z axis)
   
    gAccel[0] = accel[0];
    gAccel[1] = accel[1];
    gAccel[2] = accel[2];
    sprintf(cmd,"X: %d, Y: %d, Z: %d",accel[0],accel[1],accel[2]);
    Serial.println(cmd);
   
    // check if change direction
    if ( (accel[1]<0 && motorDir!=FORWARD) || (accel[1]>0 && motorDir!=BACKWARD) ) {
        motorFR.run(RELEASE);
        motorFL.run(RELEASE);
        motorBR.run(RELEASE);
        motorBL.run(RELEASE);
        delay(250);
    }
    motorDir = accel[1]<0?FORWARD:BACKWARD; // update direction
   
    // update speed, forward/backward depends on the yAccel
    motorSpeed[0] = motorSpeed[1] = motorSpeed[2] = motorSpeed[3] = abs(accel[1])*motorStep;

    // adjust motor speed, DC馬達的轉速反應與程式的RPM不是線性的
    if ( motorSpeed[3]>50 )
        motorSpeed[2] += 100; // BR
   
  #if 1 // update steering, right/lef depends on to the xAccel
    if ( accel[0]<0 ) { // right
        motorSpeed[0] = motorSpeed[0] - abs(accel[0])*motorStep; // FR
        motorSpeed[1] = motorSpeed[1] + abs(accel[0])*motorStep; // FL
        motorSpeed[2] = motorSpeed[2] - abs(accel[0])*motorStep; // BR
        motorSpeed[3] = motorSpeed[3] + abs(accel[0])*motorStep; // BL
    }
    else { // left
        motorSpeed[0] = motorSpeed[0] + abs(accel[0])*motorStep; // FR
        motorSpeed[1] = motorSpeed[1] - abs(accel[0])*motorStep; // FL
        motorSpeed[2] = motorSpeed[2] + abs(accel[0])*motorStep; // BR
        motorSpeed[3] = motorSpeed[3] - abs(accel[0])*motorStep; // BL
    }
  #endif
   
    // update motor speed
    motorSpeed[0] = motorSpeed[0]>250?250:(motorSpeed[0]<0?0:motorSpeed[0]);
    motorSpeed[1] = motorSpeed[1]>250?250:(motorSpeed[1]<0?0:motorSpeed[1]);
    motorSpeed[2] = motorSpeed[2]>250?250:(motorSpeed[2]<0?0:motorSpeed[2]);
    motorSpeed[3] = motorSpeed[3]>250?250:(motorSpeed[3]<0?0:motorSpeed[3]);
   
    //sprintf(cmd,"FR:%d, FL:%d, BR:%d, BL:%d",motorSpeed[0],motorSpeed[1],motorSpeed[2],motorSpeed[3]);
    //Serial.println(cmd);
    motorFR.setSpeed(motorSpeed[0]);
    motorFL.setSpeed(motorSpeed[1]);
    motorBR.setSpeed(motorSpeed[2]);
    motorBL.setSpeed(motorSpeed[3]);
    motorFR.run(motorDir);
    motorFL.run(motorDir);
    motorBR.run(motorDir);
    motorBL.run(motorDir);
    delay(250);
   
}

int listenSerialCmd() {
    char tmp; 
    uartCmdLen = 0;
    memset(uartCmdBuff,0,MAX_UARTCMDLEN);
    while( Serial.available()>0 ) {
        if( (tmp=Serial.read())=='O' ){
            uartCmdLen = 0;
        }
        uartCmdBuff[(uartCmdLen++)%MAX_UARTCMDLEN] = tmp;
        delay(5); // wait RX signal
    }
    return uartCmdLen;
}





測試結果，兩顆9V推不太動(不到五分鐘電壓就降到剩8V)，可能要改用四驅車專用電池?





我常常跟朋友說: 「Kickstarter上的好創意，一個月後淘寶就有在賣了!」 世界運轉的比你想像的還要快! 或許台灣的製造業有機會搭著這班順風車，由 Fab 轉型成為Incubator、Accelerator，讓更多青年的創意變成創業! 走出代工，共創價值。
願共勉之!








訂閱： 文章 (Atom)