しゃべれる理系: EyeBot

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

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

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

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

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

加えて、試走日は明日な上にロボットが今手元に無いからテストできない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);
}

今日はBasic Mathの授業後、
情報ネットワーク実験の発展課題をこなすため、
一人黙々とEyebotと戯れてました。

あの広い実験室にEyebotにプログラムを打ち込んでる人が一人。
そして、壁はガラス製で、もちろん透明。

実験室の廊下を通る生徒or事務員の目から見たら
変質者にしか見えn(ry

とはいえ、羞恥心を克服し何とか発展課題は完成。
作ったプログラムは、rotally encodorを回すとServo motorが
自動車のアクセルっぽく動くプログラム(のはず)です。
加速度とかが考慮されて、モーターがそれっぽく動きます(多分

明日は風見鶏の設計か。
なんか物理学の基礎が頭から消えてる感が否めないが、
とりあえず、やれるところまでやってみる予定。

話し変わって、Eyebotと戯れてコメントやら印刷やらをした後、
自作OSのための勉強をはじめました。

去年、Micro computingとComputer Architectureを
頑張って勉強したおかげで、出だしは中々好調。

今日は電源入れると、自作OSが立ち上がって
hello worldが画面に表示されるOS(?)が出来ました。

ちなみに、終了機能はありません。

一度立ち上げたら、例のボタンを長押しするか、

電源コードを抜くかです。

まぁ、初日なんでこんなもんです。
３ヵ月後にはこのOSが、それらしいOSになってることを期待。

しゃべれる理系

2009-01-23

figure-of-eight document

figure-of-eight report

2009-01-22

EyeBot figure-of-eight

2008-12-03

EyeBot

2008-11-27

Robot Vision

2008-04-17

Sensor Control Programming