目次
前
次
最終スケッチ
Arduinoの最終スケッチは、次の3処理に分けて
作成しました。
各機能に関し、備忘録としての説明を残します。
移動制御
移動制御は、次の3モードを用意して対応します。
NORMALはセンサー情報のみで移動します。
CRANK、LANEはシーケンス制御します。
センサー情報は、以下の13種がセンサーユニット
から出力されます。カッコ内は、ユニットが出力
する数値です。
- ALL_BLACK ( 0)
- ALL_WHITE ( 1)
- LEFT_WHITE ( 2)
- RIGHT_WHITE ( 3)
- CENTER ( 4)
- TINY_RIGHT ( 5)
- RIGHT ( 6)
- BIG_RIGHT ( 7)
- TINY_LEFT ( 8)
- LEFT ( 9)
- BIG_LEFT (10)
- BOTH_WHITE (11)
- ILLEAGAL (12)
NORMAL→CRANKの状態遷移は、センサー情報に
ALL_WHITEを検出した場合に限定します。
CRANK→NORMALの状態遷移は、センサー情報に
次のどれかを検出したときにしました。
- CENTER (4)
- TINY_RIGHT (5)
- TINY_LEFT (8)
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への遷移は、時間を利用しました。
ハードウエアデバッグ
ハードウエアデバッグには、シリアルインタフェースを
利用します。
デバッグ用に、コマンドを定義します。
- ? ヘルプ
- A モード変更での移動テスト
- L システム状態を示すLEDのテスト
- P 左右のモータにPWM波形で使うDUTY比設定
- S システム状態を文字列で表示
- G スタートゲートの状態表示LEDのテスト
- D センサーデータを文字列で表示
各コマンドに対応した関数を定義しました。
ヘルプ
文字列を表示するステートメントを並べて対応。
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_IDLE アイドル
- SYSMODE_WAIT スタート指示待ち
- SYSMODE_MOVE 移動
状態遷移図で示すと、以下。
スタート指示は、スイッチと超音波センサーのどちらでも
対応できるようにしています。
システム状態が、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のシーケンス
処理に潜んでしたバグを見つけ出すことができました。
目次
前
次