目次

デバッグ処理

 利用しているLattice SemiconductorのFPGA内部回路の
 動作をテスト、デバッグするために、Arduinoスケッチ
 を利用しました。

 一足飛びで、FPGA内部回路の動作テストはできないので
 スケッチをいくつか作成し、デバッグに使います。


SENSORデータ出力スケッチ

 センサー情報から、制御回路に指示を出すプロセッサは  Arduinoを利用しています。  センサー情報を受取り、状況判断後、制御回路にPWMの  DUTY比を送信します。状況を判断するために必要な  センサー情報を正しく受け取っているか確認します。  次の接続で、頭脳であるArduinoの動作をテストします。  センサー情報を出力するため、次のコマンドを用意しました。  通信条件は、以下。  コマンド'E'は、次のフォーマットを利用します。  E{value}{address} value 0から12を指定。ただし、10以上は16進を利用  address 10進2桁で00から31の32アドレスを指定  コマンドは、1レコード単位で入力します。  コマンド、パラメータを設定後、{enter}を入力し  1レコードとします。  作成スケッチは、以下。 #include <MsTimer2.h> #include <EEPROM.h> #define OFF 0 #define ON OFF+1 #define BSIZE 8 #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 byte uflag ; byte eflag ; char sbuf[BSIZE] ; byte sindex ; char cmd ; byte state ; byte xdat ; byte xadr ; byte xpat[32] ; /* MtTimer2 interrupt handler */ void update_trigger(void) { eflag = 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 show_help() { rs_puts("? help") ; crlf(); rs_puts("A activate show") ; crlf(); rs_puts("E store data to EEPROM") ; crlf(); rs_puts("e show data form EEPROM"); crlf(); rs_puts("I initialize EEPROM") ; 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 ; } void show_str(byte x) { switch ( x ) { case ALL_BLACK : rs_puts("ALL_BLACK "); break; case ALL_WHITE : rs_puts("ALL_WHITE "); break; case LEFT_WHITE : rs_puts("LEFT_WHITE "); break; case RIGHT_WHITE : rs_puts("RIGHT_WHITE"); break; case CENTER : rs_puts("CENTER "); break; case BIG_RIGHT : rs_puts("BIG_RIGHT "); break; case RIGHT : rs_puts("RIGHT "); break; case TINY_RIGHT : rs_puts("TINY_RIGHT "); break; case TINY_LEFT : rs_puts("TINY_LEFT "); break; case LEFT : rs_puts("LEFT "); break; case BIG_LEFT : rs_puts("BIG_LEFT "); break; case BOTH_WHITE : rs_puts("BOTH_WHITE "); break; default : rs_puts("ILLEAGAL "); break; } } void show_number(byte x) { char msg[2] ; /* convert */ *(msg+0) = (x / 10) + '0' ; *(msg+1) = (x % 10) + '0' ; /* zero suppress */ if ( *(msg+0) == '0' ) { *(msg+0) = ' ' ; } /* transfer */ rs_putchar( *(msg+0) ); rs_putchar( *(msg+1) ); } void setup() { /* initialize serial port*/ Serial.begin(9600); sindex = 0 ; /* initialize PORT value */ PORTB = 0x00 ; PORTC = 0x00 ; PORTD = 0x01 ; /* initialize PORT direction */ DDRB = 0xff ; DDRD = 0xfe ; /* 1000ms period */ MsTimer2::set(1000,update_trigger); /* enable */ MsTimer2::start(); /* clear flags */ uflag = OFF ; eflag = OFF ; /* clear variables */ state = 255 ; xdat = 0 ; xadr = 0 ; } void loop() { byte i ; /* */ if ( eflag == ON ) { /* clear flag */ eflag = OFF ; /* state machine */ if ( state < 32 ) { /* get data */ xdat = *(xpat+state); /* show message */ if ( state == 0 ) { rs_puts("Start !"); crlf(); } /* number */ rs_putchar('<') ; show_number( state ); rs_putchar('>') ; /* impress */ PORTB = xdat ; PORTC = xdat ^ 0x0f ; /* value */ show_number( xdat ); rs_putchar(' ') ; /* strings */ show_str(xdat) ; /* tab */ rs_putchar('\t'); /* new line */ if ( state & ON ) { crlf() ; } /* increment */ state++ ; /* show message */ if ( state == 32 ) { rs_puts("Complete !"); crlf(); } } } /* command interpreter */ if ( uflag == ON ) { /* clear flag */ uflag = OFF ; /* new line */ crlf(); /* get command */ cmd = *(sbuf+0) ; /* help */ if ( cmd == '?' ) { show_help() ; } /* activate */ if ( cmd == 'A' ) { /* transfer data */ for ( i = 0 ; i < 32 ; i++ ) { *(xpat+i) = EEPROM.read(i); } /* clear state */ state = 0 ; } /* store data */ if ( cmd == 'E' ) { /* get data */ xdat = get_hex( *(sbuf+1) ); /* get address */ xadr = get_hex( *(sbuf+2) ) ; xadr *= 10 ; xadr += get_hex( *(sbuf+3) ); /* store */ EEPROM.write( xadr , xdat ); } /* show data */ if ( cmd == 'e' ) { for ( i = 0 ; i < 32 ; i++ ) { /* get data */ xdat = EEPROM.read( i ) ; /* value */ show_number( xdat ); rs_putchar(' ') ; /* strings */ show_str(xdat) ; rs_putchar( '\t' ); /* new line */ if ( i & ON ) { crlf(); } } crlf(); } /* initialize EEPROM */ if ( cmd == 'I' ) { rs_puts("Start ") ; for ( i = 0 ; i < 32 ; i++ ) { EEPROM.write( i , 4 ); if ( i & ON ) { rs_putchar('*') ; } } rs_puts(" Complete"); crlf(); } } } void serialEvent() { char ch; if ( Serial.available() > 0 ) { /* get 1 charactor */ ch = Serial.read(); /* store */ sbuf[sindex] = ch ; /* increment */ sindex++ ; /* judge */ if ( ch == '\r' ) { sindex = 0 ; uflag = ON ; } } }  センサー情報を考えながら、数値指定するのは面倒なので  文字列をテキストファイルに書込み、Python、AWKを使った  スクリプトでアドレスと数値を生成しました。  テキストファイルの例は、以下。 CENTER ALL_WHITE RIGHT BIG_RIGHT RIGHT TINY_RIGHT CENTER ALL_BLACK TINY_LEFT LEFT BIG_LEFT LEFT TINY_LEFT CENTER CENTER  1行をコピーし、センサー情報を増やす Pythonスクリプトを作成します。 #!/usr/bin/python # open fin = open('sensor.txt','r') line = fin.read() dout = line.split('\n') # close fin.close() for e in dout : print e,'\n',e  テキストファイルから、文字列を入力し、リストにした後  リストの各要素をひとつ取出し、文字列として、2回表示  しています。  テキストファイルの内容は、次のように2倍にサイズに。 CENTER CENTER TINY_RIGHT TINY_RIGHT RIGHT RIGHT BIG_RIGHT BIG_RIGHT RIGHT RIGHT TINY_RIGHT TINY_RIGHT CENTER CENTER TINY_LEFT TINY_LEFT LEFT LEFT BIG_LEFT BIG_LEFT LEFT LEFT TINY_LEFT TINY_LEFT CENTER CENTER  センサー情報の数値、格納アドレス、文字列を表示する  AWKスクリプトは、次のように定義しました。 { result = 12 ; if ( $1 == "ALL_BLACK" ) { result = 0 ; } if ( $1 == "ALL_WHITE" ) { result = 1 ; } if ( $1 == "LEFT_WHITE" ) { result = 2 ; } if ( $1 == "RIGHT_WHITE" ) { result = 3 ; } if ( $1 == "CENTER" ) { result = 4 ; } if ( $1 == "TINY_RIGHT" ) { result = 5 ; } if ( $1 == "RIGHT" ) { result = 6 ; } if ( $1 == "BIG_RIGHT" ) { result = 7 ; } if ( $1 == "TINY_LEFT" ) { result = 8 ; } if ( $1 == "LEFT" ) { result = 9 ; } if ( $1 == "BIG_LEFT" ) { result = 10 ; } if ( $1 == "BOTH_WHITE" ) { result = 11 ; } printf("%x %2d %s\n",result,NR,$1) ; }  I/Oリダイレクトを使い、数値とアドレスを  ファイルに書き出すと、以下のように。 4 0 CENTER 4 1 CENTER 5 2 TINY_RIGHT 5 3 TINY_RIGHT 6 4 RIGHT 6 5 RIGHT 7 6 BIG_RIGHT 7 7 BIG_RIGHT 6 8 RIGHT 6 9 RIGHT 5 10 TINY_RIGHT 5 11 TINY_RIGHT 4 12 CENTER 4 13 CENTER 8 14 TINY_LEFT 8 15 TINY_LEFT 9 16 LEFT 9 17 LEFT a 18 BIG_LEFT a 19 BIG_LEFT 9 20 LEFT 9 21 LEFT 8 22 TINY_LEFT 8 23 TINY_LEFT 4 24 CENTER 4 25 CENTER  TeraTermを利用し、Arduinoとのやりとりを見ると  次のようになります。  データとアドレスを入力し、コマンドで内容が  変化したかを確認します。  EEPROMに保存されているセンサー情報を  MCR_VCマシンに与える操作は、以下。  MCR_VCマシンに接続する前に、LEDが載っている  基板で、出力4ビット値を確認しました。  センサー情報を出力すると、モータが回転  するので、制御処理が適切かを、実コース  を利用しないでも、ある程度判断できます。

タイヤ回転数表示

 机上にマシンを置き、DUTY比とタイヤ回転数の関係を  知るため、Arduinoスケッチを作成しました。  タイヤに白テープを貼付け、フォトインタラプタで  反射を拾い、パルスに変換し、5秒ごとの回転数を  カウントします。  フォトインタラプタとインタフェースをあわせた  センサーは、以下です。  パルスのエッジをシフトレジスタで捕らえて  カウンタをインクリメントします。  回路図では、次のようになります。  シフトレジスタの値が2進数で01になれば  立上がりエッジが入ったとして、カウンタを  +1します。  左右のタイヤの回転数を、5秒ごとにシリアルインタフェース  に出力します。  スケッチは、以下。 #include <MsTimer2.h> #define OFF 0 #define ON OFF+1 #define BSIZE 8 byte dflag ; byte uflag ; byte eflag ; byte lflag ; byte rflag ; char sbuf[BSIZE] ; byte sindex ; char cmd ; byte lsft ; byte rsft ; byte lcnt ; byte rcnt ; byte lcntx ; byte rcntx ; char msg[8] ; byte tmp ; byte xcnt ; /* MtTimer2 interrupt handler */ void update_trigger(void) { eflag = ON ; xcnt++ ; if ( xcnt & ON ) { PORTB |= (1 << 5) ; } else { PORTB &= ~(1 << 5) ; } } 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 show_help() { rs_puts("? help") ; crlf(); rs_puts("E enable count") ; crlf(); rs_puts("e disable count"); 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 ; } void setup() { /* initialize serial port*/ Serial.begin(9600); sindex = 0 ; /* initialize PORT value */ PORTB = 0x0f ; PORTC = 0x00 ; PORTD = 0x01 ; /* initialize PORT direction */ DDRB = 0xf0 ; DDRC = 0xff ; DDRD = 0xfe ; /* 5000ms period */ MsTimer2::set(5000,update_trigger); /* enable */ MsTimer2::start(); /* clear flags */ dflag = OFF ; uflag = OFF ; eflag = OFF ; lflag = OFF ; rflag = OFF ; /* clear variables */ lsft = 0 ; rsft = 0 ; lcnt = 0 ; rcnt = 0 ; *(msg+3) = ' ' ; *(msg+7) = '\0' ; } void loop() { /* handling */ { /* shift */ lsft <<= 1 ; rsft <<= 1 ; /* mask */ lsft &= 0x03 ; rsft &= 0x03 ; /* update */ tmp = PINB ; if ( !(tmp & 1) ) { rsft |= ON ; } if ( !(tmp & 2) ) { lsft |= ON ; } /* judge */ if ( rsft == 1 ) { rflag = ON ; } if ( lsft == 1 ) { lflag = ON ; } } /* count */ if ( rflag == ON ) { rflag = OFF ; rcnt++ ; } if ( lflag == ON ) { lflag = OFF ; lcnt++ ; } /* */ if ( eflag == ON ) { /* clear flag */ eflag = OFF ; /* copy */ rcntx = rcnt ; lcntx = lcnt ; /* clear */ rcnt = 0 ; lcnt = 0 ; /* convert */ for ( byte i = 0 ; i < 3 ; i++ ) { *(msg+2-i) = lcntx % 10 + '0' ; *(msg+6-i) = rcntx % 10 + '0' ; lcntx /= 10 ; rcntx /= 10 ; } /* show */ if ( dflag == ON ) { rs_puts( msg ) ; crlf() ; } } /* command interpreter */ if ( uflag == ON ) { /* clear flag */ uflag = OFF ; /* new line */ crlf(); /* get command */ cmd = *(sbuf+0) ; /* help */ if ( cmd == '?' ) { show_help() ; } /* enable */ if ( cmd == 'E' ) { dflag = ON ; } /* disable */ if ( cmd == 'e' ) { dflag = OFF ; } } } void serialEvent() { char ch; if ( Serial.available() > 0 ) { /* get 1 charactor */ ch = Serial.read(); /* store */ sbuf[sindex] = ch ; /* increment */ sindex++ ; /* judge */ if ( ch == '\r' ) { sindex = 0 ; uflag = ON ; } } }  実際は、次のように端末に回転数が表示されます。

DUTY比設定による左右モータテスト

 スタートスイッチを押すか、スタートゲートが開いた  ときに、マシンが走り始めます。  最初の数秒間、直進させるので、左右のモータの  回転数が同じになるように、DUTY比を決めます。  直進のため、左右のDUTY比を合わせるための  スケッチは、以下。 #include <MsTimer2.h> #include <SPI.h> #include <EEPROM.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 SYSMODE_IDLE 0 #define SYSMODE_MOVE 1 #define NORMAL 0 #define CRANK NORMAL+1 #define LANE NORMAL+2 #define MASK7F 0x7f #define DIR_NONE 0 #define DIR_RIGHT 1 #define DIR_LEFT 2 #define OPENED 0 #define CLOSED 1 #define LDUTY_ADR 0xf0 #define RDUTY_ADR LDUTY_ADR+1 #define TIM_ADR LDUTY_ADR+2 /* variables */ byte uflag ; byte strg ; byte strg_sft ; byte sysmode ; byte mode ; byte cmd ; byte sindex ; byte sbuf[16] ; byte lduty ; byte rduty ; byte lcnt ; char msg[16]; byte xdir ; byte ssidx ; byte xcnt ; byte gate_status ; byte xtim ; /* function prototype */ void update_trigger(); void show_help(); void set_led(byte x); void set_duty(byte lx,byte rx); byte get_hex(byte x); void rs_putchar(char x); void rs_puts(char *ptr); void crlf(); void strcopy(char *dst,char *src); byte get_param(byte x); void put_param(byte xadr,byte x); void show_value(byte x); void secdelay(byte x); void setup() { /* initialize serial */ Serial.begin(9600); sindex = 0 ; uflag = OFF ; /* initialize port values */ PORTB = 0x15 ; PORTC = 0xff ; PORTD = 0x05 ; /* initialize port direction */ DDRB = 0x2e ; DDRC = 0x10 ; DDRD = 0xfa ; /* set system mode */ sysmode = SYSMODE_IDLE ; mode = NORMAL ; /* clear shift register */ strg_sft = 0 ; lcnt = 0 ; ssidx = 0 ; xcnt = 0 ; gate_status = CLOSED ; xtim = 0 ; /* initialize SPI interface */ SPI.begin(); SPI.setBitOrder(MSBFIRST); SPI.setDataMode(SPI_MODE0); SPI.setClockDivider(SPI_CLOCK_DIV2); set_duty(0,0); /* 250ms period */ MsTimer2::set(250,update_trigger); /* enable */ MsTimer2::start(); } void loop() { /* start trigger handling */ if ( strg == ON ) { /* clear flag */ strg = OFF ; /* change system mode */ if ( sysmode == SYSMODE_IDLE ) { sysmode = SYSMODE_MOVE ; /* get parameters */ lduty = get_param( LDUTY_ADR ) ; rduty = get_param( RDUTY_ADR ) ; xtim = get_param( TIM_ADR ) ; /* */ rs_puts("Start !"); crlf(); /* move */ set_duty(lduty,rduty); set_led(ON); /* delay */ secdelay( xtim ); /* */ rs_puts("Stop !"); crlf(); set_duty(0,0); set_led(OFF); /* change state */ sysmode = SYSMODE_IDLE ; } else { sysmode = SYSMODE_IDLE ; set_duty(0,0); } } /* serial handling */ if ( uflag == ON ) { /* clear flag */ uflag = OFF ; /* new line */ crlf(); /* get command */ cmd = *(sbuf+0) ; /* help */ if ( cmd == '?' ) { show_help() ; } /* active mode setting */ if ( cmd == 'A' ) { /* change mode */ if ( *(sbuf+1) == 'N' ) { mode = NORMAL ; } if ( *(sbuf+1) == 'C' ) { mode = CRANK ; } if ( *(sbuf+1) == 'L' ) { mode = LANE ; } /* direction */ xdir = get_hex( *(sbuf+2) ); } /* check motor */ if ( cmd == 'P' ) { /* get and convert */ lduty = 10 * get_hex( *(sbuf+1) ) + get_hex( *(sbuf+2) ) ; rduty = 10 * get_hex( *(sbuf+3) ) + get_hex( *(sbuf+4) ) ; /* store Duty ratio to EEPROM */ put_param( LDUTY_ADR , lduty ); put_param( RDUTY_ADR , rduty ); } /* show motor duty */ if ( cmd == 'p' ) { /* left */ strcopy(msg,"LEFT = "); rs_puts( msg ); /* get parameter form EEPROM */ lduty = get_param( LDUTY_ADR ) ; /* show */ show_value( lduty ) ; /* right */ strcopy(msg,"RIGHT = "); rs_puts( msg ); /* get parameter form EEPROM */ rduty = get_param( RDUTY_ADR ) ; /* show */ show_value( rduty ) ; /* delay time */ strcopy(msg,"TIME = "); rs_puts( msg ); /* get parameter form EEPROM */ xtim = get_param( TIM_ADR ) ; /* show */ show_value( xtim ) ; } /* show start gate state */ if ( cmd == 'g' ) { /* default */ strcopy(msg,"CLOSED"); /* OPENED */ if ( gate_status == OPENED ) { strcopy(msg,"OPENED"); } /* show */ rs_puts( msg ); crlf(); } /* system status */ if ( cmd == 'S' ) { /* default */ strcopy(msg,"IDLE"); /* MOVE */ 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(); } /* delay time */ if ( cmd == 'T' ) { xtim = 0 ; for ( byte i = 0 ; i < 3 ; i++ ) { /* judge */ if ( *(sbuf+i+1) == '\r' ) break ; /* calculate */ xtim = xtim * 10 + get_hex( *(sbuf+i+1) ) ; } /* store */ put_param( TIM_ADR , xtim ); } } } void update_trigger() { /* gate state */ if ( !(PINC & (1 << START_BIT)) ) { gate_status = OPENED ; } /* 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 ; } } void show_help() { rs_puts("? help") ; crlf(); rs_puts("A active mode setting") ; crlf(); rs_puts("P set motor duty ratio") ; crlf(); rs_puts("p show motor duty ratio"); crlf(); rs_puts("g show start gate state"); crlf(); rs_puts("S show system status") ; crlf(); rs_puts("T set delay time") ; crlf(); } /* 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 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) ; } 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 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' ; } byte get_param(byte x) { return( EEPROM.read(x) ); } void put_param(byte xadr,byte x) { EEPROM.write(xadr,x) ; } void show_value(byte x) { char xmsg[4] ; byte xtmp ; /* copy */ xtmp = x ; /* convert */ *(xmsg+0) = xtmp / 100 + '0' ; xtmp %= 100 ; *(xmsg+1) = xtmp / 10 + '0' ; *(xmsg+2) = xtmp % 10 + '0' ; *(xmsg+3) = '\0' ; /* zero suppress */ if ( *(xmsg+0) == '0' ) { *(xmsg+0) = ' ' ; if ( *(xmsg+1) == '0' ) { *(xmsg+1) = ' ' ; } } /* show */ rs_puts( xmsg ); /* new line */ crlf(); } void secdelay(byte x) { while ( x ) { /* 1sec */ delay(1000) ; /* decrement */ x-- ; } }  このスケッチを利用したテストで、モータドライブ回路の  不適切な配線を見つけることができました。

走行動作テスト

 走行モードは、3種に限定しています。  各モードと左右モータのDUTY比を設定し  走行動作が正しいかをテストします。  コマンド'A'で、3モードのいずれかを設定します。  NORMALは'N'、CRANKは'C'、LANEは'L'で指定。  コマンド'P'で、左右のモータのDUTY比を指定。  コマンド'T'で、各モードの動作時間を設定します。  各モードでは、次のように操作します。  NORMAL   コマンド'A'で、NORMALに設定。    AN{enter}   コマンド'P'で、DUTY比を指定。    P3020{enter}   コマンド'T'で、時間を設定。(10から15は16進数指定)    T4{enter}   スタートトリガースイッチを押す。  CRANK   コマンド'A'で、CRANKに設定。    AC{enter}   コマンド'P'で、DUTY比を指定。    P3050{enter}   コマンド'T'で、時間を設定。(10から15は16進数指定)    TB{enter}   スタートトリガースイッチを押す。    センターラインがない、すべて黒のコース面に    出たとして、左か右に旋回します。  LANE   コマンド'A'で、LANEに設定。    AL{enter}   コマンド'P'で、DUTY比を指定。    P5020{enter}   コマンド'T'で、時間を設定。(10から15は16進数指定)    T2{enter}   スタートトリガースイッチを押す。    センターラインがない、すべて黒のコース面に出た    として、車線変更、直進、車線戻しをテストします。  Arduinoのスケッチにまとめると以下。 #include <MsTimer2.h> #include <SPI.h> #include <EEPROM.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 SYSMODE_IDLE 0 #define SYSMODE_MOVE 1 #define NORMAL 0 #define CRANK NORMAL+1 #define LANE NORMAL+2 #define MASK7F 0x7f #define DIR_NONE 0 #define DIR_RIGHT 1 #define DIR_LEFT 2 #define OPENED 0 #define CLOSED 1 #define LEFT_NUM 0 #define RIGHT_NUM 1 #define TIME_NUM 2 #define MODE_NUM 3 #define LDUTY_ADR 0x80 #define RDUTY_ADR LDUTY_ADR+1 #define TIM_ADR LDUTY_ADR+2 #define MODE_ADR LDUTY_ADR+3 /* variables */ byte uflag ; byte strg ; byte strg_sft ; byte sysmode ; byte mode ; byte cmd ; byte sindex ; byte sbuf[16] ; byte lduty ; byte rduty ; byte lcnt ; char msg[16]; byte xdir ; byte ssidx ; byte xcnt ; byte gate_status ; byte timcnt ; /* function prototype */ void update_trigger(); void show_help(); void set_led(byte x); void set_duty(byte lx,byte rx); byte get_hex(byte x); void rs_putchar(char x); void rs_puts(char *ptr); void crlf(); void strcopy(char *dst,char *src); void secdelay(byte x); void show_value(byte xk,byte x); void show_mode(byte x); byte get_param(byte x); void put_param(byte xadr,byte x); void setup() { /* initialize serial */ Serial.begin(9600); sindex = 0 ; uflag = OFF ; /* initialize port values */ PORTB = 0x15 ; PORTC = 0xff ; PORTD = 0x05 ; /* initialize port direction */ DDRB = 0x2e ; DDRC = 0x10 ; DDRD = 0xfa ; /* set system mode */ sysmode = SYSMODE_IDLE ; mode = NORMAL ; put_param( MODE_ADR , mode ); /* clear shift register */ strg_sft = 0 ; lcnt = 0 ; ssidx = 0 ; xcnt = 0 ; timcnt = 0 ; gate_status = CLOSED ; /* initialize SPI interface */ SPI.begin(); SPI.setBitOrder(MSBFIRST); SPI.setDataMode(SPI_MODE0); SPI.setClockDivider(SPI_CLOCK_DIV2); set_duty(0,0); /* 250ms period */ MsTimer2::set(150,update_trigger); /* enable */ MsTimer2::start(); } void loop() { /* start trigger handling */ if ( strg == ON ) { /* clear flag */ strg = OFF ; /* change system mode */ if ( sysmode == SYSMODE_IDLE ) { sysmode = SYSMODE_MOVE ; /* show mode */ mode = get_param( MODE_ADR ) ; show_mode( mode ); /* change LED state */ set_led(ON); /* straight */ rs_puts("straight "); crlf(); set_duty(35,30); secdelay( 3 ) ; /* mode handling */ if ( mode == NORMAL ) { rs_puts("Normal "); crlf(); /* get parameters */ lduty = get_param( LDUTY_ADR ) ; rduty = get_param( RDUTY_ADR ) ; timcnt = get_param( TIM_ADR ) ; /* move */ set_duty(lduty,rduty); secdelay( timcnt ) ; } if ( mode == CRANK ) { rs_puts("Crank Turn "); crlf(); /* get parameters */ lduty = get_param( LDUTY_ADR ) ; rduty = get_param( RDUTY_ADR ) ; timcnt = get_param( TIM_ADR ) ; /* move */ set_duty(lduty,rduty); secdelay( timcnt ) ; } if ( mode == LANE ) { /* get parameters */ lduty = get_param( LDUTY_ADR ) ; rduty = get_param( RDUTY_ADR ) ; timcnt = get_param( TIM_ADR ) ; /* move */ /* 1st turn */ rs_puts("First Turn "); crlf(); set_duty(lduty,rduty); secdelay( timcnt ) ; /* straight */ rs_puts("straight "); crlf(); set_duty(35,30); secdelay(2); /* 2nd turn */ rs_puts("Second Turn "); crlf(); set_duty(rduty,lduty); secdelay( timcnt ) ; } /* straight */ rs_puts("straight "); crlf(); set_duty(35,30); rs_puts("Exit !"); crlf(); } else { sysmode = SYSMODE_IDLE ; set_duty(0,0); } } /* serial handling */ if ( uflag == ON ) { /* clear flag */ uflag = OFF ; /* new line */ crlf(); /* get command */ cmd = *(sbuf+0) ; /* help */ if ( cmd == '?' ) { show_help() ; } /* active mode setting */ if ( cmd == 'A' ) { /* change mode */ if ( *(sbuf+1) == 'N' ) { mode = NORMAL ; } if ( *(sbuf+1) == 'C' ) { mode = CRANK ; } if ( *(sbuf+1) == 'L' ) { mode = LANE ; } /* direction */ xdir = get_hex( *(sbuf+2) ); /* store */ put_param( MODE_ADR , mode ); } /* check motor */ if ( cmd == 'P' ) { lduty = 10 * get_hex( *(sbuf+1) ) + get_hex( *(sbuf+2) ) ; rduty = 10 * get_hex( *(sbuf+3) ) + get_hex( *(sbuf+4) ) ; /* store */ put_param( LDUTY_ADR , lduty ); put_param( RDUTY_ADR , rduty ); } /* show motor duty */ if ( cmd == 'p' ) { /* get parameters */ lduty = get_param( LDUTY_ADR ) ; rduty = get_param( RDUTY_ADR ) ; timcnt = get_param( TIM_ADR ) ; mode = get_param( MODE_ADR ) ; /* left */ show_value(LEFT_NUM, lduty ); /* right */ show_value(RIGHT_NUM,rduty ); /* time */ show_value(TIME_NUM, timcnt ); /* mode */ show_value(MODE_NUM, mode ); } /* show start gate state */ if ( cmd == 'g' ) { /* default */ strcopy(msg,"CLOSED"); /* OPENED */ if ( gate_status == OPENED ) { strcopy(msg,"OPENED"); } /* show */ rs_puts( msg ); crlf(); } /* system status */ if ( cmd == 'S' ) { /* default */ strcopy(msg,"IDLE"); /* MOVE */ 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(); } /* set time counter */ if ( cmd == 'T' ) { timcnt = 0 ; for ( byte i = 0 ; i < 3 ; i++ ) { if ( *(sbuf+i+1) == '\r' ) break ; timcnt = timcnt * 10 + get_hex( *(sbuf+i+1) ); } /* store */ put_param( TIM_ADR , timcnt ); } } } void update_trigger() { /* gate state */ if ( !(PINC & (1 << START_BIT)) ) { gate_status = OPENED ; } /* 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 ; } } void show_help() { rs_puts("? help"); crlf(); rs_puts("A active mode setting"); crlf(); rs_puts("P set motor duty ratio"); crlf(); rs_puts("p show motor duty ratio"); crlf(); rs_puts("g show start gate state"); crlf(); rs_puts("S show system status"); crlf(); rs_puts("T set t seconds"); crlf(); } /* 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 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) ; } 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 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 secdelay(byte x) { byte last ; last = x ; while ( last ) { delay(1000); last--; } } void show_value(byte xk,byte x) { /* default */ strcopy(msg,"LEFT = "); if ( xk == 1 ) { strcopy(msg,"RIGHT = "); } if ( xk == 2 ) { strcopy(msg,"TIME = "); } if ( xk == 3 ) { strcopy(msg,"MODE = "); } /* value */ if ( xk < 3 ) { *(msg+ 8) = x / 10 + '0' ; *(msg+ 9) = x % 10 + '0' ; } else { *(msg+ 8) = ' '; *(msg+ 9) = 'N' ; if ( x == CRANK ) { *(msg+ 9) = 'C' ; } if ( x == LANE ) { *(msg+ 9) = 'L' ; } } *(msg+10) = '\0' ; rs_puts( msg ); crlf(); } void show_mode(byte x) { rs_puts("+++++ "); strcopy(msg,"NORMAL"); if ( x == CRANK ) { strcopy(msg,"CRANK "); } if ( x == LANE ) { strcopy(msg,"LANE "); } rs_puts( msg ); rs_puts(" +++++"); crlf(); } byte get_param(byte x) { return( EEPROM.read(x) ); } void put_param(byte xadr,byte x) { EEPROM.write(xadr,x) ; }  端末で操作すると、次のようになります。

移動距離算定

 タイヤの半径は31mmなので、1回転でどれくらい移動できるかを  Pythonスクリプトで計算しました。 #!/usr/bin/python import math # set initial parameters r = 31 pi = 4 * math.atan(1) for e in range(1,9) : tmp = 2 * r * pi / e print "1 /", e , "turn => %5.1f mm" % tmp  I/Oリダイレクトでテキストファイルに  情報を出力すると以下。 1 / 1 turn => 194.8 mm 1 / 2 turn => 97.4 mm 1 / 3 turn => 64.9 mm 1 / 4 turn => 48.7 mm 1 / 5 turn => 39.0 mm 1 / 6 turn => 32.5 mm 1 / 7 turn => 27.8 mm 1 / 8 turn => 24.3 mm  回転による移動距離がわかれば、進行方向に対する  傾きを算出できます。  CRANKにおける旋回、LANEにおける車線変更には  角度情報があれば、制御しやすくなるので、回転  数と距離の相関表を作成し、利用します。

GBCセンサーテスト

 センサーにGameBoyCameraを利用するとき、Arduinoスケッチで  ハードウエアをテストしました。  信号ピン出力論理値で、配線が間違ってないかと  センシング処理のハード制御に、ミスがないかを  調べておきます。  Arduinoスケッチは、以下。 #define OFF 0 #define ON OFF+1 #define START_BIT 4 #define SIN_BIT 5 #define LOAD_BIT 4 #define XRST_BIT 3 #define XCK_BIT 2 #define READ_BIT 1 char cmd ; byte sindex ; char sbuf[8] ; byte uflag ; byte gbcparam[8] ; word gbcdat[128] ; byte lnum ; void put_start(byte x) { if ( x ) { PORTD |= (1 << START_BIT) ; } else { PORTD &= ~(1 << START_BIT) ; } } void put_sin(byte x) { if ( x ) { PORTC |= (1 << SIN_BIT) ; } else { PORTC &= ~(1 << SIN_BIT) ; } } void put_load(byte x) { if ( x ) { PORTC |= (1 << LOAD_BIT) ; } else { PORTC &= ~(1 << LOAD_BIT) ; } } void put_xrst(byte x) { if ( x ) { PORTC |= (1 << XRST_BIT) ; } else { PORTC &= ~(1 << XRST_BIT) ; } } void put_xck(byte x) { if ( x ) { PORTC |= (1 << XCK_BIT) ; } else { PORTC &= ~(1 << XCK_BIT) ; } } void send_rst() { /* enable RESET */ put_xrst(OFF); /* CLOCK : H */ put_xck(ON); /* CLOCK : L */ put_xck(OFF); /* disable */ put_xrst(ON); } void send_start() { /* enable START */ put_start(ON); /* CLOCK : H */ put_xck(ON); /* CLOCK : L */ put_xck(OFF); /* disable */ put_start(OFF); } byte get_read() { byte result ; /* default */ result = OFF ; /* judge */ if ( PINC & (1 << READ_BIT) ) { result = ON ; } return result ; } void set_gbc_param() { *(gbcparam+0) = 0x80 ; *(gbcparam+1) = 0x00 ; *(gbcparam+2) = 0x00 ; *(gbcparam+3) = 0x80 ; *(gbcparam+4) = 0x00 ; *(gbcparam+5) = 0x01 ; *(gbcparam+6) = 0x00 ; *(gbcparam+7) = 0x01 ; } void send_param(word x) { byte i ; word px ; /* generate code */ px = x ; /* disable LOAD */ put_load(OFF) ; /* transfer */ for ( i = 0 ; i < 11 ; i++ ) { /* enable LOAD */ if ( i == 10 ) { put_load(ON); } /* impress bit data */ put_sin(OFF); if ( px & 0x400 ) { put_sin(ON); } /* CLOCK : H */ put_xck(ON); /* shift */ px <<= 1 ; /* CLOCK : L */ put_xck(OFF); } /* return initial logic state */ put_sin(OFF) ; put_load(OFF) ; } void init_gbc() { byte i ; for ( i = 0 ; i < 8 ; i++ ) { send_param( (i << 8) | *(gbcparam+i) ) ; } } void get_gbc() { } void rs_putchar(char x) { Serial.write( x ) ; } void rs_puts(char *ptr) { while ( *ptr != '\0' ) { rs_putchar( *ptr ) ; ptr++ ; } } void crlf() { rs_putchar('\r'); rs_putchar('\n'); } void show_help() { rs_puts("? help") ; crlf(); rs_puts("B start H") ; crlf(); rs_puts("b start L") ; crlf(); rs_puts("S sin H") ; crlf(); rs_puts("s sin L") ; crlf(); rs_puts("L load H") ; crlf(); rs_puts("l load L") ; crlf(); rs_puts("R xrst H") ; crlf(); rs_puts("r xrst L") ; crlf(); rs_puts("C xck H") ; crlf(); rs_puts("c xck L") ; crlf(); rs_puts("I initialize") ; crlf(); rs_puts("N set line number"); crlf(); rs_puts("G take photo") ; 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 ; } void setup() { /* */ Serial.begin(9600); sindex = 0 ; /* initialize port value */ PORTB = 0x10 ; PORTC = 0x0a ; PORTD = 0x01 ; /* set directions */ DDRB = 0xef ; DDRC = 0xfc ; DDRD = 0xfe ; /* clear flags */ uflag = OFF ; /* initialize */ lnum = 0 ; /* set parameters */ set_gbc_param() ; /* initialize GBC */ init_gbc(); } void loop() { if ( uflag == ON ) { /* clear flag */ uflag = OFF ; /* new line */ crlf(); /* get command */ cmd = *(sbuf+0) ; /* help */ if ( cmd == '?' ) { show_help(); } /* B start H */ if ( cmd == 'B' ) { put_start(ON); } /* b start L */ if ( cmd == 'b' ) { put_start(OFF); } /* S sin H */ if ( cmd == 'S' ) { put_sin(ON); } /* s sin L */ if ( cmd == 's' ) { put_sin(OFF); } /* L load H */ if ( cmd == 'L' ) { put_load(ON); } /* l load L */ if ( cmd == 'l' ) { put_load(OFF); } /* R xrst H */ if ( cmd == 'R' ) { put_xrst(ON); } /* r xrst L */ if ( cmd == 'r' ) { put_xrst(OFF); } /* C xck H */ if ( cmd == 'C' ) { put_xck(ON); } /* c xck L */ if ( cmd == 'c' ) { put_xck(OFF); } /* initialize GBC */ if ( cmd == 'I' ) { init_gbc(); send_rst(); } /* set target line */ if ( cmd == 'N' ) { lnum = 0 ; for ( byte i = 0 ; i < 3 ; i++ ) { if ( *(sbuf+i+1) == '\r' ) break ; lnum = lnum * 10 + get_hex( *(sbuf+i+1) ) ; } /* echo back */ byte tmp ; tmp = lnum ; rs_putchar( tmp / 100 + '0' ) ; tmp %= 100 ; rs_putchar( tmp / 10 + '0' ) ; rs_putchar( tmp % 10 + '0' ) ; crlf(); } /* */ if ( cmd == 'G' ) { send_start() ; get_gbc(); } } } /* 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 ; } } }  コマンドは、次のように決めました。  タイミングチャートに沿った、処理を実現するためには  正しく、信号ピンの論理値を出力しなくてはなりません。  最もハードウエアに近い部分の不具合が、システム全体に  影響するので、制御信号が正しく出力されるかテストする  ことで、上位コードの信頼度も増します。
目次

inserted by FC2 system