目次
前
次
センサー動作テスト
センサーには、GameBoyCamera(GBC)を利用します。
GBCは、大阪電気通信大学のサークル活動で、MCR_VCを始める
キッカケを与えたセンサーなので、Arduinoでの画像処理を
どこまで可能かを確認することにしました。
利用するArduino互換基板は、以下。
Arduinoの内蔵SRAMは、1024バイトだけなので、この中の
512バイトをGBCの出力画像を保存するバッファに利用。
MCRマシンを動かすために、必要なのは８ビット程度の
センサー情報なので、アナログコンパレータを入れて
A/D変換器を使わずに、２値化するハードウエアを用意。
回路図は、以下。
GBCの出力してくる１ピクセルデータは、２Vppでデジタル
回路、マイコンでは扱い難い電圧なので、アンプで２倍して
から、コンパレータで２値化します。コンパレータの参照に
利用する電圧を調整すると、2Vpp以内でも２値化できますが
拡張性を考え、２倍する回路を入れました。
動作テスト時は、Arduinoの内蔵A/D変換器を使いますが
２値化に必要な閾値がわかったなら、外付けの回路での
２値化だけを利用することに。
GBC、Arduino、２値化回路の接続は、以下。
MCRマシンを動かすマイコンには、PORTBの０から３の
４ビットに、センサーデータとして値を出力します。
センサーデータは、次のように指定します。
#define ALL_BLACK 0
#define ALL_WHITE 1
#define LEFT_WHITE 2
#define RIGHT_WHITE 3
#define CENTER 4
#define TINY_RIGHT 5
#define RIGHT 6
#define BIG_RIGHT 7
#define TINY_LEFT 8
#define LEFT 9
#define BIG_LEFT 10
#define BOTH_WHITE 11
#define ILLEAGAL 12
センサーデータを出力するまでに、次の項目をテストあるいは
確認できるスケッチを作成します。
- 初期化パラメータ設定
- １ライン＝128ピクセルのうち、奇数ピクセルだけ取得し保存
- ターゲットライン指定
- START指定からREADを検出するまでに必要なクロック数
- ２値化に指定できる閾値範囲
- １ライン＝64ピクセルの生値表示
- １ライン＝64ビットの値表示
作成したスケッチは、以下。
#include <MsTimer2.h>
#define OFF 0
#define ON OFF+1
#define MASK0F 0x0f
#define MASKFF 0xff
#define MASK400 0x400
#define START_BIT 4
#define SIN_BIT 5
#define LOAD_BIT 4
#define XRST_BIT 3
#define XCK_BIT 2
#define READ_BIT 1
#define LED_BIT 5
#define CMP_BIT 4
#define PIXLAST 8064
boolean uflag ;
byte tmp ;
byte xcnt ;
word scnt ;
char sbuf[8];
byte sindex ;
char cmd ;
byte params[8];
byte rnum ;
byte lnum ;
word gdat[64];
byte bgdat[8];
char msg[6] ;
word avr ;
word rcounter ;
word rx ;
word ry ;
void rs_putchar(char x)
{
Serial.write(x);
}
void rs_puts(char *x)
{
while ( *x != 0 ) {
rs_putchar( *x ) ;
x++ ;
}
}
void crlf()
{
rs_putchar('\r');
rs_putchar('\n');
}
void show_help()
{
rs_puts("? help") ; crlf();
rs_puts("P set parameters"); crlf();
rs_puts("p show parameters"); crlf();
rs_puts("L set line number"); crlf();
rs_puts("l show line number"); crlf();
rs_puts("G get graphic data"); crlf();
rs_puts("S show graphic data"); crlf();
rs_puts("T test"); crlf();
rs_puts(" T1 generate data"); crlf();
}
byte get_hex(char x)
{
byte result ;
/* default */
result = 0 ;
/* convert */
if ( '0' <= x && <= '9' ) { result = x - '0' ; }
if ( 'A' <= x && x <= 'F' ) { result = x - 'A' + 10 ; }
if ( 'a' <= x && x <= 'f' ) { result = x - 'a' + 10 ; }
return result ;
}
char get_asc(byte x)
{
char result ;
/* default */
result = '0' ;
/* convert */
if ( x < 10 ) { result = x + '0' ; }
if ( 9 < x && x < 16 ) { result = x - 10 + 'A' ; }
return result ;
}
void put_sin(boolean x)
{
if ( x ) { PORTC |= (1 << SIN_BIT); }
else { PORTC &= ~(1 << SIN_BIT); }
/* delay */
delayMicroseconds(1);
}
void put_load(boolean x)
{
if ( x ) { PORTC |= (1 << LOAD_BIT); }
else { PORTC &= ~(1 << LOAD_BIT); }
}
void put_rst(boolean x)
{
if ( x ) { PORTC |= (1 << XRST_BIT); }
else { PORTC &= ~(1 << XRST_BIT); }
/* delay */
delayMicroseconds(1);
}
void put_xck(boolean x)
{
if ( x ) { PORTC |= (1 << XCK_BIT); }
else { PORTC &= ~(1 << XCK_BIT); }
/* delay */
delayMicroseconds(1);
}
void put_start(boolean x)
{
if ( x ) { PORTD |= (1 << START_BIT); }
else { PORTD &= ~(1 << START_BIT); }
/* delay */
delayMicroseconds(1);
}
void rst_gbc(void)
{
/* LOW */
put_rst(OFF);
/* XCK : H */
put_xck(ON);
/* XCK : L */
put_xck(OFF);
/* HIGH */
put_rst(ON);
}
void start_gbc(void)
{
/* HIGH */
put_start(ON);
/* XCK : H */
put_xck(ON);
/* XCK : L */
put_xck(OFF);
/* LOW */
put_start(OFF);
}
void send_params(word x)
{
word tmp ;
byte i ;
/* copy */
tmp = x ;
/* loop */
for ( i = 0 ; i < 11 ; i++ ) {
/* impress data */
put_sin(OFF);
if ( tmp & MASK400 ) { put_sin(ON); }
/* load */
if ( i == 10 ) { put_load(ON); }
/* XCK : H */
put_xck(ON);
/* shift */
tmp <<= 1;
/* XCK : L */
put_xck(OFF);
}
/* default */
put_sin(OFF);
put_load(OFF);
}
void init_gbc(void)
{
byte i ;
/* reset */
rst_gbc();
/* send parameters */
for ( i = 0 ; i < 8 ; i++ ) {
send_params( (i << 8) | *(params+i) ) ;
}
}
boolean get_read()
{
boolean result ;
/* default */
result = OFF ;
/* judge */
if ( PINC & (1 << READ_BIT) ) { result = ON ; }
return result ;
}
boolean get_bin()
{
boolean result ;
/* default */
result = OFF ;
/* judge */
if ( PINB & (1 << CMP_BIT) ) { result = ON ; }
return result ;
}
boolean is_range(word x)
{
boolean result ;
/* default */
result = OFF ;
/* judge */
if ( rx <= x && x < ry ) { result = ON ; }
return result ;
}
void store_binary(byte bx)
{
byte bm ; /* block memory */
byte bd ; /* bit location */
/* judge */
if ( bx > 63 ) return ;
/* calculate block memory */
bm = (bx >> 3) ;
/* calculate bit location */
bd = 7 - (bx & 0x07) ;
/* store '1' */
*(bgdat+bm) |= (1 << bd) ;
}
void clear_gdat()
{
byte i ;
for ( i = 0 ; i < 64 ; i++ ) { *(gdat+i) = 0 ; }
}
void clear_bgdat()
{
byte i ;
for ( i = 0 ; i < 8 ; i++ ) { *(bgdat+i) = 0 ; }
}
void get_gbc(byte x)
{
word ii ;
byte j ;
word pdx ;
word xmin ;
word xmax ;
/* clear raw data and binary data */
clear_gdat();
clear_bgdat();
/* initialize GBC */
init_gbc();
/* start */
start_gbc();
rcounter = 0 ;
/* send dummy clock (10 clocks) */
for ( ii = 0 ; ii < 11 ; ii++ ) {
/* XCK : H */
put_xck(ON);
/* XCK : L */
put_xck(OFF);
/* judge */
if ( get_read() == ON ) {
rcounter = ii ;
break ;
}
}
/* show READ location */
show_value( rcounter );
crlf();
/* skip fist line */
for ( ii = 0 ; ii < 127 ; ii++ ) {
/* XCK : H */
put_xck(ON);
/* XCK : L */
put_xck(OFF);
}
/* default */
xmin = 1023 ;
xmax = 0 ;
/* line handling */
j = 0 ;
for ( ii = 0 ; ii < PIXLAST ; ii++ ) {
/* XCK : H */
put_xck(ON);
/* get raw data */
pdx = analogRead(0) ;
/* XCK : L */
put_xck(OFF);
/* judge target line */
if ( is_range(ii+128) == ON ) {
/* debug */
show_value( pdx );
putchar(' ');
if ( (ii % 8) == 7 ) { crlf() ; }
/* store */
if ( (ii & ON) == OFF ) {
/* store raw data */
*(gdat+j) = pdx ;
/* store binary data */
if ( get_bin() == ON ) { store_binary(j); }
/* increment */
j++ ;
/* maximum */
if ( xmax < pdx ) { xmax = pdx ; }
/* minimum */
if ( xmin > pdx ) { xmin = pdx ; }
}
}
/* judge */
if ( j > 127 ) break ;
}
/* calculate avrage */
avr = (xmin+xmax) >> 1 ;
}
void show_cur_binary(word x)
{
/* fixed */
*(msg+0) = '(' ;
*(msg+2) = ')' ;
*(msg+3) = ' ' ;
*(msg+4) = '\0' ;
/* judge */
*(msg+1) = '_' ;
if ( x > avr ) { *(msg+1) = '*' ; }
/* show */
rs_puts( msg );
}
void show_gdat()
{
byte i ;
char bb ;
/* avrage */
rs_puts("Avrage = ");
show_value( avr );
rs_putchar(' ');
scnt = (word)(avr * 5000.0 / 1024);
show_value( scnt );
rs_putchar('m');
rs_putchar('V');
crlf();
/* loop */
for ( i = 0 ; i < 64 ; i++ ) {
/* show */
show_value( *(gdat+i) );
/* show current binary format */
show_cur_binary( *(gdat+i) ) ;
/* new line */
if ( (i % 8) == 7 ) { crlf(); }
}
/* loop */
for ( i = 0 ; i < 64 ; i++ ) {
/* default */
bb = '0' ;
/* judge */
if ( *(gdat+i) > avr ) { bb = '1' ; }
/* show */
rs_putchar( bb ) ;
/* */
if ( (i & 7) == 7 ) { rs_putchar( '_' ); }
}
crlf();
}
void binary_display(byte x)
{
int i ;
for ( i = 7 ; i > -1 ; i-- ) {
rs_putchar( '0' + ((x >> i) & 1) );
}
}
void show_bgdat()
{
byte i ;
for ( i = 0 ; i < 8 ; i++ ) {
binary_display( *(bgdat+i) );
if ( i < 7 ) { rs_putchar('_'); }
}
crlf();
}
void show_value(word x)
{
/* delimiter */
*(msg+5) = '\0' ;
/* separate */
for ( int jj = 4 ; jj > -1 ; jj-- ) {
*(msg+jj) = get_asc( x % 10 ) ; x /= 10 ;
}
/* zero surpress */
if ( *(msg+0) == '0' ) {
*(msg+0) = ' ' ;
if ( *(msg+1) == '0' ) {
*(msg+1) = ' ' ;
if ( *(msg+2) == '0' ) {
*(msg+2) = ' ' ;
}
}
}
/* show */
rs_puts( msg );
}
/* MtTimer2 interrupt handler */
void update_trigger(void)
{
send_led( xcnt & ON );
xcnt++ ;
}
void send_led(byte x)
{
if ( x ) { PORTB |= (1 << LED_BIT); }
else { PORTB &= ~(1 << LED_BIT); }
}
/* initialilze */
void setup()
{
/* initialize serial port */
Serial.begin(9600);
sindex = 0 ;
/* pin configuration */
PORTB = 0b00010000 ;
PORTC = 0b00001000 ;
PORTD = 0b00000001 ;
/* pin direction */
DDRB = 0b11101111 ;
DDRC = 0b11111100 ;
DDRD = 0b11111110 ;
/* clear flags */
uflag = OFF ;
/* others */
scnt = 0 ;
xcnt = 0 ;
/* initialize */
clear_gdat();
clear_bgdat();
*(params+0) = 0x80 ; *(params+1) = 0x03 ;
*(params+2) = 0x00 ; *(params+3) = 0x0F ;
*(params+4) = 0x01 ; *(params+5) = 0x00 ;
*(params+6) = 0x01 ; *(params+7) = 0x21 ;
lnum = 32 ;
rx = (lnum << 7) ;
ry = rx + 128 ;
/* 500ms period */
MsTimer2::set(500,update_trigger);
/* enable */
MsTimer2::start();
}
/* endless loop */
void loop()
{
byte i ;
byte j ;
byte k ;
char msgx[5];
/* command interpreter */
if ( uflag == ON ) {
/* clear flag */
uflag = OFF ;
/* new line */
crlf();
/* get command */
cmd = *(sbuf+0) ;
/* help */
if ( cmd == '?' ) { show_help() ; }
/* set parameter */
if ( cmd == 'P' ) {
/* get register number */
rnum = get_hex( *(sbuf+1) ) ;
/* get parameter */
tmp = get_hex( *(sbuf+2) ) ;
tmp <<= 4 ;
tmp |= get_hex( *(sbuf+3) ) ;
/* store */
*(params+rnum) = tmp ;
}
/* show parameter */
if ( cmd == 'p' ) {
/* fixed */
*(msgx+4) = '\0' ;
*(msgx+1) = ':' ;
/* parameters */
for ( i = 0 ; i < 8 ; i++ ) {
/* register number */
*(msgx+0) = get_asc( i ) ;
/* parameters */
*(msgx+2) = get_asc( (*(params+i) >> 4) & MASK0F ) ;
*(msgx+3) = get_asc( *(params+i) & MASK0F ) ;
/* show */
rs_puts( msgx );
/* hexadecimal */
rs_putchar('h');
/* space */
rs_putchar(' ');
}
/* new line */
crlf();
}
/* set line number */
if ( cmd == 'L' ) {
/* clear */
lnum = 0 ;
/* loop */
for ( i = 1 ; i < 3 ; i++ ) {
/* get */
tmp = *(sbuf+i) ;
/* judge */
if ( tmp == '\r' ) break ;
/* x 10 */
lnum = (lnum << 2) + lnum ;
lnum <<= 1 ;
/* add */
lnum += get_hex(tmp) ;
}
/* judge */
if ( lnum > 63 || lnum == 0 ) {
/* default */
lnum = 32 ;
/* show range */
rs_puts("incorrect! range(1 -> 63)");
/* new line */
crlf();
} else {
rx = (lnum << 7) ;
ry = rx + 128 ;
}
}
/* show line number */
if ( cmd == 'l' ) {
/* show */
show_value( lnum );
/* new line */
crlf();
}
/* get graphic data code */
if ( cmd == 'G' ) {
/* message */
rs_puts("Start ");
/* new line */
crlf() ;
/* perform */
get_gbc(lnum);
/* message */
rs_puts("Complete !");
/* new line */
crlf() ;
}
/* show graphic data */
if ( cmd == 'S' ) {
/* binary data */
show_bgdat();
/* raw data */
show_gdat();
}
/* test */
if ( cmd == 'T' ) {
/* set random data */
if ( *(sbuf+1) == '1' ) {
randomSeed( xcnt );
/* generate data */
for ( k = 0 ; k < 64 ; k++ ) {
/* area */
tmp = 128 + random(0,64);
if ( k < 16 || k > 47 ) { tmp = 128 - random(0,64); }
*(gdat+k) = tmp ;
}
/* calculate */
word xmax ;
word xmin ;
xmax = *(gdat+0) ;
xmin = *(gdat+0) ;
for ( k = 0 ; k < 64 ; k++ ) {
if ( xmax < *(gdat+k) ) { xmax = *(gdat+k); }
if ( xmin > *(gdat+k) ) { xmin = *(gdat+k); }
}
avr = (xmax + xmin) >> 1 ;
/* show */
rs_puts("Average = ");
show_value( avr );
/* new line */
crlf();
/* convert */
clear_bgdat();
for ( k = 0 ; k < 64 ; k++ ) {
if ( *(gdat+k) > avr ) { store_binary(k); }
}
}
}
}
}
/* receive interrupt */
void serialEvent()
{
char ch;
if ( Serial.available() > 0 ) {
/* get 1 character */
ch = Serial.read();
/* store */
*(sbuf+sindex) = ch ;
/* increment */
sindex++ ;
/* judge */
if ( ch == '\r' ) {
sindex = 0 ;
uflag = ON ;
}
}
}
上のArduinoスケッチには、64ピクセルのデータを生成し
内部での２値化処理を検討できる機能を加えています。
コマンドは、以下としました。
- ? ヘルプ（コマンド一覧表示）
- P 内部レジスタに８ビット値設定
- p 内部レジスタの内容表示
- L 情報を取得するライン番号設定
- l 情報を取得するライン番号表示
- G GBCを動かして画像情報を取得
- S 取得画像情報を表示
- T 64ピクセルの画像データ生成
'T'コマンドで、ダミーデータを生成し、GBCの画像情報を
表示、２値化すると、以下となります。
64ピクセル中の最大値、最小値を求めてから、最大値と最小値の和
の1/2を閾値と決めて処理しています。
設定するパラメータとターゲットラインの確認は、'p'、'l'の
コマンドを使います。
'G'コマンドで、ターゲットラインの64ピクセルの情報を取得。
'S'コマンドで、２値化を確認。
２値化した場合、コマンドの直下にあるのは、外部回路での処理で
画像情報を表示した直下にあるのが、内部で閾値を決めて算出した
値にしています。
外部回路による２値化では、可変抵抗器のトリマーを回して
対応するので、調整していない場合は、'0'となります。
ターゲットラインの２値化を、外部回路で処理できるように
なったので、64ピクセルから８ビットの画像データを生成する
ことを考えます。
MCRのコースを走行する場合、センサーは中央にある白線の
位置を判断していきます。クランク、レーンチェンジの場合
は、後で考えるとして、中央の白線を検出することを考えて
いきます。
センサー動作をテストするArduinoスケッチでは、１ライン＝
64ピクセルは、奇数か偶数のいずれかピクセルを拾っています。
中央にする場合、位置では32から95の間を拾えばよいと判断。
関連関数はis_rangeだけで、ターゲットラインの処理を以下とします。
rx = (lnum << 7) + 32 ;
ry = rx + 64 ;
boolean is_range(word x)
{
boolean result ;
/* default */
result = OFF ;
/* judge */
if ( rx <= x && x < ry ) { result = ON ; }
return result ;
}
64ピクセルを８ピクセルごとに分けて、８ブロックを生成し
１ブロック＝１バイトのすべて白、すべて黒、黒から白、白
から黒と変化していることを判定する関数を用意。
typedef unsigned char UBYTE ;
UBYTE judge_edge(UBYTE x)
{
UBYTE result ;
/* judge */
switch ( x ) {
/* black -> white */
case 0x01 :
case 0x03 :
case 0x07 :
case 0x0f :
case 0x1f :
case 0x3f :
case 0x7f :
result = 1 ;
break ;
/* white -> black */
case 0x80 :
case 0xC0 :
case 0xE0 :
case 0xF0 :
case 0xF8 :
case 0xFC :
case 0xFE :
result = 2 ;
break ;
/* all white */
case 0xff :
result = 3 ;
break ;
/* all black */
case 0x00 :
result = 4 ;
break ;
/* others */
default :
result = 0 ;
}
return result ;
}
クランク、レーンチェンジでは、すべて白、すべて黒を
利用することもありますが、クランクに到達するまでは
中央に白線がある状態で走行できるようにします。
上の関数を少し改造し、64ピクセルから16ビットの情報に
変換すると、次のようになりました。
スケッチに追加した関数は、以下。
byte judge_edge(byte x)
{
byte result ;
/* judge */
switch ( x ) {
/* black -> white */
case 0x01 :
case 0x03 :
case 0x07 :
case 0x0f :
case 0x1f :
case 0x3f :
case 0x7f :
result = 1 ;
break ;
/* white -> black */
case 0x80 :
case 0xC0 :
case 0xE0 :
case 0xF0 :
case 0xF8 :
case 0xFC :
case 0xFE :
result = 2 ;
break ;
/* all white */
case 0xff :
result = 3 ;
break ;
/* others */
default :
result = 0 ;
}
return result ;
}
void show_sensor()
{
char xmsg[17];
byte i ;
byte j ;
byte k ;
/* default */
*(xmsg+16) = '\0';
/* loop */
for ( i = 0 ; i < 8 ; i++ ) {
/* convert code */
j = judge_edge( *(bgdat+i) );
/* calculate location */
k = (i << 1);
/* convert binary */
*(xmsg+k) = '0' ;
*(xmsg+k+1) = '0' ;
if ( j == 1 ) { *(xmsg+k+1) = '1' ; }
if ( j == 2 ) { *(xmsg+k) = '1' ; }
if ( j == 3 ) {
*(xmsg+k) = '1' ;
*(xmsg+k+1) = '1' ;
}
}
/* show */
rs_puts( xmsg );
/* new line */
crlf();
}
16ビット情報表示に、文字列を使っていますが
この16ビットを４ビットに変換するデコーダを
入れれば、利用側のマイコン、デジタル回路の
動作を単純にできます。
Arduinoスケッチでは、６ｋバイト程度のROM消費でした
から、ATmega168を使ったArduinoで対応可能。
64ピクセルから４ビットへの変換は、次の関数を作って対応。
word gen_sensor_bit()
{
word result ;
byte i ;
byte j ;
byte k ;
/* clear */
result = 0 ;
/* loop */
for ( i = 0 ; i < 8 ; i++ ) {
/* convert code */
j = judge_edge( *(bgdat+i) );
/* calculate location */
k = 15 - (i << 1);
/* convert binary */
result |= (j << k);
}
return result ;
}
byte gen_sensor_nibble(word x)
{
byte result ;
/* default */
result = ILLEAGAL ;
/* all black */
if ( x == 0x0000 ) { result = ALL_BLACK ; }
/* both white */
if ( x == 0xC001 ||
x == 0xC003 ||
x == 0x8001 ||
x == 0x8003 ) {
result = BOTH_WHITE ;
}
/* all white */
if ( x == 0xffff ) { result = ALL_WHITE ; }
/* left white */
if ( x == 0xff00 ||
x == 0xfe00 ||
x == 0xfc00 ) {
result = LEFT_WHITE ;
}
/* right white */
if ( x == 0x00ff ||
x == 0x007f ||
x == 0x003f ) {
result = RIGHT_WHITE ;
}
/* center */
if ( x == 0x03c0 ||
x == 0x01c0 ||
x == 0x0380 ) {
result = CENTER ;
}
/* tiny right */
if ( x == 0x00f0 ||
x == 0x0070 ) {
result = TINY_RIGHT ;
}
/* right */
if ( x == 0x0078 ||
x == 0x003c ) {
result = RIGHT ;
}
/* big right */
if ( x == 0x001e ||
x == 0x000f ) {
result = BIG_RIGHT ;
}
/* tiny left */
if ( x == 0x0700 ||
x == 0x0f00 ) {
result = TINY_LEFT ;
}
/* left */
if ( x == 0x1e00 ||
x == 0x3c00 ) {
result = LEFT ;
}
/* big left */
if ( x == 0x7800 ||
x == 0xf000 ) {
result = BIG_LEFT ;
}
return result ;
}
２値化されて得られる64ピクセルを関数gen_sensor_bit()で
16ビット情報に変換し、関数gen_sensor_nibbleで４ビットへ
縮退します。この４ビットをセンサー情報として出力すると
GBCを制御するArduinoの仕事はおしまい。
Arduinoスケッチは、以下となります。
#include <MsTimer2.h>
#define OFF 0
#define ON OFF+1
#define MASK0F 0x0f
#define MASKFF 0xff
#define MASK400 0x400
#define START_BIT 4
#define SIN_BIT 5
#define LOAD_BIT 4
#define XRST_BIT 3
#define XCK_BIT 2
#define READ_BIT 1
#define LED_BIT 5
#define CMP_BIT 4
#define PIXLAST 8064
#define ALL_BLACK 0
#define ALL_WHITE 1
#define LEFT_WHITE 2
#define RIGHT_WHITE 3
#define CENTER 4
#define TINY_RIGHT 5
#define RIGHT 6
#define BIG_RIGHT 7
#define TINY_LEFT 8
#define LEFT 9
#define BIG_LEFT 10
#define BOTH_WHITE 11
#define ILLEAGAL 12
boolean uflag ;
byte tmp ;
byte xcnt ;
word scnt ;
char sbuf[8];
byte sindex ;
char cmd ;
byte params[8];
byte rnum ;
byte lnum ;
word gdat[64];
byte bgdat[8];
char msg[6] ;
word avr ;
word rcounter ;
word rx ;
word ry ;
void rs_putchar(char x)
{
Serial.write(x);
}
void rs_puts(char *x)
{
while ( *x != 0 ) {
rs_putchar( *x ) ;
x++ ;
}
}
void crlf()
{
rs_putchar('\r');
rs_putchar('\n');
}
void show_help()
{
rs_puts("? help") ; crlf();
rs_puts("P set parameters"); crlf();
rs_puts("p show parameters"); crlf();
rs_puts("L set line number"); crlf();
rs_puts("l show line number"); crlf();
rs_puts("G get graphic data"); crlf();
rs_puts("S show graphic data"); crlf();
rs_puts("T test"); crlf();
rs_puts(" T1 generate data"); crlf();
}
byte get_hex(char x)
{
byte result ;
/* default */
result = 0 ;
/* convert */
if ( '0' <= x && x <= '9' ) { result = x - '0' ; }
if ( 'A' <= x && x <= 'F' ) { result = x - 'A' + 10 ; }
if ( 'a' <= x && x <= 'f' ) { result = x - 'a' + 10 ; }
return result ;
}
char get_asc(byte x)
{
char result ;
/* default */
result = '0' ;
/* convert */
if ( x < 10 ) { result = x + '0' ; }
if ( 9 < x && x < 16 ) { result = x - 10 + 'A' ; }
return result ;
}
void put_sin(boolean x)
{
if ( x ) { PORTC |= (1 << SIN_BIT); }
else { PORTC &= ~(1 << SIN_BIT); }
/* delay */
delayMicroseconds(1);
}
void put_load(boolean x)
{
if ( x ) { PORTC |= (1 << LOAD_BIT); }
else { PORTC &= ~(1 << LOAD_BIT); }
}
void put_rst(boolean x)
{
if ( x ) { PORTC |= (1 << XRST_BIT); }
else { PORTC &= ~(1 << XRST_BIT); }
/* delay */
delayMicroseconds(1);
}
void put_xck(boolean x)
{
if ( x ) { PORTC |= (1 << XCK_BIT); }
else { PORTC &= ~(1 << XCK_BIT); }
/* delay */
delayMicroseconds(1);
}
void put_start(boolean x)
{
if ( x ) { PORTD |= (1 << START_BIT); }
else { PORTD &= ~(1 << START_BIT); }
/* delay */
delayMicroseconds(1);
}
void rst_gbc(void)
{
/* LOW */
put_rst(OFF);
/* XCK : H */
put_xck(ON);
/* XCK : L */
put_xck(OFF);
/* HIGH */
put_rst(ON);
}
void start_gbc(void)
{
/* HIGH */
put_start(ON);
/* XCK : H */
put_xck(ON);
/* XCK : L */
put_xck(OFF);
/* LOW */
put_start(OFF);
}
void send_params(word x)
{
word tmp ;
byte i ;
/* copy */
tmp = x ;
/* default */
put_load(OFF);
/* loop */
for ( i = 0 ; i < 11 ; i++ ) {
/* impress data */
put_sin(OFF);
if ( tmp & MASK400 ) { put_sin(ON); }
/* load */
if ( i == 10 ) { put_load(ON); }
/* XCK : H */
put_xck(ON);
/* shift */
tmp <<= 1;
/* XCK : L */
put_xck(OFF);
}
/* default */
put_sin(OFF);
put_load(OFF);
}
void init_gbc(void)
{
byte i ;
/* reset */
rst_gbc();
/* send parameters */
for ( i = 0 ; i < 8 ; i++ ) {
send_params( (i << 8) | *(params+i) ) ;
}
}
boolean get_read()
{
boolean result ;
/* default */
result = OFF ;
/* judge */
if ( PINC & (1 << READ_BIT) ) { result = ON ; }
return result ;
}
boolean get_bin()
{
boolean result ;
/* default */
result = OFF ;
/* judge */
if ( PINB & (1 << CMP_BIT) ) { result = ON ; }
return result ;
}
boolean is_range(word x)
{
boolean result ;
/* default */
result = OFF ;
/* judge */
if ( rx <= x && x < ry ) { result = ON ; }
return result ;
}
void store_binary(byte bx)
{
byte bm ; /* block memory */
byte bd ; /* bit location */
/* judge */
if ( bx > 63 ) return ;
/* calculate block memory */
bm = (bx >> 3) ;
/* calculate bit location */
bd = 7 - (bx & 0x07) ;
/* store '1' */
*(bgdat+bm) |= (1 << bd) ;
}
void clear_gdat()
{
byte i ;
for ( i = 0 ; i < 64 ; i++ ) { *(gdat+i) = 0 ; }
}
void clear_bgdat()
{
byte i ;
for ( i = 0 ; i < 8 ; i++ ) { *(bgdat+i) = 0 ; }
}
void get_gbc(byte x)
{
word ii ;
byte j ;
word pdx ;
word xmin ;
word xmax ;
/* clear raw data and binary data */
clear_gdat();
clear_bgdat();
/* initialize GBC */
init_gbc();
/* start */
start_gbc();
rcounter = 0 ;
/* send dummy clock (10 clocks) */
for ( ii = 0 ; ii < 10 ; ii++ ) {
/* XCK : H */
put_xck(ON);
/* XCK : L */
put_xck(OFF);
/* judge */
if ( get_read() == ON ) {
rcounter = ii ;
break ;
}
}
/* show READ location */
show_value( rcounter );
crlf();
/* skip fist line */
for ( ii = 0 ; ii < 127 ; ii++ ) {
/* XCK : H */
put_xck(ON);
/* XCK : L */
put_xck(OFF);
}
/* default */
xmin = 1023 ;
xmax = 0 ;
/* line handling */
j = 0 ;
for ( ii = 0 ; ii < PIXLAST ; ii++ ) {
/* XCK : H */
put_xck(ON);
/* get raw data */
pdx = analogRead(0) ;
/* XCK : L */
put_xck(OFF);
/* judge target line */
if ( is_range(ii+128) == ON ) {
/* debug */
show_value( pdx );
putchar(' ');
if ( (ii % 8) == 7 ) { crlf() ; }
/* store */
if ( (ii & ON) == OFF ) {
/* store raw data */
*(gdat+j) = pdx ;
/* store binary data */
if ( get_bin() == ON ) { store_binary(j); }
/* increment */
j++ ;
/* maximum */
if ( xmax < pdx ) { xmax = pdx ; }
/* minimum */
if ( xmin > pdx ) { xmin = pdx ; }
}
}
/* judge */
if ( j > 127 ) break ;
}
/* calculate avrage */
avr = (xmin+xmax) >> 1 ;
}
void show_cur_binary(word x)
{
/* fixed */
*(msg+0) = '(' ;
*(msg+2) = ')' ;
*(msg+3) = ' ' ;
*(msg+4) = '\0' ;
/* judge */
*(msg+1) = '_' ;
if ( x > avr ) { *(msg+1) = '*' ; }
/* show */
rs_puts( msg );
}
void show_gdat()
{
byte i ;
char bb ;
/* avrage */
rs_puts("Avrage = ");
show_value( avr );
rs_putchar(' ');
scnt = (word)(avr * 5000.0 / 1024);
show_value( scnt );
rs_putchar('m');
rs_putchar('V');
crlf();
/* loop */
for ( i = 0 ; i < 64 ; i++ ) {
/* show */
show_value( *(gdat+i) );
/* show current binary format */
show_cur_binary( *(gdat+i) ) ;
/* new line */
if ( (i % 8) == 7 ) { crlf(); }
}
/* loop */
for ( i = 0 ; i < 64 ; i++ ) {
/* default */
bb = '0' ;
/* judge */
if ( *(gdat+i) > avr ) { bb = '1' ; }
/* show */
rs_putchar( bb ) ;
/* */
if ( (i & 7) == 7 && (i != 63) ) { rs_putchar( '_' ); }
}
crlf();
}
void binary_display(byte x)
{
int i ;
for ( i = 7 ; i > -1 ; i-- ) {
rs_putchar( '0' + ((x >> i) & 1) );
}
}
void show_bgdat()
{
byte i ;
for ( i = 0 ; i < 8 ; i++ ) {
binary_display( *(bgdat+i) );
if ( i < 7 ) { rs_putchar('_'); }
}
crlf();
}
byte judge_edge(byte x)
{
byte result ;
/* judge */
switch ( x ) {
/* black -> white */
case 0x01 :
case 0x03 :
case 0x07 :
case 0x0f :
case 0x1f :
case 0x3f :
case 0x7f :
result = 1 ;
break ;
/* white -> black */
case 0x80 :
case 0xC0 :
case 0xE0 :
case 0xF0 :
case 0xF8 :
case 0xFC :
case 0xFE :
result = 2 ;
break ;
/* all white */
case 0xff :
result = 3 ;
break ;
/* others */
default :
result = 0 ;
}
return result ;
}
void show_sensor()
{
char xmsg[17];
byte i ;
byte j ;
byte k ;
/* default */
*(xmsg+16) = '\0';
/* loop */
for ( i = 0 ; i < 8 ; i++ ) {
/* convert code */
j = judge_edge( *(bgdat+i) );
/* calculate location */
k = (i << 1);
/* convert binary */
*(xmsg+k) = '0' ;
*(xmsg+k+1) = '0' ;
if ( j == 1 ) { *(xmsg+k+1) = '1' ; }
if ( j == 2 ) { *(xmsg+k) = '1' ; }
if ( j == 3 ) {
*(xmsg+k) = '1' ;
*(xmsg+k+1) = '1' ;
}
}
/* show */
rs_puts( xmsg );
/* new line */
crlf();
}
word gen_sensor_bit()
{
word result ;
byte i ;
byte j ;
byte k ;
/* clear */
result = 0 ;
/* loop */
for ( i = 0 ; i < 8 ; i++ ) {
/* convert code */
j = judge_edge( *(bgdat+i) );
/* calculate location */
k = 15 - (i << 1);
/* convert binary */
result |= (j << k);
}
return result ;
}
byte gen_sensor_nibble(word x)
{
byte result ;
/* default */
result = ILLEAGAL ;
/* all black */
if ( x == 0x0000 ) { result = ALL_BLACK ; }
/* both white */
if ( x == 0xC001 ||
x == 0xC003 ||
x == 0x8001 ||
x == 0x8003 ) {
result = BOTH_WHITE ;
}
/* all white */
if ( x == 0xffff ) { result = ALL_WHITE ; }
/* left white */
if ( x == 0xff00 ||
x == 0xfe00 ||
x == 0xfc00 ) {
result = LEFT_WHITE ;
}
/* right white */
if ( x == 0x00ff ||
x == 0x007f ||
x == 0x003f ) {
result = RIGHT_WHITE ;
}
/* center */
if ( x == 0x03c0 ||
x == 0x01c0 ||
x == 0x0380 ) {
result = CENTER ;
}
/* tiny right */
if ( x == 0x00f0 ||
x == 0x0070 ) {
result = TINY_RIGHT ;
}
/* right */
if ( x == 0x0078 ||
x == 0x003c ) {
result = RIGHT ;
}
/* big right */
if ( x == 0x001e ||
x == 0x000f ) {
result = BIG_RIGHT ;
}
/* tiny left */
if ( x == 0x0700 ||
x == 0x0f00 ) {
result = TINY_LEFT ;
}
/* left */
if ( x == 0x1e00 ||
x == 0x3c00 ) {
result = LEFT ;
}
/* big left */
if ( x == 0x7800 ||
x == 0xf000 ) {
result = BIG_LEFT ;
}
return result ;
}
void show_value(word x)
{
/* delimiter */
*(msg+5) = '\0' ;
/* separate */
for ( int jj = 4 ; jj > -1 ; jj-- ) {
*(msg+jj) = get_asc( x % 10 ) ; x /= 10 ;
}
/* zero surpress */
if ( *(msg+0) == '0' ) {
*(msg+0) = ' ' ;
if ( *(msg+1) == '0' ) {
*(msg+1) = ' ' ;
if ( *(msg+2) == '0' ) {
*(msg+2) = ' ' ;
}
}
}
/* show */
rs_puts( msg );
}
/* MtTimer2 interrupt handler */
void update_trigger(void)
{
send_led( xcnt & ON );
xcnt++ ;
}
void send_led(byte x)
{
if ( x ) { PORTB |= (1 << LED_BIT); }
else { PORTB &= ~(1 << LED_BIT); }
}
/* initialilze */
void setup()
{
/* initialize serial port */
Serial.begin(9600);
sindex = 0 ;
/* pin configuration */
PORTB = 0b00010000 ;
PORTC = 0b00001000 ;
PORTD = 0b00000001 ;
/* pin direction */
DDRB = 0b11101111 ;
DDRC = 0b11111100 ;
DDRD = 0b11111110 ;
/* clear flags */
uflag = OFF ;
/* others */
scnt = 0 ;
xcnt = 0 ;
/* initialize */
clear_gdat();
clear_bgdat();
*(params+0) = 0x80 ; *(params+1) = 0x03 ;
*(params+2) = 0x00 ; *(params+3) = 0x0F ;
*(params+4) = 0x01 ; *(params+5) = 0x00 ;
*(params+6) = 0x01 ; *(params+7) = 0x21 ;
lnum = 32 ;
rx = (lnum << 7) ;
ry = rx + 128 ;
/* 500ms period */
MsTimer2::set(500,update_trigger);
/* enable */
MsTimer2::start();
}
/* endless loop */
void loop()
{
byte i ;
byte j ;
byte k ;
char msgx[5];
/* command interpreter */
if ( uflag == ON ) {
/* clear flag */
uflag = OFF ;
/* new line */
crlf();
/* get command */
cmd = *(sbuf+0) ;
/* help */
if ( cmd == '?' ) { show_help() ; }
/* set parameter */
if ( cmd == 'P' ) {
/* get register number */
rnum = get_hex( *(sbuf+1) ) ;
/* get parameter */
tmp = get_hex( *(sbuf+2) ) ;
tmp <<= 4 ;
tmp |= get_hex( *(sbuf+3) ) ;
/* store */
*(params+rnum) = tmp ;
}
/* show parameter */
if ( cmd == 'p' ) {
/* fixed */
*(msgx+4) = '\0' ;
*(msgx+1) = ':' ;
/* parameters */
for ( i = 0 ; i < 8 ; i++ ) {
/* register number */
*(msgx+0) = get_asc( i ) ;
/* parameters */
*(msgx+2) = get_asc( (*(params+i) >> 4) & MASK0F ) ;
*(msgx+3) = get_asc( *(params+i) & MASK0F ) ;
/* show */
rs_puts( msgx );
/* hexadecimal */
rs_putchar('h');
/* space */
rs_putchar(' ');
}
/* new line */
crlf();
}
/* set line number */
if ( cmd == 'L' ) {
/* clear */
lnum = 0 ;
/* loop */
for ( i = 1 ; i < 3 ; i++ ) {
/* get */
tmp = *(sbuf+i) ;
/* judge */
if ( tmp == '\r' ) break ;
/* x 10 */
lnum = (lnum << 2) + lnum ;
lnum <<= 1 ;
/* add */
lnum += get_hex(tmp) ;
}
/* judge */
if ( lnum > 63 || lnum == 0 ) {
/* default */
lnum = 32 ;
/* show range */
rs_puts("incorrect! range(1 -> 63)");
/* new line */
crlf();
} else {
rx = (lnum << 7) ;
ry = rx + 128 ;
}
}
/* show line number */
if ( cmd == 'l' ) {
/* show */
show_value( lnum );
/* new line */
crlf();
}
/* get graphic data code */
if ( cmd == 'G' ) {
/* message */
rs_puts("Start ");
/* new line */
crlf() ;
/* perform */
get_gbc(lnum);
/* message */
rs_puts("Complete !");
/* new line */
crlf() ;
}
/* show graphic data */
if ( cmd == 'S' ) {
/* binary data */
show_bgdat();
/* 16bits code */
show_sensor();
/* raw data */
show_gdat();
/* generate sensor data */
PORTB &= 0xf0 ;
PORTB = gen_sensor_nibble(gen_sensor_bit());
}
/* test */
if ( cmd == 'T' ) {
/* set random data */
if ( *(sbuf+1) == '1' ) {
randomSeed( xcnt );
/* generate data */
for ( k = 0 ; k << 64 ; k++ ) {
/* area */
tmp = 128 + random(0,64);
if ( k < 16 || k > 47 ) { tmp = 128 - random(0,64); }
*(gdat+k) = tmp ;
}
/* calculate */
word xmax ;
word xmin ;
xmax = *(gdat+0) ;
xmin = *(gdat+0) ;
for ( k = 0 ; k < 64 ; k++ ) {
if ( xmax < *(gdat+k) ) { xmax = *(gdat+k); }
if ( xmin > *(gdat+k) ) { xmin = *(gdat+k); }
}
avr = (xmax + xmin) >> 1 ;
/* show */
rs_puts("Average = ");
show_value( avr );
/* new line */
crlf();
/* convert */
clear_bgdat();
for ( k = 0 ; k < 64 ; k++ ) {
if ( *(gdat+k) > avr ) { store_binary(k); }
}
}
}
}
}
/* receive interrupt */
void serialEvent()
{
char ch;
if ( Serial.available() > 0 ) {
/* get 1 character */
ch = Serial.read();
/* store */
*(sbuf+sindex) = ch ;
/* increment */
sindex++ ;
/* judge */
if ( ch == '\r' ) {
sindex = 0 ;
uflag = ON ;
}
}
}
16ビットのセンサーデータから白線の幅とライン位置を
算出できないかを考えてみました。次のアルゴリズムで
簡単に求められました。
- 元の16ビットデータを右に１ビットシフト
- 元の16ビットとシフトした16ビットで排他的論理和を求める
- 排他的論理和の16ビット中で１がたっている位置を求める
- ２つの１の位置の差を白線の幅に
- ２つの１の位置の和の平均をライン位置に
過去にも同じアルゴリズムを利用しましたが、右端まで白が
存在する場合の処理が甘く、コースアウトさせていました。
右端まで白が存在する場合、２つの１の位置が同じであり
右端が白だったときは、右の１の位置を＋１してから計算
すれば、対応できました。
白線の幅とライン位置を正しく算出できるかを、シミュレーション
してみました。
RAWが元の16ビットデータ、right shiftは１ビット右シフトした
16ビット、edgeが排他的論理和の結果。
0000_0000_0000_0000 = RAW
0000_0000_0000_0000 = right shift
0000_0000_0000_0000 = edge
31 31 0(width) 31(center)
1111_1111_1111_1111 = RAW
0111_1111_1111_1111 = right shift
1000_0000_0000_0000 = edge
0 16 16(width) 8(center)
1111_1111_0000_0000 = RAW
0111_1111_1000_0000 = right shift
1000_0000_1000_0000 = edge
0 8 8(width) 4(center)
0000_0000_1111_1111 = RAW
0000_0000_0111_1111 = right shift
0000_0000_1000_0000 = edge
8 16 8(width) 12(center)
1100_0000_0000_0011 = RAW
0110_0000_0000_0001 = right shift
1010_0000_0000_0010 = edge
0 14 14(width) 7(center)
1100_0000_0000_0111 = RAW
0110_0000_0000_0011 = right shift
1010_0000_0000_0100 = edge
0 13 13(width) 6(center)
1110_0000_0000_0011 = RAW
0111_0000_0000_0001 = right shift
1001_0000_0000_0010 = edge
0 14 14(width) 7(center)
1110_0000_0000_0111 = RAW
0111_0000_0000_0011 = right shift
1001_0000_0000_0100 = edge
0 13 13(width) 6(center)
0000_1111_1111_0000 = RAW
0000_0111_1111_1000 = right shift
0000_1000_0000_1000 = edge
4 12 8(width) 8(center)
0001_1111_1111_0000 = RAW
0000_1111_1111_1000 = right shift
0001_0000_0000_1000 = edge
3 12 9(width) 7(center)
0011_1111_1111_1111 = RAW
0001_1111_1111_1111 = right shift
0010_0000_0000_0000 = edge
2 16 14(width) 9(center)
0011_1111_1111_1110 = RAW
0001_1111_1111_1111 = right shift
0010_0000_0000_0001 = edge
2 15 13(width) 8(center)
0111_1111_1111_1110 = RAW
0011_1111_1111_1111 = right shift
0100_0000_0000_0001 = edge
1 15 14(width) 8(center)
1111_1111_1111_1110 = RAW
0111_1111_1111_1111 = right shift
1000_0000_0000_0001 = edge
0 15 15(width) 7(center)
1111_1111_1111_1100 = RAW
0111_1111_1111_1110 = right shift
1000_0000_0000_0010 = edge
0 14 14(width) 7(center)
1111_1111_1111_1000 = RAW
0111_1111_1111_1100 = right shift
1000_0000_0000_0100 = edge
0 13 13(width) 6(center)
1111_1111_1111_0000 = RAW
0111_1111_1111_1000 = right shift
1000_0000_0000_1000 = edge
0 12 12(width) 6(center)
0000_0000_0000_1000 = RAW
0000_0000_0000_0100 = right shift
0000_0000_0000_1100 = edge
12 13 1(width) 12(center)
0000_0000_0000_0001 = RAW
0000_0000_0000_0000 = right shift
0000_0000_0000_0001 = edge
15 16 1(width) 15(center)
白線の幅とライン位置が算出できているとわかりました。
使ったＣ言語のソースコードは、以下。
#include <stdio.h>
#define OFF 0
#define ON OFF+1
typedef unsigned char UBYTE ;
typedef unsigned short UWORD ;
void binary_display16(UWORD x)
{
int i ;
int j ;
/* loop */
for ( i = 15 ; i > -1 ; i-- ) {
putchar('0'+((x >> i) & 1));
j = 15 - i ;
if ( (j % 4) == 3 && i > 0 ) { putchar('_'); }
}
}
UBYTE get_lp(UWORD x)
{
UBYTE result ;
int i ;
/* default */
result = 31 ;
/* search */
for ( i = 15 ; i > -1 ; i-- ) {
/* check */
if ( x & 0x8000 ) { result = 15-i ; }
/* detect */
if ( result != 31 ) break ;
/* shift */
x <<= 1 ;
}
return result ;
}
UBYTE get_rp(UWORD x)
{
UBYTE result ;
UBYTE i ;
/* default */
result = 31 ;
/* search */
for ( i = 0 ; i < 16 ; i++ ) {
/* check */
if ( x & ON ) { result = 15-i ; }
/* detect */
if ( result != 31 ) break ;
/* shift */
x >>= 1 ;
}
return result ;
}
void check_position(UWORD x)
{
UWORD xx[2] ;
UBYTE lp ;
UBYTE rp ;
UBYTE xw ;
UBYTE xc ;
/* store */
*(xx+0) = (x >> 1) ;
/* calculate */
*(xx+1) = x ^ *(xx+0) ;
/* show */
binary_display16(x); puts(" = RAW");
binary_display16(*(xx+0)); puts(" = right shift");
binary_display16(*(xx+1)); puts(" = edge");
/* get location */
lp = get_lp(*(xx+1)) ;
rp = get_rp(*(xx+1)) ;
/* judge */
if ( lp == rp && (x & ON) ) { rp = 16 ; }
/* calculate */
xw = rp - lp ;
xc = (rp + lp) >> 1 ;
/* show */
printf("%d %d %d(width) %d(center)\n",lp,rp,xw,xc);
/* new line */
putchar('\n');
}
void main(void)
{
/* all black */
check_position(0);
/* all white */
check_position(0xffff);
/* left white */
check_position(0xff00);
/* right white */
check_position(0x00ff);
/* both white */
check_position(0xc003);
check_position(0xc007);
check_position(0xe003);
check_position(0xe007);
/* random */
check_position(0x0ff0);
check_position(0x1ff0);
check_position(0x3fff);
check_position(0x3ffe);
check_position(0x7ffe);
check_position(0xfffe);
check_position(0xfffc);
check_position(0xfff8);
check_position(0xfff0);
check_position(8);
check_position(1);
}
センサーユニットから４ビットの情報として出力するための
関数は以下となります。
typedef unsigned char UBYTE ;
typedef unsigned short UWORD ;
#define ALL_BLACK 0
#define ALL_WHITE 1
#define LEFT_WHITE 2
#define RIGHT_WHITE 3
#define CENTER 4
#define TINY_RIGHT 5
#define RIGHT 6
#define BIG_RIGHT 7
#define TINY_LEFT 8
#define LEFT 9
#define BIG_LEFT 10
#define BOTH_WHITE 11
#define ILLEAGAL 12
UBYTE get_lp(UWORD x)
{
UBYTE result ;
int i ;
/* default */
result = 31 ;
/* search */
for ( i = 15 ; i > -1 ; i-- ) {
/* check */
if ( x & 0x8000 ) { result = 15-i ; }
/* detect */
if ( result != 31 ) break ;
/* shift */
x <<= 1 ;
}
return result ;
}
UBYTE get_rp(UWORD x)
{
UBYTE result ;
UBYTE i ;
/* default */
result = 31 ;
/* search */
for ( i = 0 ; i < 16 ; i++ ) {
/* check */
if ( x & ON ) { result = 15-i ; }
/* detect */
if ( result != 31 ) break ;
/* shift */
x >>= 1 ;
}
return result ;
}
UBYTE generate_sdat(UWORD x)
{
UWORD xx[2] ;
UBYTE lp ;
UBYTE rp ;
UBYTE xwidth ;
UBYTE xcenter ;
UBYTE result ;
/* store */
*(xx+0) = (x >> 1) ;
/* calculate */
*(xx+1) = x ^ *(xx+0) ;
/* get location */
lp = get_lp(*(xx+1)) ;
rp = get_rp(*(xx+1)) ;
/* judge */
if ( lp == rp && (x & ON) ) { rp = 16 ; }
/* calculate */
xwidth = rp - lp ;
xcenter = (rp + lp) >> 1 ;
/* generate sensor data */
result = ILLEAGAL ;
if ( xwidth == 0 ) { result = ALL_BLACK ; }
if ( xwidth == 16 ) { result = ALL_WHITE ; }
if ( 0 < xwidth && xwidth < 16 ) {
if ( 0 <= xcenter && xcenter < 2 ) {
result = BIG_LEFT ;
if ( xwidth > 3 ) { result = LEFT_WHITE ; }
}
if ( 2 <= xcenter && xcenter < 4 ) {
result = LEFT ;
if ( xwidth > 3 ) { result = LEFT_WHITE ; }
}
if ( 4 <= xcenter && xcenter < 6 ) {
result = TINY_LEFT ;
if ( xwidth > 4 ) { result = LEFT_WHITE ; }
}
if ( 6 <= xcenter && xcenter <= 9 ) {
result = CENTER ;
if ( xwidth > 10 ) { result = BOTH_WHITE ; }
}
if ( 10 <= xcenter && xcenter < 12 ) {
result = TINY_RIGHT ;
if ( xwidth > 4 ) { result = RIGHT_WHITE ; }
}
if ( 12 <= xcenter && xcenter < 14 ) {
result = RIGHT ;
if ( xwidth > 3 ) { result = RIGHT_WHITE ; }
}
if ( 14 <= xcenter && xcenter < 16 ) {
result = BIT_RIGHT ;
if ( xwidth > 3 ) { result = RIGHT_WHITE ; }
}
}
/* impress */
PORTB &= 0xf0 ;
PORTB |= result ;
return result ;
}
関数generate_sdatに16ビットのセンサーデータを渡すと
制御プロセッサに情報を出力できます。
黒から白、白から黒と変化する点がいくつあるのかで
場合分けして、４ビットの情報を制御プロセッサへと
出力するスケッチは、以下。
/*
Game Boy Camera handling (GBCY)
Pin assignment
PORTB
PB.B5
PB.B4 VBout <input>
PB.B3 Sensor3 <output>
PB.B2 Sensor2 <output>
PB.B1 Sensor1 <output>
PB.B0 Sensor0 <output>
PORTC
PC.B5 SIN (GBC) <output>
PC.B4 LOAD(GBC) <output>
PC.B3 XRST(GBC) <output>
PC.B2 XCK (GBC) <output>
PC.B1 Read(GBC) <input>
PC.B0 Vout(GBC) <input>
PORTD
PD.B7
PD.B6
PD.B5
PD.B4 START(GBC) <output>
PD.B3
PD.B2
PD.B1 TxD
PD.B0 RxD
*/
#include <MsTimer2.h>
#define OFF 0
#define ON OFF+1
#define MASK0F 0x0f
#define MASKF0 0xf0
#define MASKFF 0xff
#define MASK400 0x400
#define START_BIT 4
#define SIN_BIT 5
#define LOAD_BIT 4
#define XRST_BIT 3
#define XCK_BIT 2
#define READ_BIT 1
#define LED_BIT 5
#define CMP_BIT 4
#define PIXLAST 8064
#define ALL_BLACK 0
#define ALL_WHITE 1
#define LEFT_WHITE 2
#define RIGHT_WHITE 3
#define CENTER 4
#define TINY_RIGHT 5
#define RIGHT 6
#define BIG_RIGHT 7
#define TINY_LEFT 8
#define LEFT 9
#define BIG_LEFT 10
#define BOTH_WHITE 11
#define ILLEAGAL 12
boolean uflag ;
boolean aflag ;
boolean tflag ;
byte tmp ;
byte xcnt ;
word scnt ;
char sbuf[8];
byte sindex ;
char cmd ;
byte params[8];
byte rnum ;
byte lnum ;
word gbc[64]; /* raw data */
word gbcs[64]; /* shifted raw data */
char bgdat[64];
char msg[6] ;
word rcounter ; /* raster counter */
word rx ; /* line begin pixel */
word ry ; /* line finish pixel */
byte sum_cnt ; /* edge counter */
byte left_loc ; /* left edge location */
byte right_loc ; /* right edge location */
byte bport ;
void rs_putchar(char x)
{
Serial.write(x);
}
void rs_puts(char *x)
{
while ( *x != 0 ) {
rs_putchar( *x ) ;
x++ ;
}
}
void crlf()
{
rs_putchar('\r');
rs_putchar('\n');
}
void show_help()
{
rs_puts("? help"); crlf();
rs_puts("A enable auto"); crlf();
rs_puts("a disable auto"); crlf();
rs_puts("P set parameters"); crlf();
rs_puts("p show parameters"); crlf();
rs_puts("G get graphic data"); crlf();
rs_puts("S show graphic data"); crlf();
}
byte get_hex(char x)
{
byte result ;
/* default */
result = 0 ;
/* convert */
if ( '0' <= x && x <= '9' ) { result = x - '0' ; }
if ( 'A' <= x && x <= 'F' ) { result = x - 'A' + 10 ; }
if ( 'a' <= x && x <= 'f' ) { result = x - 'a' + 10 ; }
return result ;
}
char get_asc(byte x)
{
char result ;
/* default */
result = '0' ;
/* convert */
if ( x < 10 ) { result = x + '0' ; }
else { result = x - 10 + 'A' ; }
return result ;
}
void put_sin(boolean x)
{
if ( x ) { PORTC |= (1 << SIN_BIT); }
else { PORTC &= ~(1 << SIN_BIT); }
/* delay */
delayMicroseconds(1);
}
void put_load(boolean x)
{
if ( x ) { PORTC |= (1 << LOAD_BIT); }
else { PORTC &= ~(1 << LOAD_BIT); }
}
void put_rst(boolean x)
{
if ( x ) { PORTC |= (1 << XRST_BIT); }
else { PORTC &= ~(1 << XRST_BIT); }
/* delay */
delayMicroseconds(1);
}
void put_xck(boolean x)
{
if ( x ) { PORTC |= (1 << XCK_BIT); }
else { PORTC &= ~(1 << XCK_BIT); }
/* delay */
delayMicroseconds(1);
}
void put_start(boolean x)
{
if ( x ) { PORTD |= (1 << START_BIT); }
else { PORTD &= ~(1 << START_BIT); }
/* delay */
delayMicroseconds(1);
}
void rst_gbc(void)
{
/* LOW */
put_rst(OFF);
/* XCK : H */
put_xck(ON);
/* XCK : L */
put_xck(OFF);
/* HIGH */
put_rst(ON);
}
void start_gbc(void)
{
/* HIGH */
put_start(ON);
/* XCK : H */
put_xck(ON);
/* XCK : L */
put_xck(OFF);
/* LOW */
put_start(OFF);
}
void send_params(word x)
{
word xtmp ;
byte i ;
/* copy */
xtmp = x ;
/* default */
put_load(OFF);
/* loop */
for ( i = 0 ; i < 11 ; i++ ) {
/* impress data */
put_sin(OFF);
if ( xtmp & MASK400 ) { put_sin(ON); }
/* load */
if ( i == 10 ) { put_load(ON); }
/* XCK : H */
put_xck(ON);
/* shift */
xtmp <<= 1;
/* XCK : L */
put_xck(OFF);
}
/* default */
put_sin(OFF);
put_load(OFF);
}
void init_gbc(void)
{
byte i ;
/* reset */
rst_gbc();
/* send parameters */
for ( i = 0 ; i < 8 ; i++ ) { send_params( (i << 8) | *(params+i) ) ; }
}
boolean get_read()
{
boolean result ;
/* default */
result = OFF ;
/* judge */
if ( PINC & (1 << READ_BIT) ) { result = ON ; }
return result ;
}
boolean is_range(word x)
{
boolean result ;
/* default */
result = OFF ;
/* judge */
if ( rx <= x && x < ry ) { result = ON ; }
return result ;
}
void clear_gdat()
{
byte i ;
for ( i = 0 ; i < 64 ; i++ ) { *(gbc+i) = 0 ; }
}
void calc_location()
{
byte i ;
byte j ;
word r0 ;
word r1 ;
byte xsum ;
byte rl ;
byte ll ;
boolean sflag ;
/* store shift data */
*(gbcs+0) = 0 ;
/* calculate difference */
for ( i = 0 ; i < 64 ; i++ ) {
/* default */
*(bgdat+i) = -1 ;
/* temporary */
r0 = *(gbc+i) ;
r1 = *(gbcs+i) ;
/* judge */
if ( r0 > r1 ) { *(bgdat+i) = 1 ; }
if ( r0 == r1 ) { *(bgdat+i) = 0 ; }
}
/* count (negative) */
xsum = 0 ;
for ( i = 0 ; i < 64 ; i++ ) {
if ( *(bgdat+i) < 0 ) { xsum++ ; }
}
/* no change point */
rl = 0 ; /* right */
ll = 0 ; /* left */
/* one change point */
if ( xsum == 1 ) {
sflag = OFF ;
for ( i = 0 ; i < 64 ; i++ ) {
/* found */
if ( *(bgdat+i) == -1 ) {
sflag = ON ;
ll = i ;
}
/* exit */
if ( sflag == ON ) break ;
}
}
/* multi change point */
if ( xsum > 1 ) {
/* set fixed value */
xsum = 2 ;
/* left */
sflag = OFF ;
for ( i = 0 ; i < 64 ; i++ ) {
/* found */
if ( *(bgdat+i) == -1 ) {
sflag = ON ;
ll = i ;
}
/* exit */
if ( sflag == ON ) break ;
}
/* right */
sflag = OFF ;
for ( i = 0 ; i < 64 ; i++ ) {
/* revese */
j = 63 - i ;
/* found */
if ( *(bgdat+j) == -1 ) {
sflag = ON ;
rl = j ;
}
/* exit */
if ( sflag == ON ) break ;
}
}
/* result */
sum_cnt = xsum ;
left_loc = ll ;
right_loc = rl ;
}
void get_gbc(byte x)
{
word ii ;
byte j ;
word rawx ;
/* clear raw data and binary data */
clear_gdat();
/* initialize GBC */
init_gbc();
/* start */
start_gbc();
rcounter = 0 ;
/* send dummy clock (10 clocks) */
for ( ii = 0 ; ii < 10 ; ii++ ) {
/* XCK : H */
put_xck(ON);
/* XCK : L */
put_xck(OFF);
/* judge */
if ( get_read() == ON ) {
rcounter = ii ;
break ;
}
}
/* show READ location */
show_value( rcounter );
crlf();
/* skip fist line */
for ( ii = 0 ; ii < 127 ; ii++ ) {
/* XCK : H */
put_xck(ON);
/* XCK : L */
put_xck(OFF);
}
/* line handling */
j = 0 ;
for ( ii = 0 ; ii < PIXLAST ; ii++ ) {
/* XCK : H */
put_xck(ON);
/* XCK : L */
put_xck(OFF);
/* get raw data */
rawx = analogRead(0) ;
/* judge target line */
if ( is_range(ii+128) == ON ) {
/* debug */
show_value( rawx );
putchar(' ');
if ( (ii % 8) == 7 ) { crlf() ; }
/* store */
if ( (ii & ON) == ON ) {
/* store raw data */
*(gbc+j) = rawx ;
/* store shifted raw data */
if ( j ) { *(gbcs+j) = *(gbc+j-1) ; }
/* increment */
j++ ;
}
}
/* judge */
if ( j > 127 ) break ;
}
/* get change point location */
calc_location();
}
void show_value(word x)
{
/* delimiter */
*(msg+5) = '\0' ;
/* separate */
for ( int jj = 4 ; jj > -1 ; jj-- ) {
*(msg+jj) = get_asc( x % 10 ) ;
x /= 10 ;
}
/* zero surpress */
if ( *(msg+0) == '0' ) {
*(msg+0) = ' ' ;
if ( *(msg+1) == '0' ) {
*(msg+1) = ' ' ;
if ( *(msg+2) == '0' ) { *(msg+2) = ' ' ; }
}
}
/* show */
rs_puts( msg );
}
/* MtTimer2 interrupt handler */
void update_trigger(void)
{
send_led( xcnt & ON );
xcnt++ ;
if ( (xcnt % 4) == 3 ) { tflag = ON ; }
}
void send_led(byte x)
{
if ( x ) { PORTB |= (1 << LED_BIT); }
else { PORTB &= ~(1 << LED_BIT); }
}
void show_gdat()
{
byte i ;
for ( i = 0 ; i < 64 ; i++ ) {
show_value( *(gbc+i) );
if ( (i % 8) == 7 ) { crlf(); }
}
crlf();
}
byte gen_sensor_nibble()
{
byte result ;
byte ws ;
word sval ;
/* default */
result = ILLEAGAL ;
sval = *(gbc+32) ;
/* calculate wight center */
ws = (left_loc + right_loc) >> 1 ;
/* ALL_BLACK or ALL_WHITE */
if ( sum_cnt == 0 ) {
result = ALL_WHITE ;
if ( sval < 90 ) { result = ALL_BLACK ; }
}
/* LEFT_WHITE or RIGHT_WHITE */
if ( sum_cnt == 1 ) {
result = LEFT_WHITE ;
if ( ws > 49 ) { result = RIGHT_WHITE ; }
}
/* */
if ( sum_cnt == 2 ) {
/* judge CENTER */
if ( 28 < ws && ws < 35 ) { result = CENTER ; }
/* judge RIGHT side */
if ( 34 < ws && ws < 38 ) { result = TINY_RIGHT ; }
if ( 37 < ws && ws < 44 ) { result = RIGHT ; }
if ( 43 < ws && ws < 50 ) { result = BIG_RIGHT ; }
/* judge LEFT side */
if ( 25 < ws && ws < 29 ) { result = TINY_LEFT ; }
if ( 18 < ws && ws < 26 ) { result = LEFT ; }
if ( 11 < ws && ws < 19 ) { result = BIG_LEFT ; }
/* judge both end line */
if ( left_loc < 15 && 45 < right_loc ) { result = BOTH_WHITE ; }
}
return result ;
}
void impress_information()
{
bport = PORTB & MASKF0 ;
bport |= gen_sensor_nibble();
PORTB = bport ;
}
void show_info()
{
byte info ;
/* get information */
info = gen_sensor_nibble();
/* show */
switch ( info ) {
case ALL_BLACK : rs_puts("B"); break;
case ALL_WHITE : rs_puts("W"); break;
case LEFT_WHITE : rs_puts("LW"); break;
case RIGHT_WHITE : rs_puts("RW"); break;
case CENTER : rs_puts("C"); break;
case TINY_RIGHT : rs_puts("R"); break;
case RIGHT : rs_puts("RR"); break;
case BIG_RIGHT : rs_puts("RRR"); break;
case TINY_LEFT : rs_puts("L"); break;
case LEFT : rs_puts("LL"); break;
case BIG_LEFT : rs_puts("LLL"); break;
case BOTH_WHITE : rs_puts("WW"); break;
default : rs_puts("??"); break;
}
/* new line */
crlf();
}
/* initialilze */
void setup()
{
/* initialize serial port */
Serial.begin(9600);
sindex = 0 ;
/* pin configuration */
PORTB = 0b00010000 ; /* PB4 : pull up */
PORTC = 0b00001000 ; /* PC3 reset */
PORTD = 0b00000001 ;
/* pin direction */
DDRB = 0b11101111 ; /* PB4 : input , others : outputs */
DDRC = 0b11111100 ; /* PC1 and PC0 are inputs , others : output*/
DDRD = 0b11111110 ; /* PD4 = GBC start , PD1 = TxD PD0 = RxD */
/* set flags */
uflag = OFF ;
aflag = ON ;
tflag = OFF ;
/* others */
scnt = 0 ;
xcnt = 0 ;
/* initialize */
clear_gdat();
*(params+0) = 0x80 ; *(params+1) = 0x03 ;
*(params+2) = 0x00 ; *(params+3) = 0x0F ;
*(params+4) = 0x01 ; *(params+5) = 0x00 ;
*(params+6) = 0x01 ; *(params+7) = 0x21 ;
/* target line number */
lnum = 32 ;
/* target pixel number */
rx = (lnum << 7) ;
ry = rx + 128 ;
/* 500ms period */
MsTimer2::set(500,update_trigger);
/* enable */
MsTimer2::start();
}
/* endless loop */
void loop()
{
byte i ;
char msgx[5];
/* command interpreter */
if ( uflag == ON ) {
/* clear flag */
uflag = OFF ;
/* new line */
crlf();
/* get command */
cmd = *(sbuf+0) ;
/* help */
if ( cmd == '?' ) { show_help() ; }
/* enable auto */
if ( cmd == 'A' ) { aflag = ON ; }
/* disabel auto */
if ( cmd == 'a' ) { aflag = OFF ; }
/* set parameter */
if ( cmd == 'P' ) {
/* get register number */
rnum = get_hex( *(sbuf+1) ) ;
/* get parameter */
tmp = get_hex( *(sbuf+2) ) ;
tmp <<= 4 ;
tmp |= get_hex( *(sbuf+3) ) ;
/* store */
*(params+rnum) = tmp ;
}
/* show parameter */
if ( cmd == 'p' ) {
/* fixed */
*(msgx+4) = '\0' ;
*(msgx+1) = ':' ;
/* parameters */
for ( i = 0 ; i < 8 ; i++ ) {
/* register number */
*(msgx+0) = get_asc( i ) ;
/* parameters */
*(msgx+2) = get_asc( (*(params+i) >> 4) & MASK0F ) ;
*(msgx+3) = get_asc( *(params+i) & MASK0F ) ;
/* show */
rs_puts( msgx );
/* hexadecimal */
rs_putchar('h');
/* space */
rs_putchar(' ');
}
/* new line */
crlf();
}
/* get graphic data code */
if ( cmd == 'G' ) {
/* message */
rs_puts("Start "); crlf();
/* perform */
get_gbc(lnum);
/* message */
rs_puts("Complete !"); crlf();
}
/* show graphic data */
if ( cmd == 'S' ) {
/* raw data */
show_gdat();
/* show information */
show_info();
/* impress */
impress_information();
}
}
/* auto */
if ( tflag == ON ) {
/* clear flag */
tflag = OFF ;
/* perform */
if ( aflag == ON ) {
/* perform */
get_gbc(lnum);
/* impress */
impress_information();
}
}
}
/* receive interrupt */
void serialEvent()
{
char ch;
if ( Serial.available() > 0 ) {
/* get 1 character */
ch = Serial.read();
/* store */
*(sbuf+sindex) = ch ;
/* increment */
sindex++ ;
/* judge */
if ( ch == '\r' ) {
sindex = 0 ;
uflag = ON ;
}
}
}
目次
前
次
