目次
前
デバッグ処理
利用しているLattice SemiconductorのFPGA内部回路の
動作をテスト、デバッグするために、Arduinoスケッチ
を利用しました。
一足飛びで、FPGA内部回路の動作テストはできないので
スケッチをいくつか作成し、デバッグに使います。
SENSORデータ出力スケッチ
センサー情報から、制御回路に指示を出すプロセッサは
Arduinoを利用しています。
センサー情報を受取り、状況判断後、制御回路にPWMの
DUTY比を送信します。状況を判断するために必要な
センサー情報を正しく受け取っているか確認します。
次の接続で、頭脳であるArduinoの動作をテストします。
センサー情報を出力するため、次のコマンドを用意しました。
- ? ヘルプ
- A 32のセンサー情報を5秒ごとに出力
- E アドレスを指定して、EEPROMにセンサー情報を格納
- e EEPROMに格納されているセンサー情報をすべて表示
- I EEPROMのセンサー情報をCENTERで初期化
通信条件は、以下。
- 通信速度 9600bps
- データ長 8ビット
- ストップビット 1ビット
- パリティ なし
- フロー制御 なし
コマンド'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 ;
}
}
}
コマンドは、次のように決めました。
- ? ヘルプ
- B 信号ピンstartをHに設定
- b 信号ピンstartをLに設定
- S 信号ピンsinをHに設定
- s 信号ピンsinをLに設定
- L 信号ピンloadをHに設定
- l 信号ピンloadをLに設定
- R 信号ピンxrstをHに設定
- r 信号ピンxrstをLに設定
- C 信号ピンxckをHに設定
- c 信号ピンxckをLに設定
タイミングチャートに沿った、処理を実現するためには
正しく、信号ピンの論理値を出力しなくてはなりません。
最もハードウエアに近い部分の不具合が、システム全体に
影響するので、制御信号が正しく出力されるかテストする
ことで、上位コードの信頼度も増します。
目次
前