現在は、画像解析を実装したロボット制御プログラムを作ってみます。
具体的には、赤色と黄色のポールがあって、こんな感じで進む制御プログラムを作ってます。
本当はこんな感じで動いてほしいんだけど、
現在の手持ちのアルゴリズムではちょっと動きそうにない。
加えて、試走日は明日な上にロボットが今手元に無いからテストできないorz。
もっと前々からちゃんとプログラミングしとくんだった。 orz orz orz
最後に、現在のプログラムはこんな感じです。
念のため、もうちょっとアルゴリズム増やそうかなぁ。
#include "eyebot.h" #include "wabotcam.h" #include <malloc.h> /* 設定変数 */ #define RED 0 #define YELLOW 1 #define NUM_MATCH 500 // 色相一致の判断基準(範囲:0<X<4800) #define L_STOP 128 // 左車輪停止 #define L_WALK 131 // 左車輪徐行スピード #define L_RUN 138 // 左車輪走行スピード #define L_BACKWARD 125 // 左車輪後退スピード #define R_STOP 128 // 右車輪停止 #define R_WALK 125 // 右車輪徐行スピード #define R_RUN 118 // 右車輪走行スピード #define R_BACKWARD 131 // 右車輪後退スピード #define C_CENTER 128 // カメラ正面 #define C_LEFT 230 // カメラ左向き #define C_RIGHT 25 // カメラ右向き #define C_LEFT_MAX 255 // カメラ左向き最大値 #define C_RIGHT_MAX 0 // カメラ右向き最大値 #define C_TURN 30 // カメラ自動修整時の回転角度 (使用ステージ例:第5) #define C_LEFT_CENTER 35 // 目標物の左右位置の識別基準値 (使用ステージ例:第5) #define C_CENTER_RIGHT 45 // 目標物の左右位置の識別基準値 (使用ステージ例:第5) #define WAIT_TIME 500 // 旋回する時間(使用ステージ例:第2) #define FIRST_STAGE 1 // 最初のステージ (第0ステージに遷移すると終了) #define STAGE1_NEXTSTAGE 2 // 第1ステージの次ステージ #define STAGE2_NEXTSTAGE 3 // 第2ステージの次ステージ #define STAGE3_NEXTSTAGE 0 // 第3ステージの次ステージ #define STAGE4_NEXTSTAGE 5 // 第4ステージの次ステージ #define STAGE5_NEXTSTAGE 0 // 第5ステージの次ステージ /* 基本課題に対するルート案 * [1]: 0 -> 1 -> 2 -> 3 -> 0 * [2]: 0 -> 1 -> 4 -> 5 -> 0 * * 応用課題に対するルート案 * [1]: なし。 */ /* ここまで */ void run(bitset **color); void showStageRoot(); int main(){ bitset **color; int num; int key; int endFlag = FALSE; CAMInit(NORMAL); // カメラの初期化 CAMMode(AUTOBRIGHTNESS); // while(endFlag != TRUE){ LCDClear(); LCDPrintf("\n"); LCDPrintf("[Menu Display]\n"); LCDPrintf("GET: set color.\n"); LCDPrintf("RUN: run EyeBot.\n"); LCDPrintf("END: end program.\n"); LCDMenu( "GET", "", "RUN", "End" ); key = KEYGet(); /* メインメニュー画面 */ switch(key){ case KEY1: // 色相取得メニューに遷移 LCDClear(); color = COLORSetup(&num); // 画面一杯の単一色を撮影し、colorに格納する。複数個取得することも可能。 COLORInfo(color, num); // bs に格納されているすべての色についての情報を LCD に表示します。 break; case KEY2: // break; case KEY3: // EyeBotを走行状態に遷移 run(color); break; case KEY4: // プログラムの終了状態に遷移 endFlag = TRUE; break; default: LCDPrintf("\nError:default\n"); } } // 解放処理 CAMRelease(); COLORRelease(color, num); // bsを開放し、収集した色相情報をすべて破棄します。 return 0; } /* run(): 設定変数に基づいて、EyeBotを走らせる関数 */ void run(bitset **color){ int stageNumber = 0; int match, missMatch; int key = KEY1; int i, j; int osWaitTime; int leftEdge; int rightEdge; int centerPosition; int headAngle; colimage pic; BYTE hue; /* 各サーボのインスタンス生成 */ ServoHandle cameraHandle, rightHandle, leftHandle; cameraHandle = SERVOInit( SERVO10 ); // カメラ rightHandle = SERVOInit( SERVO11 ); // 右車輪 leftHandle = SERVOInit( SERVO12 ); // 左車輪 while ( key != KEY4 && KEYRead() != KEY4 ) { switch(stageNumber){ /* 第0ステージ:サブメニュー画面 & 各サーボの初期化*/ case 0: SERVOSet(cameraHandle, C_CENTER); headAngle = C_CENTER; SERVOSet(rightHandle, R_STOP); SERVOSet(leftHandle, L_STOP); LCDClear(); LCDPrintf("I'm ready!\n\n"); LCDPrintf("1: Start\n"); LCDPrintf("2: Show Stage Root\n"); LCDPrintf("3: \n"); LCDMenu( "1", "2", "3", "Back" ); key = KEYGet(); AUBeep(); if(key == KEY1) stageNumber = FIRST_STAGE; else if(key == KEY2) showStageRoot(); break; /* 第1ステージ:カメラ右向き & 1番目に取得した色相を発見するまで直進 */ case 1: SERVOSet(cameraHandle, C_RIGHT); SERVOSet(rightHandle, R_RUN); SERVOSet(leftHandle, L_RUN); match=missMatch=0; CAMGetColFrame(&pic, 0); /* 撮影した画像の各画素を色相値に変換 */ for (j = 1; j < imagecolumns - 1; j++) { for (i = 1; i < imagerows - 1; i++) { // RGB値から色相値を算出します。算出できない場合、 definehue というグローバル変数を 0 にセットします。 hue = rgb2hue(pic[i][j][0], pic[i][j][1], pic[i][j][2]); /***** ここでエラーが起きるかも! *****/ if (definehue && isHueArea(color[RED], hue)) { //v色相値 hue が色相情報 bs において認識できているかを返します。 match++; // 認識できた色相数をインクリメント // 認識できなかった場合は黒色に変え、認識できなかった色相数をインクリメント }else{ pic[i][j][0]=255; pic[i][j][1]=255; pic[i][j][2]=255; missMatch++; } } } LCDClear(); LCDMenu( " ", " ", " ", "Back" ); LCDPrintf("[Stage 1]\n"); LCDPrintf("RED = %d\n", match); LCDPrintf("Other = %d\n", missMatch); if(match > NUM_MATCH){ AUBeep(); stageNumber = STAGE1_NEXTSTAGE; } break; /* 第2ステージ:一定時間(WAIT_TIME)の間、右旋回 */ case 2: SERVOSet(rightHandle, R_WALK); SERVOSet(leftHandle, L_RUN); LCDClear(); LCDMenu( " ", " ", " ", "Back" ); LCDPrintf("[Stage 2]\n"); LCDPrintf("Now turning...\n"); osWaitTime = OSWait(WAIT_TIME); /***** サーボも止まるかも! *****/ AUBeep(); stageNumber = STAGE2_NEXTSTAGE; break; /* 第3ステージ:カメラ右向き & 2番目に取得した色相を発見するまで直進 */ case 3: SERVOSet(cameraHandle, C_RIGHT); SERVOSet(rightHandle, R_RUN); SERVOSet(leftHandle, L_RUN); match = missMatch = 0; CAMGetColFrame(&pic, 0); for (j = 1; j < imagecolumns - 1; j++) { for (i = 1; i < imagerows - 1; i++) { hue = rgb2hue(pic[i][j][0], pic[i][j][1], pic[i][j][2]); /***** ここでエラーが起きるかも! *****/ if (definehue && isHueArea(color[YELLOW], hue)) { match++; }else{ pic[i][j][0]=255; pic[i][j][1]=255; pic[i][j][2]=255; missMatch++; } } } LCDClear(); LCDMenu( " ", " ", " ", "Back" ); LCDPrintf("[Stage 3]\n"); LCDPrintf("YELLOW = %d\n", match); LCDPrintf("Other = %d\n", missMatch); // LCDPutColorGraphic( &pic ); // LCDに取得した画像を表示 if(match > NUM_MATCH){ AUBeep(); stageNumber = STAGE3_NEXTSTAGE; } /* 第4ステージ:カメラ正面 & 2番目に取得した色相を発見するまで右旋回 */ case 4: SERVOSet(rightHandle, R_WALK); SERVOSet(leftHandle, L_RUN); match=missMatch=0; CAMGetColFrame(&pic, 0); for (j = 1; j < imagecolumns - 1; j++) { for (i = 1; i < imagerows - 1; i++) { hue = rgb2hue(pic[i][j][0], pic[i][j][1], pic[i][j][2]); /***** ここでエラーが起きるかも! *****/ if (definehue && isHueArea(color[YELLOW], hue)) { match++; }else{ pic[i][j][0]=255; pic[i][j][1]=255; pic[i][j][2]=255; missMatch++; } } } LCDClear(); LCDMenu( " ", " ", " ", "Back" ); LCDPrintf("[Stage 4]\n"); LCDPrintf("Now turning...\n"); LCDPrintf("YELLOW = %d\n", match); LCDPrintf("Other = %d\n", missMatch); if(match > NUM_MATCH){ AUBeep(); stageNumber = STAGE4_NEXTSTAGE; } stageNumber = STAGE2_NEXTSTAGE; break; /* 第5ステージ:カメラ自動微調整 & 自動方向転換 & 2番目に取得した色相を見失うまで直進 */ case 5: SERVOSet(rightHandle, R_RUN); SERVOSet(leftHandle, L_RUN); match = missMatch = 0; rightEdge = leftEdge = -1; CAMGetColFrame(&pic, 0); for (j = 1; j < imagecolumns - 1; j++) { for (i = 1; i < imagerows - 1; i++) { hue = rgb2hue(pic[i][j][0], pic[i][j][1], pic[i][j][2]); /***** ここでエラーが起きるかも! *****/ if (definehue && isHueArea(color[YELLOW], hue)) { match++; if(rightEdge == -1){ rightEdge = j; }else{ leftEdge = j; } }else{ pic[i][j][0]=255; pic[i][j][1]=255; pic[i][j][2]=255; missMatch++; } } } LCDClear(); LCDMenu( " ", " ", " ", "Back" ); LCDPrintf("[Stage 5]\n"); LCDPrintf("YELLOW = %d\n", match); LCDPrintf("Other = %d\n", missMatch); /* 目標物補足 */ if(match > NUM_MATCH){ AUBeep(); centerPosition = (rightEdge + leftEdge)/2; /* 目標物の位置:中央 */ if(C_LEFT_CENTER <= centerPosition && centerPosition < C_CENTER_RIGHT){ LCDPrintf("Target Center.\n"); SERVOSet(rightHandle, R_RUN); SERVOSet(leftHandle, L_RUN); /* 目標物の位置:右 */ }else if(C_CENTER_RIGHT <= centerPosition){ LCDPrintf("Target Right.\n"); if(headAngle > C_RIGHT_MAX + C_TURN) headAngle -= C_TURN; SERVOSet(cameraHandle, headAngle); SERVOSet(rightHandle, R_WALK); SERVOSet(leftHandle, L_RUN); /* 目標物の位置:左 */ }else if(centerPosition < C_LEFT_CENTER){ LCDPrintf("Target Left.\n"); if(headAngle < C_LEFT_MAX - C_TURN) headAngle += C_TURN; SERVOSet(cameraHandle, headAngle); SERVOSet(rightHandle, R_RUN); SERVOSet(leftHandle, L_WALK); } LCDMenu( "Yellow", " ", " ", "Back" ); LCDPrintf("Yellow = %d\n", match); // 取得したredの色素数 LCDPrintf("Other = %d\n\n", missMatch); // 取得したred以外の色素数 /* Debug Code: show some variables LCDPrintf("centerPosition=%d\n", centerPosition); LCDPrintf("rightEdge=%d\n", rightEdge); LCDPrintf("leftEdge=%d\n", leftEdge); //*/ /* 目標物を見失う */ }else{ LCDPrintf("Target Lost...\n"); stageNumber = STAGE5_NEXTSTAGE; } break; /* 例外ステージ:ステージナンバーの出力 */ default: LCDPrintf("Error: Stage ???\n"); LCDPrintf("stageNumber = %d\n", stageNumber); } } // 解放処理 SERVORelease( cameraHandle ); // カメラ解放 SERVORelease( rightHandle ); // 右車輪解放 SERVORelease( leftHandle ); // 左車輪解放 } /* 現在のステージ遷移順序を表示する */ void showStageRoot(){ int currentStageNumber = FIRST_STAGE; int key; LCDClear(); LCDPrintf("Current Stage Root\n"); LCDPrintf("0 "); while(currentStageNumber != 0){ switch(currentStageNumber){ case 1: LCDPrintf("-> 1 "); currentStageNumber = STAGE1_NEXTSTAGE; break; case 2: LCDPrintf("-> 2 "); currentStageNumber = STAGE2_NEXTSTAGE; break; case 3: LCDPrintf("-> 3 "); currentStageNumber = STAGE3_NEXTSTAGE; break; case 4: LCDPrintf("-> 4 "); currentStageNumber = STAGE4_NEXTSTAGE; break; case 5: LCDPrintf("-> 5 "); currentStageNumber = STAGE5_NEXTSTAGE; break; default: LCDPrintf("\nError: currentStageNumber=%d\n", currentStageNumber); } } LCDPrintf("-> 0\n"); LCDPrintf("\nPress any key."); key = KEYGet(); } /* * *********************************************** * ここから引用したライブラリ関数 * *********************************************** */ BYTE rgb2hue(BYTE r, BYTE g, BYTE b){ int max, min, delta; int tr; max = MAX(r, MAX(g, b)); min = MIN(r, MIN(g, b)); delta = (max - min); if (delta != 0) { definehue = 1; } else { definehue = 0; return 0; } if (max == r){ //return (int)(255.0 * (1.0 * (g - b) / delta) / 6.0); // for PC // fix for negative value (thanks to mizuno) tr = (425 * ((1000 * (g - b)) / delta)) / 10000; // for Eyebot return tr &= 0xff; } else if (max == g){ // return (int)(255.0 * (2.0 + 1.0 * (b - r) / delta) / 6.0); // for PC return (425 * (2000 + (1000 * (b - r)) / delta)) / 10000; // for Eyebot } else { //return (int)(255.0 * (4.0 + 1.0 * (r - g) / delta) / 6.0); // for PC return (425 * (4000 + (1000 * (r - g)) / delta)) / 10000; // for Eyebot } } /* RGB値からSAT値を算出する (0 ~ 255) */ BYTE rgb2sat(BYTE r, BYTE g, BYTE b){ int max, min; max = MAX(r, MAX(g, b)); min = MIN(r, MIN(g, b)); if (max == 0) { return 0; } else{ return ((2550000 * (max - min)) / max) / 10000; } } /* RGB値からINT値を算出する (0 ~ 255) */ __inline__ BYTE rgb2int(BYTE r, BYTE g, BYTE b){ return MAX(r, MAX(g, b)); } /* 色相値 hue が色相情報 color において有効かどうか */ __inline__ int isHueArea(bitset *color, int hue){ return bitset_get(color, hue); } /* C による簡易 bitset の実装 (thanks to mizuno) * このライブラリでは色相情報の保持に使用する * 256bit の bitset を用意し、0 ~ 255 の色相値を各ビットに関連付ける * * 000001111111111000000000.......0000 * この場合、赤色であることを判別するための色相情報になっている */ /* bitset の初期化 */ bitset *bitset_init(int length){ bitset *bs; bs = (bitset *)malloc(((length + (BS_NBIT-1)) / BS_NBIT) * sizeof(long)); return bs; } /* bitset の pos ビット目の値を取得 (pos >= 0) */ __inline__ int bitset_get(bitset *bs, int pos){ return (bs[pos >> BS_SHIFT] & 1LL << (pos & BS_MASK)) != 0; } /* bitset の pos ビット目に値を設定 (pos >= 0) */ __inline__ void bitset_put(bitset *bs, int pos, int val){ if (val) { bs[pos >> BS_SHIFT] = 1LL << (pos & BS_MASK); } else { bs[pos >> BS_SHIFT] &= ~(1LL << (pos & BS_MASK)); } } /* 色相テーブルの実装 * bitset を使うことで一つの色について 256 bit で色相情報を保持できる * この bitset を配列で持つことで複数の色の色相情報を保持できる */ /* 色相テーブルの初期化 * num には現在の色数が入る。これは初期化命令なので 1 になる。 * num は他でも使うので便宜的に呼び出すようにしている。 */ bitset **colordata_init(int *num){ bitset **color; color = (bitset **)malloc(sizeof(bitset *) * 1); color[0] = bitset_init(256); *num = 1; return color; } /* 色相テーブルに新しい色相情報用のデータ領域を確保する */ bitset **colordata_add(bitset **color, int *num){ int p = *num; p++; color = (bitset **)realloc(color, sizeof(bitset *) * p); color[p - 1] = bitset_init(256); *num = p; return color; } /* 色相テーブルを解放 */ void colordata_free(bitset **color, int num){ int c; for (c = 0; c < num; c++){ free(color[c]); } free(color); } /* 色相テーブルのセットアッププログラム * カメラから色相値を取得し、色相テーブルに順次格納していく * number には取得した色数 * bitset ** が色相テーブルとなる */ bitset **COLORSetup(int *number){ int x, y, c, key; int max, hue, huegraph[256]; colimage buf; bitset **bs; // 色相テーブルへのポインタ int num; // 色数 /* 色相テーブルの初期化 */ bs = colordata_init(&num); do{ /* ヒストグラム用のバッファ */ for (c = 0; c < 256; c++){ huegraph[c] = 0; } do{ LCDMenu(" ", " ", " ", "CAM"); LCDSetPos(0, 10); LCDPrintf("# %2d", num); /* 撮影待ち */ CAMGetColFrame(&buf, 0); while (KEYRead() != KEY4){ LCDPutColorGraphic(&buf); CAMGetColFrame(&buf, 0); } AUBeep(); /* 撮影した画像を色相値に変換 */ for (y = 1; y < 61; y++){ for (x = 1; x < 81; x++){ hue = rgb2hue(buf[y][x][0], buf[y][x][1], buf[y][x][2]); /* 色相値が得られていれば、ヒストグラムに追加 */ if (definehue){ huegraph[hue]++; } } } /* もう一枚、同じ色について撮るかどうか * MORE: もう一枚撮る * OK: ヒストグラムの処理へ */ LCDMenu(" ", " ", "MORE", " OK"); key = 0; while ((key = KEYRead()) != KEY3 && key != KEY4) ; }while(key == KEY3); /* ヒストグラムの処理 * ヒストグラム中の最大値を基準に閾値に満たないものを 0 に * 満たすものを 1 として、bitset に設定していく * * 現在の閾値は最大値の 5 % */ /* 最大の色相値を取得 */ max = -1; for (c = 0; c < 256; c++){ if (max < huegraph[c]){ max = huegraph[c]; LCDSetPos(6, 0); LCDPrintf("MAX %3d:%4d", c, max); } } /* 閾値 (要素が最も多い色相値の要素数の 5% 以下) */ max = max / 20; for (c = 0; c < 256; c++){ bitset_put(bs[num - 1], c, max <= huegraph[c]); } /* ヒストグラムの処理結果の確認 - LCD 上に表示 */ LCDClear(); for (c = 0; c < 256; c++){ LCDLine(c % 128, 10 + (c / 128) * 30, c % 128, 10 + (c / 128) * 30 - bitset_get(bs[num - 1], c) * 5, 1); } /* これで OK かどうか、また他の色についても収集するかどうか * RE: 現在の色を取り直す * NXT: 次の色の収集に移る * DONE: 色の収集を終了 */ LCDMenu(" ", "RE", "NXT", "DONE"); key = 0; while ((key = KEYRead()) != KEY2 && key != KEY3 && key != KEY4) ; if (key == KEY3){ bs = colordata_add(bs, &num); } LCDSetPos(6, 0); LCDPrintf(" "); }while(key != KEY4); LCDClear(); *number = num; return bs; } /* 色相テーブルに格納されているすべての色の情報を LCD に表示する * num には表示する色数を指定する */ void COLORInfo(bitset **bs, int num){ int i, c; for (i = 0; i < num; i++){ for (c = 0; c < 256; c++){ LCDLine(c % 128, 5 + (i * 6) + (c / 128) * 2, c % 128, 5 + (i * 6) + (c / 128) * 2 - bitset_get(bs[i], c), 1); } } } /* 色相テーブルを開放する */ void COLORRelease(bitset **bs, int num){ colordata_free(bs, num); }