目次

最終スケッチ

 Arduinoの最終スケッチは、次の3処理に分けて
 作成しました。

 各機能に関し、備忘録としての説明を残します。


移動制御

 移動制御は、次の3モードを用意して対応します。  NORMALはセンサー情報のみで移動します。  CRANK、LANEはシーケンス制御します。  センサー情報は、以下の13種がセンサーユニット  から出力されます。カッコ内は、ユニットが出力  する数値です。  NORMAL→CRANKの状態遷移は、センサー情報に  ALL_WHITEを検出した場合に限定します。  CRANK→NORMALの状態遷移は、センサー情報に  次のどれかを検出したときにしました。  NORMAL→LANEの状態遷移は、センサー情報に  LEFT_WHITE、RIGHT_WHITEを検出した場合に  限定します。  LANE→NORMALの状態遷移は、CRANK→NORMALの  状態遷移と同じ条件とします。  モードNORMALは、デジタル回路の組合せ回路に相当  CRANK、LANEは順序回路と言えるでしょう。  CRANK、LANEのシーケンスの考え方を記します。  CRANK   ALL_WHITEを検出後、ALL_BLACKのエリアまで   スロー走行で直進します。   ALL_BLACKエリアに到達する途中に、左か右の   片側白線LEFT_WHITE、RIGHT_WHITEがあるので   それを記憶しておきます。   ALL_BLACKエリアに到着したなら、記憶している   方向に旋回します。   中央に白線が見えたなら、CRANK→NORMALと状態   遷移させます。   シーケンス処理コードは、以下とすればよいと   わかります。 /* sequencer */ switch ( state ) { /* move to ALL_BLACK area */ case 0 : lcmove() ; sensor = get_sensor(); if ( sensor == ALL_BLACK ) { state = 1 ; } break ; /* turn */ case 1 : crank_turn(); sensor = get_sensor(); if ( is_close_center(sensor) == ON ) { state = 2 ; } break ; /* return NORMAL */ case 2 : mode = NORMAL ; state = 0 ; break ; default : break ; }   移動と旋回には、専用関数を用意して対応。   移動は、NORMALの場合とほぼ同じですが、利用する   DUTY比が異なります。 void lcmove() { /* skip ALL_BLACK ALL_WHITE BOTH_WHITE ILLEAGAL */ /* get sensor data */ sensor = get_sensor() ; if ( sensor == LEFT_WHITE ) { xdir = DIR_LEFT ; } if ( sensor == RIGHT_WHITE ) { xdir = DIR_RIGHT; } if ( sensor == CENTER ) { set_duty(30,30); xdir = DIR_NONE ; } if ( sensor == BIG_RIGHT ) { set_duty(10,30); xdir = DIR_RIGHT; } if ( sensor == RIGHT ) { set_duty(15,30); xdir = DIR_NONE ; } if ( sensor == TINY_RIGHT ) { set_duty(20,30); xdir = DIR_NONE ; } if ( sensor == TINY_LEFT ) { set_duty(30,20); xdir = DIR_NONE ; } if ( sensor == LEFT ) { set_duty(30,15); xdir = DIR_NONE ; } if ( sensor == BIG_LEFT ) { set_duty(30,10); xdir = DIR_LEFT ; } /* delay */ delay(100) ; }   LEFT_WHITE、RIGHT_WHITEを検出していれば、記憶し   その他の値を検出した場合は、DUTY比を設定します。   旋回は記憶している方向に回転し、中央の白線が見えたなら   直進に切り替えます。 void crank_turn() { /* turn */ if ( xdir == DIR_RIGHT ) { set_duty(30,15) ; } if ( xdir == DIR_LEFT ) { set_duty(15,30) ; } /* delay */ delay(100); }   中央の白線は、真ん中だけではなく、多少右や左に   寄っていても、よいとします。 byte is_close_center(byte x) { byte result ; /* default */ result = OFF ; /* judge */ if ( x == CENTER ) { result = ON ; } if ( x == TINY_RIGHT ) { result = ON ; } if ( x == TINY_LEFT ) { result = ON ; } return result ; }   ALL_WHITEを検出した場合の処理は、単純にしておきます。 if ( sensor == ALL_WHITE ) { /* slow */ set_duty(30,30); /* change mode */ mode = CRANK ; /* set state */ state = 0 ; }  LANE   LEFT_WHITE、RIGHT_WHITEを検出後、ALL_BLACKエリアまで   スロー走行で直進します。   LEFT_WHITE、RIGHT_WHITEのどちらになっているかを   記憶しておき、ALL_BLACKエリアに入ったなら、記憶   している方向に車線変更します。   シーケンスにまとめると、以下。 /* sequencer */ switch ( state ) { /* move to ALL_BLACK area */ case 0 : lcmove() ; sensor = get_sensor(); if ( sensor == ALL_BLACK ) { state = 1 ; } break ; /* turn */ case 1 : lane_turn(); state = 2 ; break ; /* move on ALL_BLACK area */ case 2 : lane_move(); sensor = get_sensor(); if ( is_close_center(sensor) == ON ) { state = 3 ; } break ; /* turn */ case 3 : lane_turn_second(); sensor = get_sensor(); if ( is_close_center(sensor) == ON ) { state = 4 ; } break ; /* return NORMAL */ case 4 : mode = NORMAL ; state = 0 ; break ; default : break ; }   スロー走行、旋回、黒エリア走行は、専用関数で対応します。   スロー走行は、CRANKモードと同じなので、旋回と黒エリア走行   を考えます。   旋回は、次のようにしました。 void lane_turn() { /* turn */ if ( xdir == DIR_RIGHT ) { set_duty(30,20) ; } if ( xdir == DIR_LEFT ) { set_duty(20,30) ; } /* delay */ delay(100); /* return straight */ set_duty(30,30); } void lane_turn_second() { /* opposite turn */ if ( xdir == DIR_RIGHT ) { set_duty(20,30) ; } if ( xdir == DIR_LEFT ) { set_duty(30,20) ; } /* delay */ delay(100); }   記憶している方向に、シャーシ前方を傾けた後   直進に戻します。   黒エリア走行は、中央の白線を検出するまで直進し   逆ハンドルを切ってから、直進に戻します。 void lane_move() { /* straight */ set_duty(30,30) ; /* delay */ delay(100); }   LEFT_WHITE、RIGHT_WHITEの検出では、次のように   モードを変更します。 if ( sensor == LEFT_WHITE ) { lane_begin( DIR_LEFT ) ; } if ( sensor == RIGHT_WHITE ) { lane_begin( DIR_RIGHT ); }   モード変更とDUTY比設定には、関数lane_beginを使います。 void lane_begin(byte x) { /* slow */ set_duty(30,30); /* change mode */ mode = LANE ; /* set state */ state = 0 ; /* store direction */ xdir = x ; }   車線変更のための方向は、変数xdirに記憶しておけば   レーンチェンジ開始と終了で使えます。  走行モードは、次の状態遷移図を使いました。  IDLEからBLINDへの遷移には、ゲートセンサーかスイッチトリガー  の状態を利用します。  NORMAL、CRANK、LANEの状態遷移では、センサー情報を使います。  BLINDからNORMALへの遷移は、時間を利用しました。

ハードウエアデバッグ

 ハードウエアデバッグには、シリアルインタフェースを  利用します。  デバッグ用に、コマンドを定義します。  各コマンドに対応した関数を定義しました。  ヘルプ   文字列を表示するステートメントを並べて対応。 void show_help() { rs_puts("? help"); crlf(); rs_puts("A active mode setting"); crlf(); rs_puts("L led check"); crlf(); rs_puts("P set motor duty ratio");crlf(); rs_puts("S show system status"); crlf(); rs_puts("G stat gate state"); crlf(); rs_puts("D show sensor data"); crlf(); }  モード変更での移動テスト   モードを'N'、'C'、'L'のいずれかで指定し、続けて   NORMAL、CRANK、LANEの場合の方向指示情報を入力。  システム状態を示すLEDのテスト   デフォルトでLEDを消灯しておき   '1'を与えられると点灯します。   関数を利用します。 void set_led(byte x) { /* turn off */ PORTB &= ~(1 << LED_BIT) ; /* turn on */ if ( x ) { PORTB |= (1 << LED_BIT) ; } }  左右のモータにPWM波形で使うDUTY比設定   コマンドの後に、左、右の順にDUTY比を   10進数で指定します。   10進数の2数を取り出して、次の関数に渡します。 void set_duty(byte lx,byte rx) { byte ldat ; byte rdat ; /* generate code */ ldat = 0x80 | (lx & MASK7F); rdat = (rx & MASK7F ); /* left */ PORTB &= ~(1 << SS_BIT) ; SPI.transfer(ldat) ; PORTB |= (1 << SS_BIT) ; /* 100us interval */ delayMicroseconds(100) ; /* right */ PORTB &= ~(1 << SS_BIT) ; SPI.transfer(rdat) ; PORTB |= (1 << SS_BIT) ; }   SPIインタフェースで、左右のモータの   DUTY比を渡します。  システム状態を文字列で表示   システム状態は、変数sysmodeに格納しておき   値を取得後、文字列で表示します。  スタートゲートの状態表示LEDのテスト   スタートゲートの状態は、Arduinoのピンに接続した   信号で判定できるので、関数を利用して取得し文字列   で表示します。 byte get_start_status() { byte result ; /* defalt */ result = OPENED ; /* judge */ if ( PINC & (1 << START_BIT) ) { result = CLOSED ; } return result ; }  センサーデータを文字列で表示   センサー値を取得し、文字列に変換して表示します。 switch ( sensor ) { case ALL_BLACK : strcopy(msg,"ALL_BLACK" ); break; case ALL_WHITE : strcopy(msg,"ALL_WHITE" ); break; case LEFT_WHITE : strcopy(msg,"LEFT_WHITE" ); break; case RIGHT_WHITE : strcopy(msg,"RIGHT_WHITE"); break; case CENTER : strcopy(msg,"CENTER" ); break; case BIG_RIGHT : strcopy(msg,"BIG_RIGHT" ); break; case RIGHT : strcopy(msg,"RIGHT" ); break; case TINY_RIGHT : strcopy(msg,"TINY_RIGHT" ); break; case TINY_LEFT : strcopy(msg,"TINY_LEFT" ); break; case LEFT : strcopy(msg,"LEFT" ); break; case BIG_LEFT : strcopy(msg,"BIG_LEFT" ); break; case BOTH_WHITE : strcopy(msg,"BOTH_WHITE" ); break; default : strcopy(msg,"ILLEAGAL" ); break; }

その他

 Arduinoには、標準でシリアルインタフェース  ライブラリが用意されています。ライブラリは  オブジェクトのメソッドで扱うためプログラム  容量が増え、必要な処理を入れられないことが  おきる可能性があります。  1文字表示のオブジェクトメソッドをラッピングする  関数を定義し、文字列表示と改行を再定義しました。  1文字表示のラッパー関数は、以下。 void rs_putchar(char x) { Serial.write(x); }  文字列表示関数は、C言語のテキストなら  大抵の書籍に出ている内容で対応。 void rs_puts(char *ptr) { while ( *ptr ) { rs_putchar( *ptr ); ptr++; } }  改行は、rs_putcharを利用して  次のように定義。 void crlf() { rs_putchar('\r'); rs_putchar('\n'); }

スケッチリスト

#include <MsTimer2.h> #include <SPI.h> #define OFF 0 #define ON OFF+1 #define LED_BIT 1 #define SEL_BIT 4 #define START_BIT 5 #define SS_BIT 2 #define MOSI_BIT 3 #define SCK_BIT 5 #define TRG_BIT 3 #define OPENED 0 #define CLOSED 1 #define SYSMODE_IDLE 0 #define SYSMODE_WAIT 1 #define SYSMODE_MOVE 2 #define NORMAL 0 #define CRANK 1 #define LANE 2 #define NONE 3 #define MASK7F 0x7f #define MASK0F 0x0f #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 #define DIR_NONE 0 #define DIR_RIGHT 1 #define DIR_LEFT 2 #define TIMCNTMAX 600 /* 90sec */ /* variables */ byte tflag ; byte uflag ; byte strg ; byte gtrg ; byte strg_sft ; byte sysmode ; byte mode ; byte cmd ; byte sindex ; byte sbuf[16] ; byte lduty ; byte rduty ; byte sensor ; byte lcnt ; char msg[16]; byte xdir ; byte ssidx ; byte state ; byte xcnt ; byte gate_status ; word timcnt ; /* function prototype */ void update_trigger(); void show_help(); void set_led(byte x); void send_led(); void set_duty(byte lx,byte rx); byte get_hex(byte x); byte get_sensor(); void send_trg(); void force_stop(); void timeup(byte x); void timeup_stop(); void timeup_stopx(); void rs_putchar(char x); void rs_puts(char *ptr); void crlf(); void strcopy(char *dst,char *src); void cl_begin_primitive(byte x); void crank_begin(); void lane_begin(byte x); void lcmove(); void crank_turn(); void lane_turn(); void lane_move(); void lane_turn_second(); byte is_close_center(byte x); byte is_catch_line(byte x); void begin_move(); void show_mode(byte x); void send_mmode(byte x); void setup() { /* initialize serial */ Serial.begin(9600); sindex = 0 ; uflag = OFF ; /* initialize port values */ PORTB = 0x15 ; PORTC = 0xff ; PORTD = 0xc5 ; /* initialize port direction */ DDRB = 0x2e ; DDRC = 0x10 ; DDRD = 0xfa ; /* set system mode */ sysmode = SYSMODE_IDLE ; /* set mode */ mode = NORMAL ; /* clear shift register */ strg_sft = 0 ; lcnt = 0 ; ssidx = 0 ; state = 0 ; xcnt = 0 ; timcnt = 0 ; tflag = OFF ; /* start gate status */ gate_status = CLOSED ; /* initialize SPI interface */ SPI.begin(); SPI.setBitOrder(MSBFIRST); SPI.setDataMode(SPI_MODE0); SPI.setClockDivider(SPI_CLOCK_DIV2); set_duty(0,0); /* 150ms period */ MsTimer2::set(150,update_trigger); /* enable */ MsTimer2::start(); /* enable extern interrupt */ attachInterrupt(0,timeup_stop,RISING); } void loop() { byte i ; /* start trigger handling */ if ( strg == ON ) { /* clear event flag */ strg = OFF ; /* change system mode */ switch ( sysmode ) { case SYSMODE_IDLE : sysmode = SYSMODE_WAIT ; break ; case SYSMODE_WAIT : sysmode = SYSMODE_MOVE ; break ; case SYSMODE_MOVE : sysmode = SYSMODE_IDLE ; break ; default : sysmode = SYSMODE_IDLE ; break ; } /* show system mode */ show_mode( sysmode ) ; /* move handler */ if ( sysmode == SYSMODE_MOVE ) { begin_move(); } if ( sysmode == SYSMODE_IDLE ) { force_stop(); } } /* start gate handling */ if ( gtrg == ON ) { /* clear event flag */ gtrg = OFF ; /* change mode */ if ( sysmode == SYSMODE_WAIT ) { sysmode = SYSMODE_MOVE ; show_mode( sysmode ) ; begin_move(); } } /* time up (software timer) */ if ( tflag == ON ) { /* clear event flag */ tflag = OFF ; if ( sysmode == SYSMODE_MOVE ) { timeup_stopx(); } } /* serial handling */ if ( uflag == ON ) { /* clear event flag */ uflag = OFF ; /* new line */ crlf(); /* get command */ cmd = *(sbuf+0) ; /* help */ if ( cmd == '?' ) { show_help() ; } /* active mode setting */ if ( cmd == 'A' ) { /* change mode */ mode = NONE ; if ( *(sbuf+1) == 'N' ) { mode = NORMAL ; } if ( *(sbuf+1) == 'C' ) { mode = CRANK ; } if ( *(sbuf+1) == 'L' ) { mode = LANE ; } /* impress */ send_mmode( mode ); /* direction */ xdir = get_hex( *(sbuf+2) ); } /* system status */ if ( cmd == 'S' ) { /* default */ strcopy(msg,"IDLE"); /* select */ if ( sysmode == SYSMODE_WAIT ) { strcopy(msg,"WAIT"); } if ( sysmode == SYSMODE_MOVE ) { strcopy(msg,"MOVE"); } /* show */ rs_puts( msg ); crlf(); /* direction */ strcopy(msg,"NON") ; if ( xdir == DIR_RIGHT ) { strcopy(msg,"RIGHT"); } if ( xdir == DIR_LEFT ) { strcopy(msg,"LEFT" ); } rs_puts( msg ); crlf(); } /* start gate state */ if ( cmd == 'U' ) { /* default */ strcopy(msg,"CLOSED"); /* OPENED */ if ( gate_status == OPENED ) { strcopy(msg,"OPENED"); } /* show */ rs_puts( msg ); crlf(); } /* show sensor data */ if ( cmd == 'D' ) { /* copy */ sensor = get_sensor() ; /* set strings */ switch ( sensor ) { case ALL_BLACK : strcopy(msg,"ALL_BLACK" ); break; case ALL_WHITE : strcopy(msg,"ALL_WHITE" ); break; case LEFT_WHITE : strcopy(msg,"LEFT_WHITE" ); break; case RIGHT_WHITE : strcopy(msg,"RIGHT_WHITE"); break; case CENTER : strcopy(msg,"CENTER" ); break; case BIG_RIGHT : strcopy(msg,"BIG_RIGHT" ); break; case RIGHT : strcopy(msg,"RIGHT" ); break; case TINY_RIGHT : strcopy(msg,"TINY_RIGHT" ); break; case TINY_LEFT : strcopy(msg,"TINY_LEFT" ); break; case LEFT : strcopy(msg,"LEFT" ); break; case BIG_LEFT : strcopy(msg,"BIG_LEFT" ); break; case BOTH_WHITE : strcopy(msg,"BOTH_WHITE" ); break; default : strcopy(msg,"ILLEAGAL" ); break; } /* show */ rs_puts( msg ) ; crlf(); } } /* move */ if ( sysmode == SYSMODE_MOVE ) { /* get sensor data */ sensor = get_sensor(); /* NORMAL */ if ( mode == NORMAL ) { /* LED handling */ send_led(); /* send mode */ send_mmode( mode ); /* judge */ if ( sensor == ALL_WHITE ) { crank_begin(); } if ( sensor == LEFT_WHITE ) { lane_begin( DIR_LEFT ); } if ( sensor == RIGHT_WHITE ) { lane_begin( DIR_RIGHT ); } if ( sensor == CENTER ) { set_duty(50,50); } if ( sensor == BIG_RIGHT ) { set_duty(15,50); } if ( sensor == RIGHT ) { set_duty(25,50); } if ( sensor == TINY_RIGHT ) { set_duty(45,50); } if ( sensor == TINY_LEFT ) { set_duty(50,15); } if ( sensor == LEFT ) { set_duty(50,25); } if ( sensor == BIG_LEFT ) { set_duty(50,45); } /* skip BOTH_WHITE ILLEAGAL */ /* delay */ delay( 100 ) ; } /* CRANK */ if ( mode == CRANK ) { /* sequencer */ switch ( state ) { /* move to ALL_BLACK area */ case 0 : lcmove() ; sensor = get_sensor(); if ( sensor == ALL_BLACK ) { state = 1 ; } break ; /* turn */ case 1 : crank_turn(); sensor = get_sensor(); if ( is_close_center(sensor) == ON ) { state = 2 ; } break ; /* return NORMAL */ case 2 : mode = NORMAL ; state = 0 ; break ; default : break ; } send_led(); } /* LANE */ if ( mode == LANE ) { /* sequencer */ switch ( state ) { /* move to ALL_BLACK area */ case 0 : lcmove() ; sensor = get_sensor(); if ( sensor == ALL_BLACK ) { state = 1 ; } break ; /* 1st turn */ case 1 : lane_turn(); state = 2 ; break ; /* move on ALL_BLACK area */ case 2 : lane_move(); sensor = get_sensor(); /* ? reach the center line */ if ( is_catch_line(sensor) == ON ) { state = 3 ; } send_led(); break ; /* 2nd turn */ case 3 : lane_turn_second(); state = 4 ; break ; /* return NORMAL */ case 4 : mode = NORMAL ; state = 0 ; break ; default : break ; } send_led(); } } } void update_trigger() { /* gate state */ gate_status = CLOSED ; if ( !(PINC & (1 << START_BIT)) ) { gate_status = OPENED ; gtrg = ON ; } /* led handling */ lcnt++ ; if ( sysmode == SYSMODE_IDLE ) { set_led( lcnt & ON ) ; } /* shift */ strg_sft <<= 1 ; /* mask */ strg_sft &= 0x03 ; /* update LSB */ if ( (PINB & 0x01) == OFF ) { strg_sft |= ON ; } /* judge */ if ( strg_sft == 0x01 ) { strg = ON ; } /* ? time up */ timcnt++ ; if ( timcnt == TIMCNTMAX ) { timcnt = 0 ; tflag = ON ; } } void show_help() { rs_puts("? help"); crlf(); rs_puts("A active mode setting"); crlf(); /* rs_puts("P set motor duty ratio");crlf(); */ rs_puts("S show system status"); crlf(); rs_puts("U start gate state"); crlf(); rs_puts("D show sensor data"); crlf(); } byte get_sensor() { byte result ; /* get GBC nibble */ PORTC |= (1 << SEL_BIT); result = PINC & MASK0F ; return result ; } /* 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 ; } } } void set_led(byte x) { /* turn off */ PORTB &= ~(1 << LED_BIT) ; /* turn on */ if ( x ) { PORTB |= (1 << LED_BIT) ; } } void send_led() { xcnt++ ; set_led( xcnt & ON ); } void set_duty(byte lx,byte rx) { byte ldat ; byte rdat ; byte lrtmp[4] ; /* set both duty ratios */ ldat = (lx & MASK7F); rdat = (rx & MASK7F ); /* generate code */ *(lrtmp+0) = 0x30 | ((ldat >> 4) & MASK0F) ; *(lrtmp+1) = 0x20 | (ldat & MASK0F) ; *(lrtmp+2) = 0x10 | ((rdat >> 4) & MASK0F) ; *(lrtmp+3) = (rdat & 0x0f) ; /* loop */ for ( byte i = 0 ; i < 4 ; i++ ) { /* enable */ PORTB &= ~(1 << SS_BIT) ; /* transfer */ SPI.transfer(*(lrtmp+i)) ; /* disable */ PORTB |= (1 << SS_BIT) ; /* 10us interval */ delayMicroseconds(10) ; } } byte get_hex(byte x) { byte result ; /* default */ result = 0 ; /* judge */ 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 ; } void send_trg() { /* TRG : H */ PORTD |= (1 << TRG_BIT); /* wait */ delayMicroseconds(100); /* TRG : L */ PORTD &= ~(1 << TRG_BIT); } void force_stop() { /* show message */ rs_puts("force_stop"); crlf(); /* stop both motors */ set_duty(0,0); /* change mode */ sysmode = SYSMODE_IDLE ; show_mode( sysmode ); send_mmode( NONE ); } void timeup(byte x) { /* show message */ strcopy(msg,"TIME UP !") ; rs_puts( msg ) ; strcopy(msg,"(hardware)"); if ( x ) { strcopy(msg,"(software)"); } rs_puts( msg ); crlf(); /* stop both motors */ set_duty(0,0); /* change mode */ sysmode = SYSMODE_IDLE ; show_mode( sysmode ); send_mmode( NONE ); } void timeup_stop() { timeup(OFF); } void timeup_stopx() { timeup(ON); } void rs_putchar(char x) { Serial.write(x); } void rs_puts(char *ptr) { while ( *ptr ) { rs_putchar( *ptr ); ptr++; } } void crlf() { rs_putchar('\r'); rs_putchar('\n'); } void strcopy(char *dst,char *src) { while ( *src ) { *dst = *src ; dst++ ; src++ ; } *dst = '\0' ; } void cl_begin_primitive(byte x) { /* slow */ set_duty(25,25); /* change mode */ mode = x ; /* send mode */ send_mmode( mode ); /* set state */ state = 0 ; } void crank_begin() { cl_begin_primitive(CRANK) ; } void lane_begin(byte x) { cl_begin_primitive(LANE) ; /* store direction */ xdir = x ; } void lcmove() { /* skip ALL_BLACK ALL_WHITE BOTH_WHITE ILLEAGAL */ /* get sensor data */ sensor = get_sensor() ; if ( sensor == LEFT_WHITE ) { xdir = DIR_LEFT ; } if ( sensor == RIGHT_WHITE ) { xdir = DIR_RIGHT; } if ( sensor == CENTER ) { set_duty(25,25); xdir = DIR_NONE ; } if ( sensor == BIG_RIGHT ) { set_duty( 5,25); xdir = DIR_RIGHT; } if ( sensor == RIGHT ) { set_duty(10,25); xdir = DIR_NONE ; } if ( sensor == TINY_RIGHT ) { set_duty(15,25); xdir = DIR_NONE ; } if ( sensor == TINY_LEFT ) { set_duty(25, 5); xdir = DIR_NONE ; } if ( sensor == LEFT ) { set_duty(25,10); xdir = DIR_NONE ; } if ( sensor == BIG_LEFT ) { set_duty(25,15); xdir = DIR_LEFT ; } /* delay */ delay(100) ; } void crank_turn() { /* turn */ if ( xdir == DIR_RIGHT ) { set_duty(35, 5) ; } if ( xdir == DIR_LEFT ) { set_duty( 5,35) ; } /* delay */ delay(100); } void lane_turn() { /* turn */ if ( xdir == DIR_RIGHT ) { set_duty(35,15) ; } if ( xdir == DIR_LEFT ) { set_duty(15,35) ; } /* delay */ delay(100); /* return straight */ set_duty(35,35); } void lane_move() { /* straight */ set_duty(35,35) ; /* delay */ delay(100); } void lane_turn_second() { /* opposite turn */ if ( xdir == DIR_RIGHT ) { set_duty(15,35) ; } if ( xdir == DIR_LEFT ) { set_duty(35,15) ; } /* delay */ delay(100); } byte is_close_center(byte x) { byte result ; /* default */ result = OFF ; /* judge */ if ( x == CENTER ) { result = ON ; } if ( x == TINY_RIGHT ) { result = ON ; } if ( x == TINY_LEFT ) { result = ON ; } return result ; } #define ALL_BLACK 0 byte is_catch_line(byte x) { byte result ; /* default */ result = ON ; /* judge */ if ( x == ILLEAGAL ) { result = OFF ; } if ( x == ALL_BLACK ) { result = ON ; } return result ; } void begin_move() { /* clear counter */ timcnt = 0 ; /* turn on LED */ set_led(ON) ; /* start timer */ /* send_trg(); */ /* blind run */ set_duty(15,15); delay(500) ; set_duty(35,35); delay(500) ; set_duty(50,50); delay(500) ; /* set SYSMODE_MOVE */ mode = NORMAL ; send_mmode( mode ); } void show_mode(byte x) { byte xcode ; /* judge */ switch ( x ) { case SYSMODE_WAIT : xcode = 1 ; break ; case SYSMODE_MOVE : xcode = 2 ; break ; default : xcode = 3 ; break ; } /* adjust */ xcode <<= 6 ; /* impress */ PORTD &= 0x37 ; PORTD |= xcode ; } void send_mmode(byte x) { byte tmp ; /* judge mode */ tmp = 0xf0 ; if ( x == NORMAL ) { tmp |= 3 ; } if ( x == CRANK ) { tmp |= 1 ; } if ( x == LANE ) { tmp |= 2 ; } /* enable */ PORTB &= ~(1 << SS_BIT) ; /* transfer */ SPI.transfer( tmp ) ; /* disable */ PORTB |= (1 << SS_BIT) ; }  システム状態は、次の3種類としました。  状態遷移図で示すと、以下。  スタート指示は、スイッチと超音波センサーのどちらでも  対応できるようにしています。  システム状態が、SYSMODE_MOVEのときに、NORMAL  CRANK、LANEの3モードの判断を入れました。  どのシステム状態にいるのかは、外付けLEDの  点灯パターンで確認できます。

机上動作チェック

 センサーに対しての処理が妥当か、Pythonスクリプトで  確認しました。  Pythonスクリプトは、以下。 #!/usr/bin/python # show duty ratio setting def set_duty(x): # concatenate result = "<left(" + str(x[0]) + ")_right(" + str(x[1]) + ")>" # return result def is_center(x): result = 0 if x == "CENTER" or x == "TINY_RIGHT" or x == "TINY_LEFT" : result = 1 return result def show_value(x) : # clear strings addstr = "" # get parameters xx = x[0] modex = x[1] # flag = 0 # judge if xx == "ALL_WHITE" : tmpx = set_duty([30,30]) addstr = "-> CRANK" if xx == "LEFT_WHITE" : tmpx = set_duty([30,30]) addstr = "-> LANE Directoin(DIR_LEFT)" if xx == "RIGHT_WHITE" : tmpx = set_duty([30,30]) addstr = "-> LANE Directoin(DIR_RIGHT)" if xx == "CENTER" : tmpx = set_duty([50,50]) flag = 1 if xx == "BIG_RIGHT" : tmpx = set_duty([25,50]) if xx == "RIGHT" : tmpx = set_duty([35,50]) flag = 1 if xx == "TINY_RIGHT" : tmpx = set_duty([45,50]) if xx == "TINY_LEFT" : tmpx = set_duty([50,45]) if xx == "LEFT" : tmpx = set_duty([50,35]) flag = 1 if xx == "BIG_LEFT" : tmpx = set_duty([50,25]) # show addx = "\t:" if flag == 1 : addx = "\t\t:" print xx,addx,tmpx,modex,addstr def show_value_seq(x) : patx = "\t" if x[1] == "LANE" : patx = "\t\t" print x[0],"\t:",x[1],patx,"state(",str(x[2]),")" # define control def execute(x) : # get informations xmode = x[0] sensorx = x[1] t = x[2] # NORMAL mode result = "NONE" if xmode == "NORMAL" : # default result = ["NORMAL",t] if sensorx == "ALL_WHITE" : show_value( [sensorx,xmode] ) result = ["CRANK",t] if sensorx == "LEFT_WHITE" : show_value( [sensorx,xmode] ) result = ["LANE",t] if sensorx == "RIGHT_WHITE" : show_value( [sensorx,xmode] ) result = ["LANE",t] if sensorx == "CENTER" : show_value( [sensorx,xmode] ) if sensorx == "BIG_RIGHT" : show_value( [sensorx,xmode] ) if sensorx == "RIGHT" : show_value( [sensorx,xmode] ) if sensorx == "TINY_RIGHT" : show_value( [sensorx,xmode] ) if sensorx == "TINY_LEFT" : show_value( [sensorx,xmode] ) if sensorx == "LEFT" : show_value( [sensorx,xmode] ) if sensorx == "BIG_LEFT" : show_value( [sensorx,xmode] ) # CRANK if xmode == "CRANK" : # default result = ["CRANK",t] # sequencer if t == 0 : print " lcmove() \t=>", if sensorx == "ALL_BLACK" : t = 1 result = ["CRANK",t] elif t == 1 : print " crank_turn() \t=>", if is_center(sensorx) == 1 : t = 2 result = ["CRANK",t] elif t == 2 : print " return NORMAL \t=>", xmode = "NORMAL" t = 0 result = ["NORMAL",t] # show show_value_seq([sensorx,xmode,t]) # LANE if xmode == "LANE" : # default result = ["LANE",t] # sequencer if t == 0 : print " lcmove() \t\t=>", if ( sensorx == "ALL_BLACK" ) : t = 1 result = ["LANE",t] elif t == 1 : print " lane_turn() \t\t=>", t = 2 result = ["LANE",t] elif t == 2 : print " lane_move() \t\t=>", if is_center(sensorx) == 1 : t = 3 result = ["LANE",t] elif t == 3 : print " lane_turn_second() \t=>", if is_center(sensorx) == 1 : t = 4 result = ["LANE",t] elif t == 4 : print " return NORMAL \t\t=>", xmode = "NORMAL" t = 0 result = ["NORMAL",0] # show show_value_seq([sensorx,xmode,t]) # return result # initialize mode mode = "NORMAL" # open fin = open('sensor.txt','r') line = fin.read() dout = line.split('\n') # close fin.close() state = 0 for e in dout : xmode = execute( [mode,e,state] ) # judge if mode <> xmode[0] : mode = xmode[0] # change state state = xmode[1]  用意したセンサーデータファイル内容は、以下。 CENTER TINY_RIGHT RIGHT BIG_RIGHT RIGHT TINY_RIGHT CENTER TINY_LEFT LEFT BIG_LEFT LEFT TINY_LEFT CENTER LEFT_WHITE CENTER CENTER ALL_WHITE CENTER CENTER ALL_BLACK ALL_BLACK ALL_BLACK ALL_BLACK ALL_BLACK CENTER CENTER CENTER CENTER TINY_RIGHT RIGHT BIG_RIGHT RIGHT TINY_RIGHT CENTER ALL_WHITE CENTER CENTER ALL_BLACK ALL_BLACK ALL_BLACK TINY_LEFT CENTER CENTER  Pythonスクリプトでチェックすると、次のようになりました。 CENTER : <left(50)_right(50)> NORMAL TINY_RIGHT : <left(45)_right(50)> NORMAL RIGHT : <left(35)_right(50)> NORMAL BIG_RIGHT : <left(25)_right(50)> NORMAL RIGHT : <left(35)_right(50)> NORMAL TINY_RIGHT : <left(45)_right(50)> NORMAL CENTER : <left(50)_right(50)> NORMAL TINY_LEFT : <left(50)_right(45)> NORMAL LEFT : <left(50)_right(35)> NORMAL BIG_LEFT : <left(50)_right(25)> NORMAL LEFT : <left(50)_right(35)> NORMAL TINY_LEFT : <left(50)_right(45)> NORMAL CENTER : <left(50)_right(50)> NORMAL LEFT_WHITE : <left(30)_right(30)> NORMAL -> LANE Directoin(DIR_LEFT) lcmove() => CENTER : LANE state( 0 ) lcmove() => CENTER : LANE state( 0 ) lcmove() => ALL_WHITE : LANE state( 0 ) lcmove() => CENTER : LANE state( 0 ) lcmove() => CENTER : LANE state( 0 ) lcmove() => ALL_BLACK : LANE state( 1 ) lane_turn() => ALL_BLACK : LANE state( 2 ) lane_move() => ALL_BLACK : LANE state( 2 ) lane_move() => ALL_BLACK : LANE state( 2 ) lane_move() => ALL_BLACK : LANE state( 2 ) lane_move() => CENTER : LANE state( 3 ) lane_turn_second() => CENTER : LANE state( 4 ) return NORMAL => CENTER : NORMAL state( 0 ) CENTER : <left(50)_right(50)> NORMAL TINY_RIGHT : <left(45)_right(50)> NORMAL RIGHT : <left(35)_right(50)> NORMAL BIG_RIGHT : <left(25)_right(50)> NORMAL RIGHT : <left(35)_right(50)> NORMAL TINY_RIGHT : <left(45)_right(50)> NORMAL CENTER : <left(50)_right(50)> NORMAL ALL_WHITE : <left(30)_right(30)> NORMAL -> CRANK lcmove() => CENTER : CRANK state( 0 ) lcmove() => CENTER : CRANK state( 0 ) lcmove() => ALL_BLACK : CRANK state( 1 ) crank_turn() => ALL_BLACK : CRANK state( 1 ) crank_turn() => ALL_BLACK : CRANK state( 1 ) crank_turn() => TINY_LEFT : CRANK state( 2 ) return NORMAL => CENTER : NORMAL state( 0 ) CENTER : <left(50)_right(50)> NORMAL  テキストファイルの内容から、CRANKとLANEのシーケンス  処理に潜んでしたバグを見つけ出すことができました。
目次

inserted by FC2 system