机上エミュレーション
状態遷移図からマシンを動かすためのファームウエアを 作成します。テスト、デバッグは、コースが必要ですが コースに出る前に設計した状態遷移をしているのかを 机上で調べておきます。 Arduino基板を、複数枚所有しているので、１枚をセンサー データ出力のエミュレータで使い、MCR_VCマシンに載せる方 と接続します。左においたArduinoがMCR_VCマシンに載る方で、右にある Arduinoがセンサーデータを出力。 右にあるArduinoは、センサーデータを内蔵EEPROMに保存し その内容をPersonal Computerで変更可能にと考えました。 EEPROMは、32バイトを１ページとして、８ページ分確保 できます。次のように、状態ごとのセンサーデータ格納 しておきます。
- Page0 NONE
- Page1 NORMAL
- Page2 CRANK
- Page3 ROTATE
- Page4 CRANK
- Page5 CHANGE
- Page6 BLIND
- Page7 spare
センサーデータを出力するArduinoのスケッチを考えていきます。
コマンドを用意し、機能を割り当てます。
センサーデータ出力
32バイトのセンサーデータを、１秒あるいは２秒ごとに
出力するので、タイマー割込みを利用。
開始と停止を、1文字コマンド'A'、'a'に割り当てます。
コマンドインタプリタは、次のように定義。
/* enable sensor out sequence */
if ( cmd == 'A' ) {
/* enable */
dflag = ON ;
/* clear */
ssindex = 0 ;
/* message */
rs_puts("begin");
/* new line */
crlf();
}
/* disable sensor out sequence */
if ( cmd == 'a' ) {
/* disable */
dflag = OFF ;
/* clear */
ssindex = 0 ;
/* message */
rs_puts("fine");
/* new line */
crlf();
}
フラグdflagを用意し、タイマー割込みでセンサーデータを
出力するか否かを指定。タイマー割込みでは、１秒ないしは
２秒ごとに、通知フラグをセット。
タイマー割込みの本体は、以下。
void update_trigger()
{
/* impress */
if ( xcnt & 1 ) { PORTB |= (1 << LED_BIT); }
else { PORTB &= ~(1 << LED_BIT); }
/* increment */
xcnt++ ;
/* impress sensor data */
if ( dflag == ON ) {
/* increment */
if ( (xcnt & 3) == 3 ) {
timcnt++ ;
/* event flag with interval 1000ms */
if ( timcnt == 4 ) {
timcnt = 0 ;
sflag = ON ;
}
}
}
}
タイマー割込みで、１秒ごとに通知フラグsflagをセット。
通知フラグsflagの受け側では、クリアした後、配列に格納
されているセンサーデータを取り出して出力。
センサーデータ出力制御は、dflagの論理値で判断。
if ( sflag & dflag ) {
/* clear flag */
sflag = OFF ;
/* get sensor data from array */
tmp = *(ssensor+ssindex);
/* show with binary format */
binary_display( tmp ); crlf();
/* impress */
put_sensor( tmp );
/* update index */
ssindex++ ;
/* judge */
if ( ssindex == 0x20 ) {
ssindex = 0 ;
rs_puts("---------");
crlf();
}
}
配列は32バイトとし、これを１ページ分としておきます。
EEPROM操作
状態ごとに１ページを割り当て、センサーデータの端末から
自由に設定できるようにします。
配列とEEPROMのやりとりができるように、２コマンドを用意。
'L' ページ番号を指定して、EEPROMから配列にデータ転送
'S' ページ番号を指定して、配列からEEPROMにデータ転送
コマンドインタプリタは、簡単。
/* load sensor data */
if ( cmd == 'L' ) {
/* get block index */
eadr = get_bnum() ;
/* copy */
for ( byte i = 0 ; i < 32 ; i++ ) {
/* get 1 byte from EEPROM */
edat = EEPROM.read(eadr);
/* store 1 byte to array */
*(ssensor+i) = edat ;
/* address increment */
eadr++ ;
}
}
/* save sensor data */
if ( cmd == 'S' ) {
/* get block index */
eadr = get_bnum() ;
/* copy */
for ( byte i = 0 ; i < 32 ; i++ ) {
/* load 1 byte to array */
edat = *(ssensor+i) ;
/* put 1 byte from EEPROM */
EEPROM.write(eadr,edat);
/* address increment */
eadr++ ;
}
}
配列、EEPROMの内容を参照したいので、専用関数を
定義して、使い勝手をよくします。
void show_context(boolean x)
{
word last ;
/* default */
last = 32 ;
*(msg+2) = ' ';
if ( x == ON ) { last = 256 ; }
/* loop */
for ( word i = 0 ; i < last ; i++ ) {
tmp = *(ssensor+i);
if ( x == ON ) { tmp = EEPROM.read( i ) ; }
/* separate */
*(msg+0) = get_asc( (tmp >> 4) & MASK0F );
*(msg+1) = get_asc( tmp & MASK0F );
/* send */
rs_putchar( *(msg+0) );
rs_putchar( *(msg+1) );
rs_putchar( *(msg+2) );
/* new line */
if ( (i & MASK0F) == 15 ) { crlf(); }
}
}
配列、EEPROMのどちらの内容を表示するかを
フラグで指定できるようにしました。
対応するコマンドは、'D'、'E'に。
'D' 配列の内容表示
'E' EEPROMの内容表示
配列内容操作
配列は32バイトのセンサーデータを保存するので
１バイトごとの設定と32バイトに同じ値の格納を
できるようにします。
１バイトごとの設定には、コマンド'e'を使い
32バイトに同一値保存は、コマンド'f'を利用
とします。
コマンドインタプリタは、次のように定義。
/* edit sensor data */
if ( cmd == 'e' ) {
/* get address */
eadr = get_hex( *(sbuf+1) );
eadr *= 10 ;
eadr += get_hex( *(sbuf+2) );
/* get data */
edat = get_value( sbuf+3 );
/* judge */
if ( eadr > 31 ) {
rs_puts("Wrong index!");
} else {
*(ssensor+eadr) = edat ;
}
}
/* fill sensor data */
if ( cmd == 'f' ) {
/* get data */
edat = get_value( sbuf+3 );
/* store */
for ( byte i = 0 ; i < 32 ; i++ ) { *(ssensor+i) = edat ; }
}
まとめると、以下。
#include <EEPROM.h>
#include <MsTimer2.h>
#define OFF 0
#define ON OFF+1
#define MASK0F 0x0f
#define MASKF0 0xf0
#define MASKFF 0xff
#define MASK7F 0x7f
#define LED_BIT 5
/* variables */
boolean uflag ;
boolean oflag ;
boolean dflag ;
boolean sflag ;
byte cmd ;
byte sindex ;
byte sbuf[8] ;
byte xcnt ;
char msg[4] ;
byte tmp ;
byte timcnt ;
byte ssindex ;
byte ssensor[32] ;
byte eadr ;
byte edat ;
/* function prototype */
void update_trigger();
void show_help();
byte get_hex(byte x);
char get_asc(byte x);
void rs_putchar(char x);
void rs_puts(char *ptr);
void crlf();
void binary_display(byte x);
void put_sensor(byte x);
byte get_bnum();
byte get_value(byte *x);
void show_context(boolean x);
void setup()
{
/* initialize serial */
Serial.begin(9600);
sindex = 0 ;
/* clear flags */
uflag = OFF ;
dflag = OFF ;
oflag = ON ;
/* initialize port values */
PORTB = 0x00 ;
PORTC = 0x0f ; /* pull up */
PORTD = 0x001 ;
/* initialize port direction */
DDRB = 0xff ;
DDRC = 0xf0 ; /* lower nibble inputs */
DDRD = 0xfe ;
/* clear */
xcnt = 0 ;
timcnt = 0 ;
/* 250ms period */
MsTimer2::set(250,update_trigger);
/* enable */
MsTimer2::start();
}
void loop()
{
/* opening message */
if ( oflag == ON ) {
/* new line */
crlf();
/* clear flag */
oflag = OFF ;
/* message */
rs_puts("Hello , Darling !");
/* new line */
crlf();
}
/* impress sensor data */
if ( sflag & dflag ) {
/* clear flag */
sflag = OFF ;
/* get sensor data from array */
tmp = *(ssensor+ssindex);
/* show with binary format */
binary_display( tmp ); crlf();
/* impress */
put_sensor( tmp );
/* update index */
ssindex++ ;
/* judge */
if ( ssindex == 0x20 ) {
ssindex = 0 ;
rs_puts("---------");
crlf();
}
}
/* serial handling */
if ( uflag == ON ) {
/* clear flag */
uflag = OFF ;
/* new line */
crlf();
/* get command */
cmd = *(sbuf+0) ;
/* help */
if ( cmd == '?' ) { show_help() ; }
/* enable sensor out sequence */
if ( cmd == 'A' ) {
/* enable */
dflag = ON ;
/* clear */
ssindex = 0 ;
/* message */
rs_puts("begin");
/* new line */
crlf();
}
/* disable sensor out sequence */
if ( cmd == 'a' ) {
/* disable */
dflag = OFF ;
/* clear */
ssindex = 0 ;
/* message */
rs_puts("fine");
/* new line */
crlf();
}
/* load sensor data */
if ( cmd == 'L' ) {
/* get block index */
eadr = get_bnum() ;
/* copy */
for ( byte i = 0 ; i < 32 ; i++ ) {
/* get 1 byte from EEPROM */
edat = EEPROM.read(eadr);
/* store 1 byte to array */
*(ssensor+i) = edat ;
/* address increment */
eadr++ ;
}
}
/* save sensor data */
if ( cmd == 'S' ) {
/* get block index */
eadr = get_bnum() ;
/* copy */
for ( byte i = 0 ; i < 32 ; i++ ) {
/* load 1 byte to array */
edat = *(ssensor+i) ;
/* put 1 byte from EEPROM */
EEPROM.write(eadr,edat);
/* address increment */
eadr++ ;
}
}
/* show sensor array context */
if ( cmd == 'D' ) { show_context(OFF); }
/* edit sensor data */
if ( cmd == 'e' ) {
/* get address */
eadr = get_hex( *(sbuf+1) );
eadr *= 10 ;
eadr += get_hex( *(sbuf+2) );
/* get data */
edat = get_value( sbuf+3 );
/* judge */
if ( eadr > 31 ) {
rs_puts("Wrong index!");
} else {
*(ssensor+eadr) = edat ;
}
}
/* fill sensor data */
if ( cmd == 'f' ) {
/* get data */
edat = get_value( sbuf+3 );
/* store */
for ( byte i = 0 ; i < 32 ; i++ ) { *(ssensor+i) = edat ; }
}
/* show EEPROM context */
if ( cmd == 'E' ) { show_context(ON); }
}
}
void update_trigger()
{
/* impress */
if ( xcnt & 1 ) { PORTB |= (1 << LED_BIT); }
else { PORTB &= ~(1 << LED_BIT); }
/* increment */
xcnt++ ;
/* impress sensor data */
if ( dflag == ON ) {
/* increment */
if ( (xcnt & 3) == 3 ) {
timcnt++ ;
/* event flag with interval 1000ms */
if ( timcnt == 4 ) {
timcnt = 0 ;
sflag = ON ;
}
}
}
}
void show_help()
{
rs_puts("? help"); crlf();
rs_puts("A enable impressing"); crlf();
rs_puts("a disable impressing"); crlf();
rs_puts("L load sensor data"); crlf();
rs_puts("S save sensor data"); crlf();
rs_puts("D show sensor data"); crlf();
rs_puts("e edit sensor data"); crlf();
rs_puts("f fill sensor data"); crlf();
rs_puts("E show EEPROM context");crlf();
}
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');
}
char get_asc(byte x)
{
char result ;
/* default */
result = '0' ;
/* convert */
if ( x < 10 ) { result = x + '0' ; }
if ( 9 < x && x < 16 ) { result = x - 10 + 'A' ; }
return result ;
}
void binary_display(byte x)
{
int i ;
for ( i = 7 ; i > -1 ; i-- ) {
rs_putchar('0'+((x >> i) & 1));
}
}
void put_sensor(byte x)
{
byte dh ;
byte dl ;
/* separate */
dh = x & MASKF0 ;
dl = x & MASK0F ;
/* impress upper nibble */
PORTD &= MASK0F ;
PORTD |= dh ;
/* impress lower nibble */
PORTB &= MASKF0 ;
PORTB |= dl ;
}
byte get_bnum()
{
byte result ;
/* get */
result = get_hex( *(sbuf+1) );
/* calculate */
result <<= 5 ;
return result ;
}
byte get_value(byte *x)
{
byte result ;
/* first */
result = get_hex( *x );
result <<= 4 ;
/* first */
result += get_hex( *(x+1) );
return result ;
}
void show_context(boolean x)
{
word last ;
/* default */
last = 32 ;
*(msg+2) = ' ';
if ( x == ON ) { last = 256 ; }
/* loop */
for ( word i = 0 ; i < last ; i++ ) {
tmp = *(ssensor+i);
if ( x == ON ) { tmp = EEPROM.read( i ) ; }
/* separate */
*(msg+0) = get_asc( (tmp >> 4) & MASK0F );
*(msg+1) = get_asc( tmp & MASK0F );
/* send */
rs_putchar( *(msg+0) );
rs_putchar( *(msg+1) );
rs_putchar( *(msg+2) );
/* new line */
if ( (i & MASK0F) == 15 ) { 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 ;
}
}
}
端末での操作は、次のようになります。
起動し、配列の内容を表示後、EEPROMからロードして表示。
センサーデータの出力開始。
センサーデータの出力停止。
EEPROMの内容表示。
センサーデータの８ビットは、ポートＢ、Ｄの上位、下位に
１ニブルと分割して出力。
PORTB
PB3 sensor_3
PB2 sensor_2
PB1 sensor_1
PB0 sensor_0
PORTD
PD7 sensor_7
PD6 sensor_6
PD5 sensor_5
PD4 sensor_4
センサーデータは、Pythonスクリプトで自動生成して
編集できるようにしました。
Pythonスクリプトは、以下。
#!/usr/bin/python
import random
def getHex(x):
if x == '0001' :
result = 1
elif x == '0010' :
result = 2
elif x == '0011' :
result = 3
elif x == '0100' :
result = 4
elif x == '0101' :
result = 5
elif x == '0110' :
result = 6
elif x == '0111' :
result = 7
elif x == '1000' :
result = 8
elif x == '1001' :
result = 9
elif x == '1010' :
result = 10
elif x == '1011' :
result = 11
elif x == '1100' :
result = 12
elif x == '1101' :
result = 13
elif x == '1110' :
result = 14
elif x == '1111' :
result = 15
else:
result = 0
#
return result
def genHex(x):
# upper
dh = getHex('0'+x[0:3])
# lower
dl = getHex(x[3:7])
# calculate
result = (dh << 4) | dl
#
return result
mydic = []
mydic.append('ALL_BLACK')
mydic.append('ALL_WHITE')
mydic.append('LEFT_WHITE')
mydic.append('RIGHT_WHITE')
mydic.append('CENTER')
mydic.append('TINY_RIGHT')
mydic.append('RIGHT')
mydic.append('BIG_RIGHT')
mydic.append('TINY_LEFT')
mydic.append('LEFT')
mydic.append('BIG_LEFT')
mydic.append('BOTH_WHITE')
mydic.append('ILLEAGAL')
sdic = {'ALL_BLACK':'0000000'}
sdic.update({'ALL_WHITE':'1111111'})
sdic.update({'LEFT_WHITE':'0000111'})
sdic.update({'RIGHT_WHITE':'1110000'})
sdic.update({'CENTER':'0001000'})
sdic.update({'TINY_RIGHT':'0000100'})
sdic.update({'RIGHT':'0000010'})
sdic.update({'BIG_RIGHT':'0000001'})
sdic.update({'TINY_LEFT':'0010000'})
sdic.update({'LEFT':'0100000'})
sdic.update({'BIG_LEFT':'1000000'})
sdic.update({'BOTH_WHITE':'1000001'})
sdic.update({'ILLEAGAL':'1010101'})
for e in range(0,32):
# generate random number
rr = int( 11 * random.random() )+1
sensor = mydic[rr]
print sdic[sensor],'%3d' % genHex(sdic[sensor]),sensor
状態NORMALに与えるセンサーデータを生成すると
次のようになりました。
0010000 32 TINY_LEFT
0010000 32 TINY_LEFT
1111111 239 ALL_WHITE
0100000 64 LEFT
0100000 64 LEFT
1111111 239 ALL_WHITE
0000100 4 TINY_RIGHT
0000001 1 BIG_RIGHT
0010000 32 TINY_LEFT
0000001 1 BIG_RIGHT
0001000 8 CENTER
1111111 239 ALL_WHITE
0000010 2 RIGHT
0010000 32 TINY_LEFT
1110000 224 RIGHT_WHITE
1111111 239 ALL_WHITE
0000100 4 TINY_RIGHT
1110000 224 RIGHT_WHITE
0001000 8 CENTER
0100000 64 LEFT
0100000 64 LEFT
0100000 64 LEFT
0000111 7 LEFT_WHITE
0000111 7 LEFT_WHITE
0000001 1 BIG_RIGHT
0000010 2 RIGHT
0010000 32 TINY_LEFT
1000001 129 BOTH_WHITE
0000001 1 BIG_RIGHT
0100000 64 LEFT
0100000 64 LEFT
1111111 239 ALL_WHITE
乱数を利用してセンサーデータを生成したので、実際のコースでは
絶対に発生しないパターンもありますが、それを検出しても誤認を
しないで動くことが要求されるので、これはこれで使えるとの判断
です。
机上エミュレーションで使ったArduino基板は、以下。
Arduino基板には、USB/シリアル変換器を使わず、74HC04を
利用したシリアルインタフェース回路を接続しました。
Arduino基板の右側にみえる、６ピン（２ｘ３）のヘッダに
接続するワイヤーケーブルを利用します。
回路図は、以下。
MCR_VCマシンを動かすArduinoスケッチには、次のシーケンサを
入れて動作確認。
#define MD_NONE 0
#define MD_NORMAL 1
#define MD_CRANK 2
#define MD_ROTATE 3
#define MD_LANE 4
#define MD_CHANGE 5
#define MD_BLIND 6
state = MD_NONE ;
while ( ON ) {
switch ( state ) {
/* NONE */
case 0 : /* get start trigger */
sensor = get_start_trigger() ;
state = MD_NONE+1 ;
break;
case 1 : /* judge */
state = MD_NONE+1 ;
if ( sensor == OPENED ) {
xl = 10 ;
xr = 10 ;
dir = DIR_CENTER ;
set_duty(xl,xr);
state = MD_NONE+2 ;
}
dir = DIR_CENTER ;
*(dir+1) = DIR_CENTER ;
break ;
case 2 : /* delay */
state = MD_NONE ;
delay_ms(100);
break ;
case 3 : /* accelate */
xl += 25 ; /* xl = 35 */
xr += 25 ; /* xr = 35 */
set_duty(xl,xr);
state = MD_NONE+4 ;
break;
case 4 : /* delay */
state = MD_NONE+5 ;
delay_ms(100);
break ;
case 5 : /* accelate */
xl += 15 ; /* xl = 50 */
xr += 15 ; /* xr = 50 */
set_duty(xl,xr);
state = MD_NONE+6 ;
break;
case 6 : /* delay */
state = MD_NORMAL ;
delay_ms(100);
break ;
/* NORMAL */
case 10 : /* get sensor information */
sensor = get_sensor();
state = MD_NORMAL+1 ;
break;
case 11 : /* set parameters */
if ( sensor == ALL_BLACK ) {
xl = 10 ;
xr = 10 ;
dtime = 10 ;
}
if ( sensor == ALL_WHITE ) {
xl = 30 ;
xr = 30 ;
dtime = 30 ;
}
if ( sensor == LEFT_WHITE ) {
xl = 50 ;
xr = 50 ;
dtime = 30 ;
}
if ( sensor == RIGHT_WHITE ) {
xl = 50 ;
xr = 50 ;
dtime = 30 ;
}
if ( sensor == CENTER ) {
xl = 50 ;
xr = 50 ;
dtime = 50 ;
}
if ( sensor == TINY_RIGHT ) {
xl = 50 ;
xr = 55 ;
dtime = 25 ;
}
if ( sensor == RIGHT ) {
xl = 50 ;
xr = 60 ;
dtime = 25 ;
}
if ( sensor == BIG_RIGHT ) {
xl = 50 ;
xr = 70 ;
dtime = 25 ;
}
if ( sensor == TINY_LEFT ) {
xl = 55 ;
xr = 50 ;
dtime = 25 ;
}
if ( sensor == LEFT ) {
xl = 60 ;
xr = 50 ;
dtime = 25 ;
}
if ( sensor == BIG_LEFT ) {
xl = 70 ;
xr = 50 ;
dtime = 25 ;
}
if ( sensor == BOTH_WHITE ) {
xl = 30 ;
xr = 30 ;
dtime = 10 ;
}
set_duty(xl,xr);
state = MD_NORMAL+2 ;
break;
case 12 : /* delay */
delay_ms(dtime);
state = MD_NORMAL+3 ;
break;
case 13 : /* judge */
state = MD_NORMAL ;
if ( sensor == ALL_WHITE ) { state = MD_CRANK ; }
if ( sensor == LEFT_WHITE ) {
state = MD_LANE ;
*(dir+0) = DIR_LEFT ;
}
if ( sensor == RIGHT_WHITE ) {
state = MD_LANE ;
*(dir+0) = DIR_RIGHT ;
}
break;
/* CRANK */
case 20 : /* get sensor information */
sensor = get_sensor();
state = MD_CRANK+1 ;
break;
case 21 : /* set parameters */
if ( sensor == ALL_BLACK ) {
xl = 10 ;
xr = 10 ;
dtime = 10 ;
}
if ( sensor == ALL_WHITE ) {
xl = 30 ;
xr = 30 ;
dtime = 10 ;
}
if ( sensor == LEFT_WHITE ) {
xl = 30 ;
xr = 30 ;
dtime = 20 ;
}
if ( sensor == RIGHT_WHITE ) {
xl = 30 ;
xr = 30 ;
dtime = 20 ;
}
if ( sensor == CENTER ) {
xl = 40 ;
xr = 40 ;
dtime = 30 ;
}
if ( sensor == TINY_RIGHT ) {
xl = 40 ;
xr = 45 ;
dtime = 25 ;
}
if ( sensor == RIGHT ) {
xl = 40 ;
xr = 50 ;
dtime = 25 ;
}
if ( sensor == BIG_RIGHT ) {
xl = 40 ;
xr = 55 ;
dtime = 25 ;
}
if ( sensor == TINY_LEFT ) {
xl = 45 ;
xr = 40 ;
dtime = 25 ;
}
if ( sensor == LEFT ) {
xl = 50 ;
xr = 40 ;
dtime = 25 ;
}
if ( sensor == BIG_LEFT ) {
xl = 55 ;
xr = 40 ;
dtime = 25 ;
}
if ( sensor == BOTH_WHITE ) {
xl = 30 ;
xr = 30 ;
dtime = 20 ;
}
set_duty(xl,xr);
state = MD_CRANK+2 ;
break;
case 22 : /* delay */
delay_ms(dtime);
state = MD_CRANK+3 ;
break;
case 23 : /* judge */
state = MD_CRANK ;
if ( sensor == LEFT_WHITE ) {
state = MD_ROTATE ;
*(dir+0) = DIR_LEFT ;
}
if ( sensor == RIGHT_WHITE ) {
state = MD_ROTATE ;
*(dir+0) = DIR_RIGHT ;
}
break;
/* ROTATE */
case 30 : /* turn */
dtime = 30 ;
if ( dir == DIR_LEFT ) {
xl = 30 ;
xr = 50 ;
}
if ( dir == DIR_RIGHT ) {
xl = 50 ;
xr = 30 ;
}
set_duty(xl,xr);
state = MD_ROTATE+1 ;
break;
case 31 : /* delay */
delay_ms(dtime);
state = MD_ROTATE+2 ;
break;
case 32 : /* get sensor information */
sensor = get_sensor();
state = MD_ROTATE+3 ;
break;
case 33 : /* judge */
state = MD_ROTATE+1 ;
if ( sensor == CENTER ) {
state = MD_NORMAL ;
*(dir+0) = DIR_CENTER ;
xl = 30 ;
xr = 30 ;
dtime = 30 ;
set_duty(xl,xr);
delay_ms(dtime);
}
break;
/* LANE */
case 40 : /* get sensor information */
sensor = get_sensor();
state = MD_LANE+1 ;
break;
case 41 : /* set parameters */
if ( sensor == ALL_BLACK ) {
xl = 10 ;
xr = 10 ;
dtime = 10 ;
}
if ( sensor == ALL_WHITE ) {
xl = 30 ;
xr = 30 ;
dtime = 10 ;
}
if ( sensor == LEFT_WHITE ) {
xl = 30 ;
xr = 30 ;
dtime = 30 ;
}
if ( sensor == RIGHT_WHITE ) {
xl = 30 ;
xr = 30 ;
dtime = 30 ;
}
if ( sensor == CENTER ) {
xl = 40 ;
xr = 40 ;
dtime = 30 ;
}
if ( sensor == TINY_RIGHT ) {
xl = 40 ;
xr = 45 ;
dtime = 25 ;
}
if ( sensor == RIGHT ) {
xl = 40 ;
xr = 50 ;
dtime = 25 ;
}
if ( sensor == BIG_RIGHT ) {
xl = 40 ;
xr = 55 ;
dtime = 25 ;
}
if ( sensor == TINY_LEFT ) {
xl = 45 ;
xr = 40 ;
dtime = 25 ;
}
if ( sensor == LEFT ) {
xl = 50 ;
xr = 40 ;
dtime = 25 ;
}
if ( sensor == BIG_LEFT ) {
xl = 55 ;
xr = 40 ;
dtime = 25 ;
}
set_duty(xl,xr);
state = MD_LANE+2 ;
break;
case 42 : /* delay */
delay_ms();
state = MD_LANE+3 ;
break;
case 43 : /* judge */
state = MD_LANE ;
if ( sensor == ALL_BLACK ) {
state = MD_CHANGE ;
*(dir+0) = DIR_CENTER ;
}
break;
/* CHANGE */
case 50 : /* turn */
dtime = 50 ;
if ( *(dir+0) == DIR_LEFT ) {
xl = 35 ;
xr = 55 ;
}
if ( *(dir+0) == DIR_RIGHT ) {
xl = 55 ;
xr = 35 ;
}
set_duty(xl,xr);
state = MD_CHANGE+1 ;
break;
case 51 : /* delay */
delay_ms(dtime);
state = MD_CHANGE+2 ;
break;
case 52 : /* straight */
xl = 30 ;
xr = 30 ;
dtime = 50 ;
set_duty(xl,xr);
state = MD_CHANGE+3 ;
break;
case 53 : /* delay */
delay_ms(dtime);
state = MD_CHANGE+4 ;
break;
case 54 : /* set opposite direction */
if ( *(dir+0) == DIR_LEFT ) {
*(dir+1) = DIR_RIGHT ;
}
if ( *(dir+0) == DIR_RIGHT ) {
*(dir+1) = DIR_LEFT ;
}
state = MD_BLIND ;
break;
/* BLIND */
case 60 : /* get sensor information */
sensor = get_sensor();
state = MD_BLIND+1 ;
break;
case 61 : /* judge */
state = MD_BLIND+2 ;
if ( sensor == CENTER ) { state = MD_BLIND+3 ; }
break;
case 62 : /* delay */
delay_ms(dtime);
state = MD_BLIND ;
break;
case 63 : /* turn */
dtime = 30 ;
if ( *(dir+1) == DIR_LEFT ) {
xl = 25 ;
xr = 35 ;
}
if ( *(dir+1) == DIR_RIGHT ) {
xl = 35 ;
xr = 25 ;
}
set_duty(xl,xr);
state = MD_BLIND+4 ;
break;
case 64 : /* delay */
delay_ms(dtime);
state = MD_BLIND+5 ;
break;
case 65 : /* straight */
xl = 30 ;
xr = 30 ;
dtime = 50 ;
set_duty(xl,xr);
state = MD_BLIND+6 ;
break;
case 66 : /* delay */
delay_ms(dtime);
state = MD_BLIND+7 ;
break;
/* return NORMAL */
case 67 :
dir = DIR_CENTER ;
*(dir+1) = DIR_CENTER ;
state = MD_NORMAL ;
break;
default :
break;
}
}
RTOSを利用できる場合には、より現実に近いカタチで
机上デバッグスケッチを作成できます。
#include <MsTimer2.h>
#define OFF 0
#define ON OFF+1
#define MD_IDLE 0
#define MD_RUN MD_IDLE+1
#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 TSK_ID_MAX 9
#define TSK_ID0 0
#define TSK_ID1 1
#define TSK_ID2 2
#define TSK_ID3 3
#define TSK_ID4 4
#define TSK_ID5 5
#define TSK_ID6 6
#define TSK_ID7 7
#define TSK_ID8 8
#define XNORMAL 1
#define XCRANK 2
#define XROTATE 3
#define XLANE 4
#define XCHANGE 5
#define XBLIND 6
#define DIR_CENTER 0
#define DIR_RIGHT 1
#define DIR_LEFT 2
#define TTS_SUSPEND 0
#define TTS_WAIT TTS_SUSPEND+1
#define TTS_READY TTS_SUSPEND+2
typedef struct {
void (*tsk)(void);
word wcount ;
} TCBP ;
TCBP tcbx[TSK_ID_MAX] ;
TCBP pcur_tsk ;
long timcnt ;
char sbuf[8] ;
char sindex ;
char cmd ;
byte UFLAG ;
byte EFLAG ;
word xready ;
word xsuspend ;
word xwaitq ;
byte run_tsk ;
byte WFLAG ;
byte dcnt ;
byte state ;
byte xdir[2] ;
byte xcnt1 ;
byte xcnt2 ;
byte xcnt3 ;
byte xcnt4 ;
byte xcnt5 ;
byte xcnt6 ;
byte road[96];
byte rdx ;
byte ldx ;
void serialEvent();
void rs_putchar(char x)
{
Serial.write(x);
}
void rs_puts(char *x)
{
while ( *x != '\0' ) {
rs_putchar( *x ) ;
x++ ;
}
}
void crlf()
{
rs_putchar('\r');
rs_putchar('\n');
}
void show_help()
{
rs_puts("? help"); crlf();
rs_puts("E enable"); crlf();
rs_puts("e disable"); crlf();
}
void show_info(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 TINY_RIGHT : rs_puts("TINY_RIGHT") ; break ;
case RIGHT : rs_puts("RIGHT") ; break ;
case BIG_RIGHT : rs_puts("BIG_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 init_road()
{
byte i ;
/* clear */
for ( i = 0 ; i < 96 ; i++ ) {
*(road+i) = CENTER ;
}
/* NORMAL */
*(road+ 0) = CENTER ; *(road+ 1) = TINY_RIGHT ;
*(road+ 2) = CENTER ; *(road+ 3) = TINY_LEFT ;
*(road+ 4) = ALL_WHITE ; *(road+ 5) = CENTER ;
*(road+ 6) = CENTER ; *(road+ 7) = LEFT_WHITE ;
*(road+ 8) = CENTER ; *(road+ 9) = CENTER ;
*(road+10) = RIGHT_WHITE ; *(road+11) = CENTER ;
*(road+12) = TINY_LEFT ; *(road+13) = CENTER ;
*(road+14) = TINY_RIGHT ; *(road+15) = CENTER ;
/* CRANK */
*(road+ 0+16) = CENTER ; *(road+ 1+16) = LEFT_WHITE ;
*(road+ 2+16) = TINY_LEFT ; *(road+ 3+16) = TINY_RIGHT ;
*(road+ 4+16) = CENTER ; *(road+ 5+16) = ALL_BLACK ;
*(road+ 6+16) = CENTER ; *(road+ 7+16) = ILLEAGAL ;
*(road+ 8+16) = CENTER ; *(road+ 9+16) = RIGHT_WHITE ;
*(road+10+16) = TINY_LEFT ; *(road+11+16) = TINY_RIGHT ;
*(road+12+16) = CENTER ; *(road+13+16) = ALL_BLACK ;
*(road+14+16) = CENTER ; *(road+15+16) = ILLEAGAL ;
/* ROTATE */
*(road+ 0+32) = ALL_BLACK ; *(road+ 1+32) = ALL_BLACK ;
*(road+ 2+32) = BIG_LEFT ; *(road+ 3+32) = BIG_LEFT ;
*(road+ 4+32) = LEFT ; *(road+ 5+32) = TINY_LEFT ;
*(road+ 6+32) = CENTER ; *(road+ 7+32) = TINY_RIGHT;
*(road+ 8+32) = ILLEAGAL ; *(road+ 9+32) = ILLEAGAL ;
*(road+10+32) = ILLEAGAL ; *(road+11+32) = ILLEAGAL ;
*(road+12+32) = ILLEAGAL ; *(road+13+32) = ILLEAGAL ;
*(road+14+32) = ILLEAGAL ; *(road+15+32) = ILLEAGAL ;
/* LANE */
*(road+ 0+48) = CENTER ; *(road+ 1+48) = TINY_LEFT ;
*(road+ 2+48) = CENTER ; *(road+ 3+48) = TINY_RIGHT ;
*(road+ 4+48) = CENTER ; *(road+ 5+48) = ALL_BLACK ;
*(road+ 6+48) = ILLEAGAL ; *(road+ 7+48) = ILLEAGAL ;
*(road+ 8+48) = ILLEAGAL ; *(road+ 9+48) = ILLEAGAL ;
*(road+10+48) = ILLEAGAL ; *(road+11+48) = ILLEAGAL ;
*(road+12+48) = ILLEAGAL ; *(road+13+48) = ILLEAGAL ;
*(road+14+48) = ILLEAGAL ; *(road+15+48) = ILLEAGAL ;
/* CHANGE */
*(road+ 0+64) = ALL_BLACK ; *(road+ 1+64) = ALL_BLACK ;
*(road+ 2+64) = ALL_BLACK ; *(road+ 3+64) = ALL_BLACK ;
*(road+ 4+64) = BIG_LEFT ; *(road+ 5+64) = LEFT ;
*(road+ 6+64) = TINY_LEFT ; *(road+ 7+64) = CENTER ;
/* BLIND */
*(road+ 0+80) = ALL_BLACK ; *(road+ 1+80) = ALL_BLACK ;
*(road+ 2+80) = ALL_BLACK ; *(road+ 3+80) = ALL_BLACK ;
*(road+ 4+80) = BIG_LEFT ; *(road+ 5+80) = LEFT ;
*(road+ 6+80) = TINY_LEFT ; *(road+ 7+80) = CENTER ;
}
void cre_tsk(byte tid,void (*tsk)(void))
{
tcbx[tid].tsk = tsk;
tcbx[tid].wcount = 0;
}
void sta_tsk(byte tid,byte sta)
{
word tmp ;
tmp = (1 << tid);
if ( sta == TTS_READY ) { xready |= tmp; }
if ( sta == TTS_SUSPEND ) { xsuspend |= tmp; }
if ( sta == TTS_WAIT ) { xwaitq |= tmp; }
}
void rsm_tsk(byte tid)
{
word tmp ;
tmp = (1 << tid);
xready |= tmp;
xsuspend &= ~tmp;
xwaitq &= ~tmp;
}
void sus_tsk(byte tid)
{
word tmp ;
tmp = (1 << tid);
xready &= ~tmp;
xsuspend |= tmp;
xwaitq &= ~tmp;
}
void slp_tsk(void)
{
sus_tsk(run_tsk);
}
void wai_tsk(word x)
{
word tmp ;
tmp = (1 << run_tsk);
xready &= ~tmp;
xsuspend &= ~tmp;
xwaitq |= tmp;
tcbx[run_tsk].wcount = x ;
}
byte is_tsk_ready(byte tid)
{
return( (xready >> tid) & ON ) ;
}
void init_os()
{
/* clear flag */
WFLAG = OFF ;
/* create */
cre_tsk(TSK_ID0,tsk0_proc);
cre_tsk(TSK_ID1,tsk1_proc);
cre_tsk(TSK_ID2,tsk2_proc);
cre_tsk(TSK_ID3,tsk3_proc);
cre_tsk(TSK_ID4,tsk4_proc);
cre_tsk(TSK_ID5,tsk5_proc);
cre_tsk(TSK_ID6,tsk6_proc);
cre_tsk(TSK_ID7,tsk7_proc);
cre_tsk(TSK_ID8,tsk8_proc);
/* initialize state */
sta_tsk(TSK_ID0,TTS_READY);
sta_tsk(TSK_ID1,TTS_SUSPEND);
sta_tsk(TSK_ID2,TTS_SUSPEND);
sta_tsk(TSK_ID3,TTS_SUSPEND);
sta_tsk(TSK_ID4,TTS_SUSPEND);
sta_tsk(TSK_ID5,TTS_SUSPEND);
sta_tsk(TSK_ID6,TTS_SUSPEND);
sta_tsk(TSK_ID7,TTS_READY);
sta_tsk(TSK_ID8,TTS_READY);
/* clear counter */
xcnt1 = 0 ;
}
void timer_handler()
{
byte i ;
word xtmp ;
for ( i = 0 ; i < TSK_ID_MAX ; i++ ) {
/* judge */
if ( xwaitq & (1 << i) ) {
xtmp = tcbx[i].wcount ;
xtmp-- ;
tcbx[i].wcount = xtmp ;
if ( xtmp == 0 ) { rsm_tsk(i); }
}
}
}
byte get_sensor(byte xid,byte xc)
{
byte result ;
byte xptr ;
/* default */
result = 0 ;
/* check ID */
if ( xid > TSK_ID6 ) { result = 0 ; }
else {
/* calculate address */
xptr = ((xid-1) << 4) | xc ;
/* get data */
result = *(road+xptr);
}
return result ;
}
void tsk0_proc()
{
/* judge */
if ( EFLAG == ON ) {
/* IDLE -> RUN */
if ( state == MD_IDLE ) {
xcnt1 = 0 ;
state = MD_RUN ;
rsm_tsk( XNORMAL ) ;
rs_puts("-> NORMAL");
crlf();
}
}
if ( EFLAG == OFF ) {
/* RUN -> IDLE */
if ( state == MD_RUN ) {
state = MD_IDLE ;
rs_puts(" suspend all task");
crlf();
sus_tsk( TSK_ID1 ) ;
sus_tsk( TSK_ID2 ) ;
sus_tsk( TSK_ID3 ) ;
sus_tsk( TSK_ID4 ) ;
sus_tsk( TSK_ID5 ) ;
sus_tsk( TSK_ID6 ) ;
}
}
}
void tsk1_proc()
{
byte xx ;
byte eflag ;
/* */
eflag = OFF ;
show_hex( xcnt1 );
/* get code */
xx = get_sensor(XNORMAL,xcnt1);
rs_putchar('\t');
show_info( xx );
rs_putchar('\t');
/* set duty ratio */
switch ( xx ) {
case ALL_BLACK : ldx = 0 ; rdx = 0 ; break ;
case ALL_WHITE : ldx = 30 ; rdx = 30 ; break ;
case LEFT_WHITE : ldx = 10 ; rdx = 10 ; break ;
case RIGHT_WHITE : ldx = 10 ; rdx = 10 ; break ;
case CENTER : ldx = 50 ; rdx = 50 ; break ;
case TINY_RIGHT : ldx = 50 ; rdx = 55 ; break ;
case RIGHT : ldx = 50 ; rdx = 60 ; break ;
case BIG_RIGHT : ldx = 50 ; rdx = 65 ; break ;
case TINY_LEFT : ldx = 55 ; rdx = 50 ; break ;
case LEFT : ldx = 60 ; rdx = 50 ; break ;
case BIG_LEFT : ldx = 65 ; rdx = 50 ; break ;
case BOTH_WHITE : ldx = 10 ; rdx = 10 ; break ;
default : ldx = 10 ; rdx = 10 ; break ;
}
/* show duty */
show_duty();
/* judge */
if ( xx == ALL_WHITE ) {
xcnt2 = 0 ;
/* TSK_ID2(CRANK) */
rsm_tsk( XCRANK ) ;
eflag = ON ;
}
if ( xx == LEFT_WHITE || xx == RIGHT_WHITE ) {
/* set directions */
if ( xx == LEFT_WHITE ) {
*(xdir+0) = DIR_LEFT ;
*(xdir+1) = DIR_RIGHT ;
}
if ( xx == RIGHT_WHITE ) {
*(xdir+0) = DIR_RIGHT ;
*(xdir+1) = DIR_LEFT ;
}
/* TSK_ID4(LANE) */
xcnt4 = 0 ;
rsm_tsk( XLANE ) ;
eflag = ON ;
}
/* DEBUG */
crlf();
xcnt1++;
if ( xcnt1 == 16 ) { xcnt1 = 0 ; }
/* delay 100ms */
if ( eflag == ON ) {
eflag = OFF ;
slp_tsk();
} else {
wai_tsk( 15 );
}
}
void tsk2_proc()
{
byte xx ;
byte eflag ;
/* */
eflag = OFF ;
rs_putchar('\t');
show_hex( xcnt2 );
/* get code */
xx = get_sensor(XCRANK,xcnt2);
rs_putchar('\t');
show_info( xx );
rs_putchar('\t');
/* DEBUG */
if ( xcnt2 == 0 ) {
crlf();
rs_puts("+ CRANK +");
crlf();
}
/* set duty ratio */
switch ( xx ) {
case ALL_BLACK : ldx = 10 ; rdx = 10 ; break ;
case ALL_WHITE : ldx = 10 ; rdx = 10 ; break ;
case LEFT_WHITE : ldx = 10 ; rdx = 10 ; break ;
case RIGHT_WHITE : ldx = 10 ; rdx = 10 ; break ;
case CENTER : ldx = 30 ; rdx = 30 ; break ;
case TINY_RIGHT : ldx = 30 ; rdx = 35 ; break ;
case RIGHT : ldx = 30 ; rdx = 40 ; break ;
case BIG_RIGHT : ldx = 30 ; rdx = 45 ; break ;
case TINY_LEFT : ldx = 35 ; rdx = 30 ; break ;
case LEFT : ldx = 40 ; rdx = 30 ; break ;
case BIG_LEFT : ldx = 45 ; rdx = 30 ; break ;
case BOTH_WHITE : ldx = 10 ; rdx = 10 ; break ;
default : ldx = 10 ; rdx = 10 ; break ;
}
/* show duty */
show_duty();
/* judge */
if ( xx == ALL_BLACK ) {
xcnt3 = 0 ;
/* */
rs_puts("DIR:");
if ( *(xdir+0) == DIR_LEFT ) { rs_putchar('L'); }
if ( *(xdir+0) == DIR_RIGHT ) { rs_putchar('R'); }
rs_puts("\t>>ROTATE");
crlf();
/* TSK_ID3(ROTATE) */
rsm_tsk( XROTATE ) ;
eflag = ON ;
}
if ( xx == LEFT_WHITE || xx == RIGHT_WHITE ) {
/* set directions */
if ( xx == LEFT_WHITE ) {
*(xdir+0) = DIR_LEFT ;
*(xdir+1) = DIR_RIGHT ;
}
if ( xx == RIGHT_WHITE ) {
*(xdir+0) = DIR_RIGHT ;
*(xdir+1) = DIR_LEFT ;
}
}
/* DEBUG */
crlf();
xcnt2++;
if ( xcnt2 == 16 ) { xcnt2 = 0 ; }
/* delay 100ms */
if ( eflag == ON ) {
eflag = OFF ;
slp_tsk();
} else {
wai_tsk( 100 );
}
}
void tsk3_proc()
{
byte xx ;
byte eflag ;
/* */
eflag = OFF ;
rs_puts("\t\t");
show_hex( xcnt3 );
/* get code */
xx = get_sensor(XROTATE,xcnt3);
rs_putchar('\t');
show_info( xx );
rs_putchar('\t');
/* DEBUG */
if ( xcnt3 == 0 ) {
crlf();
rs_puts("+ ROTATE + ");
crlf();
}
/* set duty ratio */
switch ( xx ) {
case ALL_BLACK : ldx = 10 ; rdx = 10 ; break ;
case ALL_WHITE : ldx = 10 ; rdx = 10 ; break ;
case LEFT_WHITE : ldx = 10 ; rdx = 50 ; break ;
case RIGHT_WHITE : ldx = 50 ; rdx = 10 ; break ;
case CENTER : ldx = 30 ; rdx = 30 ; break ;
case TINY_RIGHT : ldx = 30 ; rdx = 35 ; break ;
case RIGHT : ldx = 30 ; rdx = 40 ; break ;
case BIG_RIGHT : ldx = 30 ; rdx = 45 ; break ;
case TINY_LEFT : ldx = 35 ; rdx = 30 ; break ;
case LEFT : ldx = 40 ; rdx = 30 ; break ;
case BIG_LEFT : ldx = 45 ; rdx = 30 ; break ;
case BOTH_WHITE : ldx = 25 ; rdx = 25 ; break ;
default : ldx = 10 ; rdx = 10 ; break ;
}
/* show duty */
show_duty();
/* judge */
if ( xx == CENTER ) {
*(xdir+0) = DIR_CENTER ;
*(xdir+1) = DIR_CENTER ;
rs_puts("\t>>NORMAL");
crlf();
/* TSK_ID1(NORMAL) */
rsm_tsk( XNORMAL ) ;
eflag = ON ;
}
/* DEBUG */
crlf();
xcnt3++;
if ( xcnt3 == 16 ) { xcnt3 = 0 ; }
/* delay 1000ms */
if ( eflag == ON ) {
eflag = OFF ;
slp_tsk();
} else {
wai_tsk( 100 );
}
}
void tsk4_proc()
{
byte xx ;
byte eflag ;
/* */
eflag = OFF ;
rs_putchar('\t');
show_hex( xcnt4 );
/* get code */
xx = get_sensor(XLANE,xcnt4);
rs_putchar('\t');
show_info( xx );
rs_putchar('\t');
/* set duty ratio */
switch ( xx ) {
case ALL_BLACK : ldx = 10 ; rdx = 10 ; break ;
case ALL_WHITE : ldx = 25 ; rdx = 25 ; break ;
case LEFT_WHITE : ldx = 10 ; rdx = 10 ; break ;
case RIGHT_WHITE : ldx = 10 ; rdx = 10 ; break ;
case CENTER : ldx = 30 ; rdx = 30 ; break ;
case TINY_RIGHT : ldx = 30 ; rdx = 35 ; break ;
case RIGHT : ldx = 30 ; rdx = 40 ; break ;
case BIG_RIGHT : ldx = 30 ; rdx = 45 ; break ;
case TINY_LEFT : ldx = 35 ; rdx = 30 ; break ;
case LEFT : ldx = 40 ; rdx = 30 ; break ;
case BIG_LEFT : ldx = 45 ; rdx = 30 ; break ;
case BOTH_WHITE : ldx = 10 ; rdx = 10 ; break ;
default : ldx = 10 ; rdx = 10 ; break ;
}
/* show duty */
show_duty();
/* DEBUG */
if ( xcnt4 == 0 ) {
crlf();
rs_puts("+ LANE +");
crlf();
}
/* judge */
if ( xx == ALL_BLACK ) {
xcnt5 = 0 ;
/* message */
rs_puts("\t>>CHANGE");
crlf();
/* TSK_ID5(CHANGE) */
rsm_tsk( XCHANGE ) ;
eflag = ON ;
}
/* DEBUG */
crlf();
xcnt4++;
if ( xcnt4 == 16 ) { xcnt4 = 0 ; }
/* delay 1000ms */
if ( eflag == ON ) {
eflag = OFF ;
slp_tsk();
} else {
wai_tsk( 100 );
}
}
void tsk5_proc()
{
byte xx ;
byte eflag ;
/* */
eflag = OFF ;
rs_puts("\t\t");
show_hex( xcnt5 );
/* get code */
xx = get_sensor(XCHANGE,xcnt5);
rs_putchar('\t');
show_info( xx );
rs_putchar('\t');
/* set duty ratio */
switch ( xx ) {
case ALL_BLACK : ldx = 30 ; rdx = 30 ; break ;
case ALL_WHITE : ldx = 10 ; rdx = 10 ; break ;
case LEFT_WHITE : ldx = 50 ; rdx = 10 ; break ;
case RIGHT_WHITE : ldx = 10 ; rdx = 50 ; break ;
case CENTER : ldx = 50 ; rdx = 50 ; break ;
case TINY_RIGHT : ldx = 55 ; rdx = 50 ; break ;
case RIGHT : ldx = 60 ; rdx = 50 ; break ;
case BIG_RIGHT : ldx = 65 ; rdx = 50 ; break ;
case TINY_LEFT : ldx = 50 ; rdx = 55 ; break ;
case LEFT : ldx = 50 ; rdx = 60 ; break ;
case BIG_LEFT : ldx = 50 ; rdx = 65 ; break ;
case BOTH_WHITE : ldx = 10 ; rdx = 10 ; break ;
default : ldx = 10 ; rdx = 10 ; break ;
}
/* show duty */
show_duty();
/* DEBUG */
if ( xcnt5 == 0 ) {
crlf();
rs_puts("+ CHANGE +");
rs_puts("\tDir:");
if ( *(xdir+0) == DIR_LEFT ) { rs_putchar('L'); }
if ( *(xdir+0) == DIR_RIGHT ) { rs_putchar('R'); }
crlf();
}
/* judge */
if ( xcnt5 == 8 ) {
xcnt6 = 0 ;
/* message */
rs_puts("\t>>BLIND");
crlf();
/* TSK_ID5(BLIND) */
rsm_tsk( XBLIND ) ;
eflag = ON ;
}
/* DEBUG */
crlf();
xcnt5++;
if ( xcnt5 == 16 ) { xcnt5 = 0 ; }
/* delay 1000ms */
if ( eflag == ON ) {
eflag = OFF ;
slp_tsk();
} else {
wai_tsk( 100 );
}
}
void tsk6_proc()
{
byte xx ;
byte eflag ;
/* */
eflag = OFF ;
rs_puts("\t\t\t");
show_hex( xcnt6 );
/* get code */
xx = get_sensor(XBLIND,xcnt6);
rs_putchar('\t');
show_info( xx );
rs_putchar('\t');
/* set duty ratio */
switch ( xx ) {
case ALL_BLACK : ldx = 30 ; rdx = 30 ; break ;
case ALL_WHITE : ldx = 30 ; rdx = 30 ; break ;
case LEFT_WHITE : ldx = 50 ; rdx = 10 ; break ;
case RIGHT_WHITE : ldx = 10 ; rdx = 50 ; break ;
case CENTER : ldx = 50 ; rdx = 50 ; break ;
case TINY_RIGHT : ldx = 55 ; rdx = 50 ; break ;
case RIGHT : ldx = 60 ; rdx = 50 ; break ;
case BIG_RIGHT : ldx = 65 ; rdx = 50 ; break ;
case TINY_LEFT : ldx = 50 ; rdx = 55 ; break ;
case LEFT : ldx = 50 ; rdx = 60 ; break ;
case BIG_LEFT : ldx = 50 ; rdx = 65 ; break ;
case BOTH_WHITE : ldx = 10 ; rdx = 10 ; break ;
default : ldx = 10 ; rdx = 10 ; break ;
}
/* show duty */
show_duty();
/* DEBUG */
if ( xcnt6 == 0 ) {
crlf();
rs_puts("+ BLIND +");
rs_puts("\tDir:");
if ( *(xdir+1) == DIR_LEFT ) { rs_putchar('L'); }
if ( *(xdir+1) == DIR_RIGHT ) { rs_putchar('R'); }
crlf();
}
/* judge */
if ( xx == CENTER ) {
/* message */
rs_puts("\t>>NORMAL");
crlf();
/* TSK_ID1(NORMAL) */
rsm_tsk( XNORMAL ) ;
eflag = ON ;
}
/* DEBUG */
crlf();
xcnt6++;
if ( xcnt6 == 16 ) { xcnt6 = 0 ; }
/* delay 1000ms */
if ( eflag == ON ) {
eflag = OFF ;
slp_tsk();
} else {
wai_tsk( 100 );
}
}
void tsk7_proc(void)
{
/* command intepreter */
if ( UFLAG == ON ) {
/* clear flag */
UFLAG = OFF ;
/* new line */
crlf() ;
/* get command */
cmd = sbuf[0] ;
/* help */
if ( cmd == '?' ) { show_help() ; }
/* enable */
if ( cmd == 'E' ) { EFLAG = ON ; }
/* disable */
if ( cmd == 'e' ) { EFLAG = OFF; }
}
}
void tsk8_proc(void)
{
/* impress */
if ( dcnt & ON ) { PORTB |= (1 << 5); }
else { PORTB &= ~(1 << 5); }
/* increment */
dcnt++ ;
/* delay 500ms */
wai_tsk( 50 );
}
void update_trigger()
{
WFLAG = ON ;
}
char get_hex(byte x)
{
char result ;
/* default */
result = '0' ;
/* convert */
if ( x < 10 ) { result = x + '0' ; }
if ( x > 9 ) { result = x - 10 + 'A' ; }
return result ;
}
void show_hex(byte x)
{
byte dh ;
byte dl ;
/* separate */
dh = (x >> 4) & 15 ;
dl = x & 15 ;
/* */
rs_putchar( get_hex( dh ) ) ;
rs_putchar( get_hex( dl ) ) ;
}
void binary_display(byte x)
{
int i ;
for ( i = 7 ; i > -1 ; i-- ) {
rs_putchar('0'+((x >> i) & ON));
}
}
void show_duty()
{
char msg[3] ;
/* delimiter */
*(msg+2) = '\0';
/* left */
*(msg+0) = ldx / 10 + '0' ;
*(msg+1) = ldx % 10 + '0' ;
if ( *(msg+0) == ' ' ) { *(msg+0) = ' ' ; }
rs_puts("Left = ");
rs_puts( msg );
/* right */
*(msg+0) = rdx / 10 + '0' ;
*(msg+1) = rdx % 10 + '0' ;
if ( *(msg+0) == ' ' ) { *(msg+0) = ' ' ; }
rs_puts(" Right = ");
rs_puts( msg );
/* new line */
crlf();
}
/* initialize */
void setup()
{
/* initialize serial port */
Serial.begin(9600);
sindex = 0 ;
/* */
PORTB = 0xff ;
PORTC = 0xff ;
/* data direction */
DDRB = 0xff ;
DDRC = 0x00 ;
/* initialize variables */
sindex = 0 ;
UFLAG = OFF ;
dcnt = 0 ;
xcnt1 = 0 ;
timcnt = 0 ;
/* initialize USO */
init_os();
run_tsk = TSK_ID0 ;
state = MD_IDLE ;
/* initialize table */
init_road();
/* trigger period */
MsTimer2::set(10,update_trigger);
/* enable */
MsTimer2::start();
/* direction */
*(xdir+0) = DIR_CENTER ;
*(xdir+1) = DIR_CENTER ;
ldx = 0 ;
rdx = 0 ;
show_duty();
/* */
crlf();
show_help();
}
/* main */
void loop()
{
/* 10ms has passed */
if ( WFLAG == ON ) {
/* clear flag */
WFLAG = OFF ;
/* call */
timer_handler();
}
/* round robbin */
pcur_tsk = tcbx[run_tsk] ;
if ( is_tsk_ready( run_tsk ) != OFF ) { (*(pcur_tsk.tsk))(); }
run_tsk++;
if ( run_tsk == TSK_ID_MAX ) { run_tsk = TSK_ID0 ; }
}
/* 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 ;
}
}
}
目次 前 次