マイクロマウス2000で採用したプログラム（robotics.z80）
;-----------------------------------------------------
; ロボティクス入門のプログラムを参考とした。
; ただしメカニカルセンサは使わず
; 2000.11.23 不死川魔沙王
;-----------------------------------------------------
PPIA EQU 20H ;8255のAポート（横センサ）
PPIB EQU 21H ;8255のBポート（モータ回転制御）
PPIC EQU 22H ;8255のCポート（前センサ、前から7と0使用）
PPIW EQU 23H ;コントロールワード
MOT EQU PPIB ;PPIBがモーター制御
;
;
MFOWARD EQU 00010011b ;前進
MBACK EQU 00110001b ;後退
MRELEASE EQU 01000100b ;解放（モータロック解除）
TURNLEFT EQU 00010001b ;回転左
TURNRIGHT EQU 00110011b ;回転右
HOSEIMIGI EQU 00010100b ;右に補正（右励磁オフ）
HOSEIHIDARI EQU 01000011b ;左に補正（左励磁オフ）
;
;-------------------------------------- WORK AREA
;
DW 0
DW 0
DW 0
DW 0
DS 0F8H
;
;---------------------------------------
;
NOP ;CPUの慣らし運転
NOP
NOP
NOP
NOP
LD A,99H
OUT (PPIW),A ;A,Cポート入力、Bポート出力の宣言
;-----------------------------------------
; 本文ここから
;-----------------------------------------
ld a,01000100b
out (mot),a ;motor initialize
MAIN_ST:
CALL FMOVE
CALL SHIBARAKU
JP MAIN_ST
;-----------------------------------------
; 本文ここまで
;-----------------------------------------
;-------------------------------------------------------------------------
; 1区間を姿勢修正しつつ前進するルーチン
;-------------------------------------------------------------------------
FMOVE: LD A,204 ;区間データの設定
LD (COUNT),A
FL: CALL MOVEF ;1パルス前進
IN A,(PPIC);前センサの検索
and 11111111b
JP NZ,FHOSEI ;壁があればFHOSEIへ
CALL SHUSEI ;横センサによる姿勢制御
LD A,(COUNT)
DEC A ;(COUNT)値を1減算
LD (COUNT),A
JP NZ,FL ;0でなければFLへ
JP Fret ;0ならばFRETへ
FHOSEI: ;前壁を検知したら00011000b が出るまで進む
CALL SHIBARAKU
CALL PSHUSEI
IN A,(PPIA)
AND 00001111b
JP NZ,LSENSE
CALL GIRIGIRI
CALL MOVER
JP FRET
LSENSE: IN A,(PPIA)
AND 11110000b
JP NZ,WSENSE
CALL GIRIGIRI
CALL MOVEL
JP FRET
WSENSE:
CALL UTURN
JP FRET2
FRET:
CALL PSHUSEI
FRET2: RET
;-------------------------------------------------------------------------
; 1パルス前進
;-------------------------------------------------------------------------
MOVEF: LD A,MFOWARD
OUT (MOT),A
CALL IPPATU
XOR A
OUT (MOT),A
CALL WAIT1
RET
;--------------------------------------
; 一区間走行
;--------------------------------------
KMOVEF: LD D,202 ;区間走行データ
FLP: LD A,MFOWARD
OUT (MOT),A
CALL IPPATU ;パルス一発分のタイマ
XOR A ;00000000bをBポートに出力
OUT (MOT),A
CALL WAIT1 ;タイマ
DEC D ;区間走行データを-1
JP NZ,FLP ;0でなければFLPへ
RET
;----------------------------------------
; 壁ぎりぎりまで近づく
;----------------------------------------
GIRIGIRI:
CALL MOVEF
in a,(ppia)
; AND 00011000b
XOR 00011000b
JP NZ,GIRIGIRI
; JP Z,GIRIGIRI
GIRET: RET
;------------------------------------------
; しばらく止まる
;-------------------------------------------
SHIBARAKU:
PUSH BC
LD B,30
LOOPS:
LD A,01000100b
OUT (MOT),A
CALL WAIT2
DJNZ LOOPS
POP BC
RET
;-------------------------------------------
; 1パルス左に動く
;------------------------------------------
PULSEL:
PUSH BC
LD B,7
PUSH AF
LOOPPL:
LD A,TURNLEFT
OUT (MOT),A
LD A,0
OUT (MOT),A
CALL WAIT2
DJNZ LOOPPL
POP AF
POP BC
RET
;-------------------------------------------
; 1パルス右に動く
;------------------------------------------
PULSER:
PUSH BC
LD B,7
PUSH AF
LOOPPR:
LD A,TURNRIGHT
OUT (MOT),A
LD A,0
OUT (MOT),A
CALL WAIT2
DJNZ LOOPPR
POP AF
POP BC
RET
;------------------------------------------------------
; パルスによる小修正
;------------------------------------------------------
PSHUSEI:
LD A,(PPIA)
AND 10001000b
JP Z,LP
CALL PULSER
JP PRET
LP: LD A,(PPIA)
AND 00010001b
JP Z,PRET
CALL PULSEL
PRET: RET
;--------------------------------------
; パルス一発分のタイマ
;--------------------------------------
IPPATU: PUSH BC
LD B,10h
DJNZ 0+$
POP BC
RET
;--------------------------------------
; タイマサブルーチン1（はやい）
;--------------------------------------
WAIT1: PUSH BC
LD B,15 ;15*200で時間稼ぎしている
WLP11: LD C,200
WLP12: DEC C
JP NZ,WLP12
DEC B
JP NZ,WLP11
POP BC
RET
;--------------------------------------
; 左90度回転
;--------------------------------------
MOVEL: LD D,110 ;110発で90度回転
LLP: LD A,TURNLEFT
OUT (MOT),A
CALL IPPATU
XOR A
OUT (MOT),A
CALL WAIT2 ;ゆっくり回転
DEC D
JP NZ,LLP
RET
;--------------------------------------
; 右90度回転
;--------------------------------------
MOVER: LD D,105 ;105発で右90度回転
RLP: LD A,TURNRIGHT
OUT (MOT),A
CALL IPPATU
XOR A
OUT (MOT),A
CALL WAIT2 ;ゆっくり回転
DEC D
JP NZ,RLP
RET
;--------------------------------------
; 180度回転
;--------------------------------------
UTURN: LD D,210
ULP: LD A,TURNLEFT
OUT (MOT),A
CALL IPPATU
XOR A
OUT (MOT),A
CALL WAIT2 ;ゆっくり回転
DEC D
JP NZ,ULP
RET
;--------------------------------------
; タイマサブルーチン2（おそい）
;--------------------------------------
WAIT2: PUSH BC
LD B,30 ;30*200で時間稼ぎしている
WLP21: LD C,200
WLP22: DEC C
JP NZ,WLP22
DEC B
JP NZ,WLP21
POP BC
RET
;--------------------------------------
; 修正動作
;一方を励磁オフにして、他方に1パルス加える
;--------------------------------------
RFMOVE: LD A,HOSEIMIGI
OUT (MOT),A
CALL IPPATU ;左は普通に止め、
LD A,00010100b ;右は励磁オフにして流す
CALL WAIT1
RET
LFMOVE: LD A,HOSEIHIDARI
OUT (MOT),A
CALL IPPATU ;右は普通に止め、
LD A,01000011b ;左は励磁オフにして流す
CALL WAIT1
RET
;--------------------------------------
; 姿勢修正ルーチン
;--------------------------------------
SHUSEI: IN A,(PPIA) ;横センサデータ取りこみ
PUSH AF
AND 11110000b ;左センサデータを
LD B,A ;Bレジスタに取りこみ
POP AF
AND 00001111b ;右センサデータを
LD C,A ;Cレジスタに取りこみ
ADD A,B ;右左に壁が無ければ
JP Z,LPEND ;LPENDへ
LD A,B
ADD A,00H ;左側に壁が無ければ
JP Z,LLLP ;LLLPへ
LD A,C
ADD A,00H ;右側に壁が無ければ
JP Z,L1 ;L1へ
BIT 3,A ;右の内側を検知(XXXX1000）で
JP NZ,LF ;LFへ
LD A,B
BIT 4,A ;左の内側を検知で
JP NZ,RF ;RFへ
JP LPEND ;中心を検知でLPENDへ
LLLP: LD A,C ;右センサで見る
BIT 3,A ;右の内側を検知(XXXX1000）で
JP NZ,LF ;LFへ（上と同じ）
BIT 0,A ;右の外側を検知で
JP NZ,RF ;RFへ
JP LPEND ;右の中心検知でLPENDへ
L1: LD A,B ;左センサで見る
BIT 4,A ;左の内側を検知で
JP NZ,RF ;RFへ
BIT 7,A ;左の外側を検知で
JP NZ,LF ;LFへ
JP Z,LPEND ;左の中心を検知でLPENDへ
LF: CALL LFMOVE ;左へ1パルスモータを動かす
JP LPEND
RF: CALL RFMOVE ;右へ1パルスモータを動かす
JP LPEND
PUSH BC
LD B,0FH
LPEND: RET
;-------------------------------------------
; データ格納エリア
;--------------------------------------------
COUNT DB 0 ;１区間走行データを入れる
FLAG DB 0 ;壁検索終了データを入れる
DATA DB 0 ;壁データ格納場所
DATA1 DB 0 ;壁検索の1回目データ
DATA2 DB 0 ;壁検索の2回目データ
DATANX DB 0 ;マウスの向きにあった壁データ
;---------------------------------------------------
; プログラムの終わり
;---------------------------------------------------
owari:
ld a,01000100b
out (MOT),A
halt
end