走行モード遷移テスト
ForthでMCRマシンを動かす前に、Pythonを使い センサー値とモード値をパラメータで入力すると 次のモードを返す関数を定義して、机上での動作 確認をします。 モードは、次の６種を用意。
- NONE
- NORMAL
- CRANK
- TURN
- LANE
- BLIND
モードは、文字列で表現した方が理解しやすいので
次のように定義。
NONE = 0
NORMAL = 1
CRANK = 2
TURN = 3
LANE = 4
BLIND = 5
センサー値も文字列で定義。
ALL_BLACK = 0
RIGHT = 1
CENTER = 2
RIGHT_WHITE = 3
LEFT = 4
OUTOFV = 5
LEFT_WHITE = 6
ALL_WHITE = 7
関数に渡すパラメータは、少ない方がわかりやすいので
８ビットの上位４ビットにセンター値、下位４ビットに
モード値を入れて、次のように内部で扱うことに。
def calcmode(x):
# separate
imode = x % 16
isensor = (x >> 4) % 16
# default
result = NONE
# ???
return result
状態遷移図を利用して、センサー値とモード値から
次のモード値を決定します。
NORMAL -> CRANK
モード値がNORMALでセンサー値がALL_WHITEなら
CRANKに遷移。
if imode == NORMAL :
result = NORMAL
if isensor == ALL_WHITE :
result = CRANK
NORMAL -> LANE
モード値がNORMALでセンサー値がLEFT_WHITEか
RIGHT_WHITEなら、LANEに遷移。
CRANKへの遷移を合わせて、以下とすればよいはず。
# NORMAL => CRANK , LANE
if imode == NORMAL :
result = NORMAL
if isensor == ALL_WHITE :
result = CRANK
if isensor == LEFT_WHITE :
result = LANE
if isensor == RIGHT_WHITE :
result = LANE
CRANK -> TURN
モード値がCRANKでセンサー値がLEFT_WHITEか
RIGHT_WHITEなら、TURNに遷移。
if imode == CRANK :
result = CRANK
if isensor == LEFT_WHITE :
result = TURN
if isensor == RIGHT_WHITE :
result = TURN
TURN -> NORMAL
モード値がTURNでセンサー値がCENTERなら
NORMALに遷移。
if imode == TURN :
result = TURN
if isensor == CENTER :
result = NORMAL
LANE -> BLIND
モード値がLANEでセンサー値がALL_BLACKなら
BLINDに遷移。
if imode == LANE :
result = LANE
if isensor == ALL_BLACK :
result = BLIND
BLIND -> NORMAL
モード値がBLINDでセンサー値がCENTERなら
NORMALに遷移。
if imode == BLIND :
result = BLIND
if isensor == CENTER :
result = NORMAL
センサー値は、ALL_BLACKを除いて、どの値に
なっても、モード値はNORMALに遷移してよいと
考え直して、次のように変更。
if imode == BLIND :
result = BLIND
if isensor != ALL_BLACK :
result = NORMAL
NORMAL -> NONE
モード値がNORMALのときに、タイムアウトすると
NONEに遷移するとします。
タイムアウトには、時間をカウントしている変数
timcntxがある値より大きくなれば、遷移させると
して実現。
変数を使って、状態遷移させます。
if timcntx > 1000 :
result = NONE
ここまでの内容を関数にまとめると以下。
def calcmode(x):
# separate
imode = x % 16
isensor = (x >> 4) % 16
# default
result = NONE
# NORMAL => CRANK , LANE
if imode == NORMAL :
result = NORMAL
if isensor == ALL_WHITE :
result = CRANK
if isensor == LEFT_WHITE :
result = LANE
if isensor == RIGHT_WHITE :
result = LANE
# CRANK -> TURN
if imode == CRANK :
result = CRANK
if isensor == LEFT_WHITE :
result = TURN
if isensor == RIGHT_WHITE :
result = TURN
# TURN -> NORMAL
if imode == TURN :
result = TURN
if isensor == CENTER :
result = NORMAL
# LANE -> BLIND
if imode == LANE :
result = LANE
if isensor == ALL_BLACK :
result = BLIND
# BLIND -> NORMAL
if imode == BLIND :
result = BLIND
if isensor != ALL_BLACK :
result = NORMAL
# timer counter
if timcntx > 1000 :
result = NONE
return result
テストしやすいように、数値を文字列に変換する
関数を定義しておきます。
def putsensor(x) :
xstr = ['ALL_BLACK','RIGHT','CENTER','RIGHT_WHITE','LEFT','OUTOFV','LEFT_WHITE','ALL_WHITE']
return xstr[x]
def showmode(x):
msg = ['NONE','NORMAL','CRANK','TURN','LANE','BLIND',"???"]
print msg[x]
テスト動作を加えて、まとめます。
NONE = 0
NORMAL = 1
CRANK = 2
TURN = 3
LANE = 4
BLIND = 5
ALL_BLACK = 0
RIGHT = 1
CENTER = 2
RIGHT_WHITE = 3
LEFT = 4
OUTOFV = 5
LEFT_WHITE = 6
ALL_WHITE = 7
def putsensor(x) :
xstr = ['ALL_BLACK','RIGHT','CENTER','RIGHT_WHITE','LEFT','OUTOFV','LEFT_WHITE','ALL_WHITE']
return xstr[x]
def calcmode(x):
# separate
imode = x % 16
isensor = (x >> 4) % 16
print "Sensor =",isensor,"Mode = ",imode,'->',
#print "Sensor =",putsensor(isensor),"Mode = ",imode,'->',
# default
result = NONE
# NORMAL => CRANK , LANE
if imode == NORMAL :
result = NORMAL
if isensor == ALL_WHITE :
result = CRANK
if isensor == LEFT_WHITE :
result = LANE
if isensor == RIGHT_WHITE :
result = LANE
# CRANK -> TURN
if imode == CRANK :
result = CRANK
if isensor == LEFT_WHITE :
result = TURN
if isensor == RIGHT_WHITE :
result = TURN
# TURN -> NORMAL
if imode == TURN :
result = TURN
if isensor == CENTER :
result = NORMAL
# LANE -> BLIND
if imode == LANE :
result = LANE
if isensor == ALL_BLACK :
result = BLIND
# BLIND -> NORMAL
if imode == BLIND :
result = BLIND
if isensor != ALL_BLACK :
result = NORMAL
# timer counter
if timcntx > 1000 :
result = NONE
return result
def showmode(x):
msg = ['NONE','NORMAL','CRANK','TURN','LANE','BLIND',"???"]
print msg[x]
timcntx = 100
print "timer counter = ",timcntx
for ee in range(1,6) :
for e in range(8) :
xx = (e << 4) + ee
yy = calcmode( xx )
showmode(yy)
timcntx = 10000
print "timer counter = ",timcntx
for ee in range(1,6) :
for e in range(8) :
xx = (e << 4) + ee
yy = calcmode( xx )
showmode(yy)
使い方は、以下。
I/Oリダイレクトによりテキストファイルに
入れたので、内容を確認すると、以下。
timer counter = 100
Sensor = 0 Mode = 1 -> NORMAL
Sensor = 1 Mode = 1 -> NORMAL
Sensor = 2 Mode = 1 -> NORMAL
Sensor = 3 Mode = 1 -> LANE
Sensor = 4 Mode = 1 -> NORMAL
Sensor = 5 Mode = 1 -> NORMAL
Sensor = 6 Mode = 1 -> LANE
Sensor = 7 Mode = 1 -> CRANK
Sensor = 0 Mode = 2 -> CRANK
Sensor = 1 Mode = 2 -> CRANK
Sensor = 2 Mode = 2 -> CRANK
Sensor = 3 Mode = 2 -> TURN
Sensor = 4 Mode = 2 -> CRANK
Sensor = 5 Mode = 2 -> CRANK
Sensor = 6 Mode = 2 -> TURN
Sensor = 7 Mode = 2 -> CRANK
Sensor = 0 Mode = 3 -> TURN
Sensor = 1 Mode = 3 -> TURN
Sensor = 2 Mode = 3 -> NORMAL
Sensor = 3 Mode = 3 -> TURN
Sensor = 4 Mode = 3 -> TURN
Sensor = 5 Mode = 3 -> TURN
Sensor = 6 Mode = 3 -> TURN
Sensor = 7 Mode = 3 -> TURN
Sensor = 0 Mode = 4 -> BLIND
Sensor = 1 Mode = 4 -> LANE
Sensor = 2 Mode = 4 -> LANE
Sensor = 3 Mode = 4 -> LANE
Sensor = 4 Mode = 4 -> LANE
Sensor = 5 Mode = 4 -> LANE
Sensor = 6 Mode = 4 -> LANE
Sensor = 7 Mode = 4 -> LANE
Sensor = 0 Mode = 5 -> BLIND
Sensor = 1 Mode = 5 -> NORMAL
Sensor = 2 Mode = 5 -> NORMAL
Sensor = 3 Mode = 5 -> NORMAL
Sensor = 4 Mode = 5 -> NORMAL
Sensor = 5 Mode = 5 -> NORMAL
Sensor = 6 Mode = 5 -> NORMAL
Sensor = 7 Mode = 5 -> NORMAL
timer counter = 10000
Sensor = 0 Mode = 1 -> NONE
Sensor = 1 Mode = 1 -> NONE
Sensor = 2 Mode = 1 -> NONE
Sensor = 3 Mode = 1 -> NONE
Sensor = 4 Mode = 1 -> NONE
Sensor = 5 Mode = 1 -> NONE
Sensor = 6 Mode = 1 -> NONE
Sensor = 7 Mode = 1 -> NONE
Sensor = 0 Mode = 2 -> NONE
Sensor = 1 Mode = 2 -> NONE
Sensor = 2 Mode = 2 -> NONE
Sensor = 3 Mode = 2 -> NONE
Sensor = 4 Mode = 2 -> NONE
Sensor = 5 Mode = 2 -> NONE
Sensor = 6 Mode = 2 -> NONE
Sensor = 7 Mode = 2 -> NONE
Sensor = 0 Mode = 3 -> NONE
Sensor = 1 Mode = 3 -> NONE
Sensor = 2 Mode = 3 -> NONE
Sensor = 3 Mode = 3 -> NONE
Sensor = 4 Mode = 3 -> NONE
Sensor = 5 Mode = 3 -> NONE
Sensor = 6 Mode = 3 -> NONE
Sensor = 7 Mode = 3 -> NONE
Sensor = 0 Mode = 4 -> NONE
Sensor = 1 Mode = 4 -> NONE
Sensor = 2 Mode = 4 -> NONE
Sensor = 3 Mode = 4 -> NONE
Sensor = 4 Mode = 4 -> NONE
Sensor = 5 Mode = 4 -> NONE
Sensor = 6 Mode = 4 -> NONE
Sensor = 7 Mode = 4 -> NONE
Sensor = 0 Mode = 5 -> NONE
Sensor = 1 Mode = 5 -> NONE
Sensor = 2 Mode = 5 -> NONE
Sensor = 3 Mode = 5 -> NONE
Sensor = 4 Mode = 5 -> NONE
Sensor = 5 Mode = 5 -> NONE
Sensor = 6 Mode = 5 -> NONE
Sensor = 7 Mode = 5 -> NONE
NORMAL→NONEの遷移では、変数timcntxを参照しているので
10000を格納して、モードの遷移が起きることを確認して
います。
目次 前 次