Robot Vision

お久です。今日は、ちょっとした近況報告などなど。

現在は、画像解析を実装したロボット制御プログラムを作ってみます。

具体的には、赤色と黄色のポールがあって、こんな感じで進む制御プログラムを作ってます。

本当はこんな感じで動いてほしいんだけど、

現在の手持ちのアルゴリズムではちょっと動きそうにない。

加えて、試走日は明日な上にロボットが今手元に無いからテストできない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   // カメラ自動修整時の回転角度 (使用ステージ例：第５)
#define C_LEFT_CENTER 35 // 目標物の左右位置の識別基準値 (使用ステージ例：第５)
#define C_CENTER_RIGHT 45 // 目標物の左右位置の識別基準値 (使用ステージ例：第５)

#define WAIT_TIME 500 // 旋回する時間(使用ステージ例：第２)

#define FIRST_STAGE 1  // 最初のステージ (第０ステージに遷移すると終了)
#define STAGE1_NEXTSTAGE 2 // 第１ステージの次ステージ
#define STAGE2_NEXTSTAGE 3 // 第２ステージの次ステージ
#define STAGE3_NEXTSTAGE 0 // 第３ステージの次ステージ
#define STAGE4_NEXTSTAGE 5 // 第４ステージの次ステージ
#define STAGE5_NEXTSTAGE 0 // 第５ステージの次ステージ

/* 基本課題に対するルート案
* [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){

/* 第０ステージ：サブメニュー画面 & 各サーボの初期化*/
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;

/* 第１ステージ：カメラ右向き & １番目に取得した色相を発見するまで直進 */
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;

/* 第２ステージ：一定時間(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;

/* 第３ステージ：カメラ右向き & ２番目に取得した色相を発見するまで直進 */
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;
}


/* 第４ステージ：カメラ正面 & ２番目に取得した色相を発見するまで右旋回 */
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;

/* 第５ステージ：カメラ自動微調整 & 自動方向転換 & ２番目に取得した色相を見失うまで直進 */
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);
}

Tweet