目次
前
次
路面センサーにカメラを利用
MCRでは、床面の白線に従ってマシンを走行させます。
路面センサーを携帯電話やスマートフォンで
使われるカメラを利用して走行させます。
床面の白線さえ認識できれば、センサーはカメラ
であろうが、赤外線センサーであろうが構わない
のです。
カメラには、人工網膜チップを採用している
Game Boy Cameraを使います。
人工網膜チップは、光の感度を人間の視覚に近く
してあるので、扱いやすいと言われています。
Game Boy Camera(GBC)から、情報を取得するには
Arduinoを使います。
車にのせて走行しなければならないので、GBC
Arduino、AMFORTHと並べて板に固定。
マシンに載せると、以下。
左の紫色ワイヤーがのびている基板がGBC。
その右にArduino、AMFORTH基板を続けて配置。
Arduinoのプログラムは、以下。
/*
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 ;
}
}
}
GBCから取得した路面の情報は、次のコードに変換しています。
0 ALL_BLACK
1 ALL_WHITE
2 LEFT_WHITE
3 RIGHT_WHITE
4 CENTER
5 TINY_RIGHT
6 RIGHT
7 BIG_RIGHT
8 TINY_LEFT
9 LEFT
10 BIG_LEFT
11 BOTH_WHITE
12 ILLEAGAL
４ビット情報を、AMFORTHに入力できるようにします。
10ピンケーブルを利用し、接続は以下。
1 Vcc : Vcc
2
3
4
5
6 GBC_3 : PORTC.B3
7 GBC_2 : PORTC.B2
8 GBC_1 : PORTC.B1
9 GBC_0 : PORTC.B0
10 GND : GND
AMFORTHは、次の制御基板に接続。
モータは12Vの産業用を使ってます。
MCRでは、単３アルカリ電池あるいは２次
電池８本との規定があるので、DCDC変換
器で12Vまで昇圧。
タイヤの後方にある基板に、DCDC変換器を
配置してあります。この位置に配置すると
走行中にショートなどの危険が減ります。
モータ制御基板とカメラをまとめて配置する
ことを考えて、ダイソーから部材を入手。
カメラ用の穴をあけて、Arduino基板を配置して
いきます。Arduino基板は、２枚使い、片方には
AmForthを入れておきます。
左からGBC、Arduino基板、AmForth基板と並べて
ボックスの下に電池を置けば、安定します。
目次
前
次
