;マイクロマウス2001「肉弾人生」 2001.11.22 藤川雅男 ; ;スラロームで位置修正しながら進むプログラム ;壁に正面衝突したら後退する ;壁データを覚えながら前進し右手法で迷路を解く ; kioku.z80(shusei2.z80をもとに作成) ; 基本的な考え方 ;定速回転が154パルスに達したら減速に入る。 ;定速回転時はつねに前センサを監視し、反応があったらすぐに減速に入る。 ;前の壁に衝突したら後退する。 ;定速運転時に2回横壁データを取り、ANDした結果をWDATAに入れる。 ; ; 右手法 ;WDATAの数字と東西南北の区画のデータを見て次の行動を決定する。 ;WDATAの各ビットの意味 ;   0100 右壁あり ;   1000 前壁あり ;   0001 左壁あり ;   0010 後壁あり ; 00 北向き ;  01 東向き ;  10 南向き ;  11 西向き ; 00 その区画にきた回数(0〜3) ;条件1:左右に壁があるときは直進する。 ;条件2:初めての分岐点にきたときは、右回転、直進、左回転、Uターンの ; 順に優先順位をつける。 ;条件3:1回以上通過したことのある分岐点にきたときは通過回数の ; 少ないほうへ進む。 ; 通過回数がすべて同じであった場合には条件2によって判断する。 ;条件4:袋小路はUターンする。 ; ;このもともとのWDATAは壁がある方向を示しているものであるが、 ;マウスから見た相対的な方向を示しているだけである。 ;マウスの向きは場所によって異なるため、これでは条件2・3について ;調べられない。 ;そのため、これを絶対方向に修正してやる必要がある。 ; CTC0 EQU 10H ; CTC1 EQU 11H ; CTC2 EQU 12H ; CTC3 EQU 13H ; ; SIOCA EQU 19H ; RS-232C SIODA EQU 18H SIOCB EQU 1BH SIODB EQU 1AH ; PAD: EQU 1CH ; PAC: EQU 1DH ; PBD: EQU 1EH ; PBC: EQU 1FH ; PCD EQU 80H ;前壁センサ CON: EQU PAD ; PUSH-SW LED EQU PAD ; PSW EQU PAD ; MOT EQU PAD ;Aポート出力でモータ制御 SEN EQU PBD ;Bポート入力(負論理)でセンサ MAESEN EQU PCD ;前センサ00110000 ; ;  右左 ; SASETU EQU 00000000b USETU EQU 10100000b ZENSHIN EQU 10000000b KOUTAI EQU 00100000b ; NASHI EQU 0 ;TRNDAT用データ MIGI EQU 1 USHIRO EQU 2 HIDARI EQU 3 ; GOAL1 EQU MAP+12H GOAL2 EQU MAP+78H GOAL3 EQU MAP+87H GOAL4 EQU MAP+88H ;---------------------------------------------本文ここから ; PROGRAM START ; ORG 8000H HOKO: DS 1 ;移動方向(ZENSHINなど) MOTD: DS 1 ;モータへの出力信号バッファ IFR: DS 1 IFL: DS 1 WDATA1: DS 1 ;一回目の横壁チェックのときのデータ WDATA2: DS 1 ;二回目の横壁チェックのときのデータ WDATA: DS 1 ;壁データ ADATA: DS 1 ;壁データ絶対方向 TRNDAT: DS 1 ;回転方向データ(0回転なし1右2反転3左) SHUSEI: DS 1 ;後戻りの修正を行ったか(した:1,しない:0) ADDR: DS 2 ;現在のアドレス ADDRN: DS 2 ;北→前のアドレス(adrsifで変換する) ADDRE: DS 2 ;東→右のアドレス ADDRS: DS 2 ;南→後のアドレス ADDRW: DS 2 ;西→左のアドレス ADDR0: DS 2 ;ワーク領域(アドレスシフト時使う) ADDRB: DS 2 ;一つ前のアドレスを保存 DS 0E8H ;8100H DW INTR0 ;CTC0割り込みアドレス DW INTR1 ;CTC1割り込みアドレス DW INTR2 ;CTC2割り込みアドレス DW INTR3 ;CTC3割り込みアドレス MAIN: DI LD SP,0 CALL CPUINI CALL PIOINI CALL CTCINI ; LD A,ZENSHIN LD (HOKO),A LD (MOTD),A LD A,0 LD (WDATA),A ;壁データ初期化 LD (IFR),A ;モータ回転用フラグ初期化 LD (IFL),A LD (SHUSEI),A ;修正「なし」に設定 LD HL,MAP+10H ;最初に到達する区画のアドレス入力 LD (ADDR),HL LD A,NASHI ;回転方向初期化(回転しないに設定) LD (TRNDAT),A LD HL,MAP ;マップ情報クリア LD A,0FFH MAPCL: LD (HL),0 INC HL DEC A JP NZ,MAPCL LD HL,MAP LD A,11000111b ;スタート区画のデータ入力 LD (HL),A ld (addrb),hl LD A,0D5H ;CTC1,3イニシャライズ OUT (CTC3),A OUT (CTC1),A EI MAIN2: CALL TRN ;TRNDATに従い回転 LD A,ZENSHIN LD (MOTD),A LD (HOKO),A CALL KUKAN ;一歩前進 LD A,(SHUSEI) ;後戻り修正したか? AND A JP Z,MAIN21 LD HL,(ADDRB) LD (ADDR),HL MAIN21: XOR A ;後戻りフラグを0にする LD (SHUSEI),A CALL SHIFT ;壁データを絶対方向に変換して格納 LD A,(ADATA) LD HL,(ADDR) LD (HL),A LD C,L ;ゴールに入ったら終了 LD HL,GOAL1 LD A,L CP C JP Z,MINEND LD HL,GOAL2 LD A,L CP C JP Z,MINEND LD HL,GOAL3 LD A,L CP C JP Z,MINEND LD HL,GOAL4 LD A,L CP C JP Z,MINEND MINLP: CALL ADRSET ;現在のアドレスの四方のアドレスを計算する CALL NEXT ;次に進む区画のアドレスを決定しADDRに入れる JP MAIN2 MINEND: HALT ; ;************************************************ ; 次に進む方向を決める ;************************************************ ; 考え方 ;Cに1を入れる。 ;マウスから見た壁データ右を見る。 ; 壁がなかったら右の区画の情報を見る。 ; 来た回数がC以下ならば右折する。 ; Cより大きければ前の壁を見る。 ; 壁があったら前の壁を見る。 ;マウスから見た壁データ前を見る。 ; 壁がなかったら前の区画の情報を見る。 ; 来た回数がC以下ならば前進する。 ; Cより大きければ左の壁を見る。 ; 壁があったら左の壁を見る。 ;マウスから見た壁データ左を見る。 ; 壁がなかったら後の区画の情報を見る。 ; 来た回数がC以下ならば左折する。 ; Cより大きければCに+1してはじめに戻る。 ; 壁があったらUターンする。 ;壁情報を入れ、addrに次に進むアドレスを入れる。 ;Cは増えつづけるが壁データの「来た回数」は ;3までしかないので、無限ループにはならない。 ;このとき周りの区画のアドレスを調べる必要がある。 ;これは現在のアドレスと現在の方向から計算する。 ; NEXT: LD HL,(ADDR) ;今のアドレスを保存 LD (ADDRB),HL ;(後戻り用) LD C,01H ;通過確認用データC=01H LD A,(WDATA) AND 00110000b LD B,A RRC B ;方向データをBのビット0と1にうつす RRC B RRC B RRC B JP Z,NLP2 ;北向きならワークエリア回転させない NLP1: CALL ADRSIF ;レジスタBの値だけワークエリアを左回りに回転 DEC B JP NZ,NLP1 NLP2: LD A,(WDATA) AND 00000100b ;右の壁を調べる JP NZ,NLP3 ;壁があればNLP3へ LD HL,(ADDRE) ;右の区画の壁情報を見る LD A,(HL) AND 11000000b ;通過回数をAレジスタに取り出す RLCA RLCA CP C ;右の区画の通過回数がCより多ければNLP3へ JP NC,NLP3 LD A,MIGI ;右回転のデータをTRNDATへ LD (TRNDAT),A LD A,(WDATA) ADD A,10H ;現在のマウスの向きのデータに10Hを足して AND 00110000b ;次のマウスの方向を指定 LD HL,(ADDRE) ;次に進む区画のアドレスを指定 JP NRET NLP3: LD A,(WDATA) AND 00001000b ;前の壁を調べる JP NZ,NLP4 ;壁があればNLP4へ LD HL,(ADDRN) ;前の区画の壁情報を見る LD A,(HL) AND 11000000b ;通過回数をAレジスタに取り出す RLCA RLCA CP C ;前の区画の通過回数がCより多ければNLP4へ JP NC,NLP4 LD A,NASHI ;前進のデータをTRNDATへ LD (TRNDAT),A LD A,(WDATA) AND 00110000b ;次のマウスの方向を指定 LD HL,(ADDRN) ;次に進む区画のアドレスを指定 JP NRET NLP4: LD A,(WDATA) AND 00000001b ;左の壁を調べる JP NZ,NLP5 ;壁があればNLP5へ LD HL,(ADDRW) ;左の区画の壁情報を見る LD A,(HL) AND 11000000b ;通過回数をAレジスタに取り出す RLCA RLCA CP C ;左の区画の通過回数がCより多ければNLP5へ JP NC,NLP5 LD A,HIDARI ;左回転のデータをTRNDATへ LD (TRNDAT),A LD A,(WDATA) ADD A,30H ;現在のマウスの向きのデータに30Hを足して AND 00110000b ;次のマウスの方向を指定 LD HL,(ADDRW) ;次に進む区画のアドレスを指定 JP NRET NLP5: LD A,(WDATA) AND 00000010b ;後の壁を調べる JP NZ,NLP6 ;壁があればNLP6へ LD HL,(ADDRS) ;後の区画の壁情報を見る LD A,(HL) AND 11000000b ;通過回数をAレジスタに取り出す RLCA RLCA CP C ;後の区画の通過回数がCより多ければNLP6へ JP NC,NLP6 LD A,USHIRO ;180度回転のデータをTRNDATへ LD (TRNDAT),A LD A,(WDATA) ADD A,20H ;現在のマウスの向きのデータに20Hを足して AND 00110000b ;次のマウスの方向を指定 LD HL,(ADDRS) ;次に進む区画のアドレスを指定 NRET: LD (ADDR),HL LD (WDATA),A RET NLP6: INC C ;通過回数を+1する JP NLP2 ;NLP2に戻る ; ; 回転割り当てルーチン ; TRN: LD A,(TRNDAT) CP NASHI JP Z,TRNEND CP MIGI JP Z,TRNR CP HIDARI JP Z,TRNL CALL U_TURN JP TRNEND TRNR: CALL R_TURN JP TRNEND TRNL: CALL L_TURN TRNEND: RET ; ; 迷路を記憶する ; ;迷路は16×16区画ある。1区画のデータを8ビットであらわす。つまり、 ;256バイトあればすべての区画について情報を記憶することができる。 ;各ビットの意味 ;b7 b6 b5 b4 b3 b2 b1 b0 ;b7,b6 :その区画を通過した回数(0〜3。すなわち00,01,10,11) ;b5,b4 :マウスの向き(スタート北向き。北:00、東:01、南:10、西:11) ;b3 :マウスの前の壁の有無(→北の壁の有無) ;b2 :マウスの右の壁の有無(→東の壁の有無) ;b1 :マウスの後の壁の有無(→南の壁の有無) ;b0 :マウスの左の壁の有無(→西の壁の有無) ; ;記憶するアドレスの割り付け(↑北) ;F0H 90F0H ・・・ ;E0H 90E0H ・・・ ;D0H 90E0H ・・・ ;・・・ ;20H 9020H 9021H ・・・ ;10H 9010H 9011H 9012H ・・・ ;00H 9000H 9001H 9002H ・・・ 900DH 900EH 900FH ; 0H 1H 2H ・・・ DH EH FH ;始点9000H(例)で北向きに出発する。終点は9087H,9088H,9077H,9078Hである。 ; ; 現在のアドレス+10H ;現在のアドレス-1H 現在のアドレス 現在のアドレス+1H ; 現在のアドレス-10H ; ;このような関係になっていることがわかる。 ; ; 壁データの変換 ; ;WDATAの壁データはマウスから見た相対的なものであるので、記憶時は ;北を基準とした絶対的なものに変換する必要がある。 ;すなわち、 ;マウスが北向き(b5b4が"00")なら変換なし ;マウスが東向き(b5b4が"01")なら下4桁を1回右にローテート ;マウスが南向き(b5b4が"10")なら下4桁を2回右にローテート ;マウスが西向き(b5b4が"11")なら下4桁を3回右にローテート ;ローテートの回数は向きをあらわす数字と一致している。 ; ; ; 壁データを絶対方向に変換する ; SHIFT: LD A,(WDATA) AND 00110000b ;マウス方向データをBのビット0と1に LD B,A RRC B RRC B RRC B RRC B LD A,(WDATA) AND 00001111b INC B SHLP1: DEC B JP Z,SHLP2 ;レジスタBが0であればSHLP2へ RRA JP NC,SHLP1 ADD A,00001000b JP SHLP1 SHLP2: LD B,A LD A,(WDATA) ;マウス向きと壁データを合成 AND 00110000b OR B LD B,A LD A,(ADDR) AND 11000000b ;今までに来た回数をとりだす OR B ADD A,01000000b ;一回増やす LD (ADATA),A ;ADATAに保存 RET ; ; 区画のアドレス割付 ; ;現在の区画のアドレスが決まれば東西南北の区画も決まる。 ;迷路の端と端がつながっているように見えるが、現実の ;壁を見て行ける領域の記憶しか参照しないので問題はない。 ; ADRSET: LD HL,(ADDR) ;現在位置のアドレス LD BC,10H ADD HL,BC LD (ADDRN),HL ;10Hを加算して(ADDRN)に格納 LD HL,(ADDR) ;現在位置のアドレス INC HL LD (ADDRE),HL ;1Hを加算して(ADDRE)に格納 LD HL,(ADDR) ;現在位置のアドレス LD BC,10H SBC HL,BC LD (ADDRS),HL ;10Hを減算して(ADDRS)に格納 LD HL,(ADDR) ;現在位置のアドレス DEC HL LD (ADDRW),HL ;1Hを減算して(ADDRW)に格納 RET ; ; アドレスシフト ; ;ADRSETで四方の区画のアドレスが求まったが、これはマウス ;自身の感覚とはずれがある。これを一致させるためには、 ;マウスが向いている方向を「北」であるというようにアドレスを ;変換する必要がある。 ADRSIF: LD HL,(ADDRN) LD (ADDR0),HL LD HL,(ADDRE) LD (ADDRN),HL LD HL,(ADDRS) LD (ADDRE),HL LD HL,(ADDRW) LD (ADDRS),HL LD HL,(ADDR0) LD (ADDRW),HL RET ; ;******************************************************* ; 一区間前進サブルーチン ;******************************************************* KUKAN: ; ; スタート時加速(前センサ監視つき) ; ACR: LD HL,MPDATA LD B,40 AC2: LD A,0D5H OUT (CTC1),A OUT (CTC3),A LD A,(HL) OUT (CTC1),A OUT (CTC3),A ACW2: JP HIT ;衝突チェック ACW3: IN A,(MAESEN) ;前壁チェック BIT 5,A JP NZ,ACW4 PUSH BC LD B,00001000b LD A,(WDATA) OR B LD (WDATA),A ;前壁ありフラグセット POP BC JP BRR ;前壁あったら減速 ACW4: BIT 4,A JP NZ,AC3 PUSH BC LD B,00001000b LD A,(WDATA) OR B LD (WDATA),A ;前壁ありフラグセット POP BC JP BRR ;前壁あったら減速 AC3: LD A,(IFR) OR A JP Z,AC3 LD A,0 LD (IFR),A LD (IFL),A INC HL LD A,B OR A JP NZ,AC2 LD A,(WDATA) ;壁データ初期化 AND 11110000b LD (WDATA),A JP CNR1 HIT: PUSH BC ;衝突チェック、衝突したら後退 IN A,(SEN) CPL AND 00011000b CP 00011000b JP NZ,NOHIT ;衝突なければ何もしない CALL STIME LD A,KOUTAI LD (HOKO),A LD (MOTD),A HACR: LD HL,MPDATA HAC2: LD A,0D5H OUT (CTC1),A OUT (CTC3),A LD A,(HL) OUT (CTC1),A OUT (CTC3),A LD B,00001000b LD A,(WDATA) OR B LD (WDATA),A ;前壁ありフラグセット HACR2: LD HL,MPDATA LD B,9 ;9パルス加速 HAC22: LD A,0D5H OUT (CTC1),A OUT (CTC3),A LD A,(HL) OUT (CTC1),A OUT (CTC3),A HAC32: LD A,(IFR) OR A JP Z,HAC32 LD A,0 LD (IFR),A LD (IFL),A INC HL LD A,B OR A JP NZ,HAC22 HBRR: LD B,9 ;減速9パルスで HBR2: LD A,0D5H OUT (CTC1),A OUT (CTC3),A LD A,(HL) OUT (CTC1),A OUT (CTC3),A HBR3: LD A,(IFR) OR A JP Z,HBR3 LD A,0 LD (IFR),A LD (IFL),A DEC HL HBR_E: LD A,B OR A JP NZ,HBR2 CALL STIME ;しばらく停止 HCHK12: IN A,(SEN) ;横壁データ入力 CPL AND 11110000b JP Z,HCHK22 LD B,00000100b ;右壁ありフラグセット LD A,(WDATA) OR B LD (WDATA),A JP HCHK32 HCHK22: LD B,11111011b ;右壁なしフラグセット LD A,(WDATA) AND B LD (WDATA),A HCHK32: IN A,(SEN) CPL AND 00001111b JP Z,HCHK42 LD A,(WDATA) LD B,00000001b ;左壁ありフラグセット OR B LD (WDATA),A JP HITEND HCHK42: LD A,(WDATA) LD B,11111110b ;左壁なしフラグセット AND B LD (WDATA),A HITEND: POP BC LD A,1 ;修正したフラグを1にする LD (SHUSEI),A RET NOHIT: POP BC JP ACW3 ; ; 定速走行〜減速ルーチン ; 定速走行(1区間分) ; CNR1: LD B,74 ;一回目の横壁チェックまでのパルス CN11: CALL WCHER ;横位置修正 IN A,(MAESEN) ;前壁チェックは毎回する BIT 5,A JP NZ,CN12 PUSH BC LD B,00001000b LD A,(WDATA) OR B LD (WDATA),A ;前壁ありフラグセット POP BC JP BRR ;前壁あったら減速 CN12: BIT 4,A JP NZ,CN13 PUSH BC LD B,00001000b LD A,(WDATA) OR B LD (WDATA),A ;前壁ありフラグセット POP BC JP BRR ;前壁あったら減速 CN13: LD A,(IFR) OR A JP Z,CN13 LD A,0 LD (IFR),A LD (IFL),A LD A,B OR A JP NZ,CN11 ; ; 1回目の横壁チェック ; CHK1: LD A,00000000b ;フラグ初期化 LD (WDATA1),A IN A,(SEN) CPL AND 11110000b JP Z,CHK2 LD B,00000100b ;右壁ありフラグセット LD A,(WDATA1) OR B LD (WDATA1),A JP CHK3 CHK2: LD B,11111011b ;右壁なしフラグセット LD A,(WDATA1) AND B LD (WDATA1),A CHK3: IN A,(SEN) CPL AND 00001111b JP Z,CHK4 LD A,(WDATA1) LD B,00000001b ;左壁ありフラグセット OR B LD (WDATA1),A JP CHK5 CHK4: LD A,(WDATA1) LD B,11111110b ;左壁なしフラグセット AND B LD (WDATA1),A CHK5: ; ; 定速走行(1区間分)つづき ; CNR12: LD B,20 ;二回目の横壁チェックまでのパルス CN112: CALL WCHER ;横位置修正 IN A,(MAESEN) ;前壁チェックは毎回する BIT 5,A JP NZ,CN122 PUSH BC LD B,00001000b LD A,(WDATA) OR B LD (WDATA),A ;前壁ありフラグセット POP BC JP BRR ;前壁あったら減速 CN122: BIT 4,A JP NZ,CN132 PUSH BC LD B,00001000b LD A,(WDATA) OR B LD (WDATA),A ;前壁ありフラグセット POP BC JP BRR ;前壁あったら減速 CN132: LD A,(IFR) OR A JP Z,CN132 LD A,0 LD (IFR),A LD (IFL),A LD A,B OR A JP NZ,CN112 ; ; 2回目の横壁チェック ; CHK12: LD A,00000000b ;フラグ初期化 LD (WDATA2),A IN A,(SEN) CPL AND 11110000b JP Z,CHK22 LD B,00000100b ;右壁ありフラグセット LD A,(WDATA2) OR B LD (WDATA2),A JP CHK32 CHK22: LD B,11111011b ;右壁なしフラグセット LD A,(WDATA2) AND B LD (WDATA2),A CHK32: IN A,(SEN) CPL AND 00001111b JP Z,CHK42 LD A,(WDATA2) LD B,00000001b ;左壁ありフラグセット OR B LD (WDATA2),A JP CHK52 CHK42: LD A,(WDATA2) LD B,11111110b ;左壁なしフラグセット AND B LD (WDATA2),A CHK52: LD A,(WDATA1) LD B,A LD A,(WDATA2) AND B ;一回目と二回目の結果をANDして(WDATA)とORする LD B,A LD A,(WDATA) OR B LD (WDATA),A ; ; 定速走行(1区間分)さらにつづき ; CNR13: LD B,56 ;減速位置までのパルス CN113: CALL WCHER ;横位置修正 IN A,(MAESEN) ;前壁チェックは毎回する BIT 5,A JP NZ,CN123 PUSH BC LD B,000001000b LD A,(WDATA) OR B LD (WDATA),A ;前壁ありフラグセット POP BC JP BRR ;前壁あったら減速 CN123: BIT 4,A JP NZ,CN133 PUSH BC LD B,00001000b LD A,(WDATA) OR B LD (WDATA),A ;前壁ありフラグセット POP BC JP BRR ;前壁あったら減速 CN133: LD A,(IFR) OR A JP Z,CN133 LD A,0 LD (IFR),A LD (IFL),A LD A,B OR A JP NZ,CN113 ; ; 減速停止 ; BRR: LD B,32 BR2: LD A,0D5H OUT (CTC1),A OUT (CTC3),A LD A,(HL) OUT (CTC1),A OUT (CTC3),A BR3: LD A,(IFR) OR A JP Z,BR3 LD A,0 LD (IFR),A LD (IFL),A DEC HL BR_E: LD A,B OR A JP NZ,BR2 ; ; しばらく止まるルーチン ; STIME: LD A,0D5H OUT (CTC1),A OUT (CTC3),A LD A,0 ;0を入れると割り込み信号が出ないのを利用する OUT (CTC1),A OUT (CTC3),A LD BC,0C000H ST1: DEC C JP NZ,ST1 DJNZ ST1 RET ;******************************************************* ; 細かいサブルーチン ;******************************************************* ; ; スタート時加速ルーチン(センサ監視なし) ; ACR2: LD HL,MPDATA LD B,40 AC22: LD A,0D5H OUT (CTC1),A OUT (CTC3),A LD A,(HL) OUT (CTC1),A OUT (CTC3),A AC32: LD A,(IFR) OR A JP Z,AC32 LD A,0 LD (IFR),A LD (IFL),A INC HL LD A,B OR A JP NZ,AC22 RET ; ; 90度右回転 ; R_TURN: LD A,USETU LD (HOKO),A LD (MOTD),A CALL ACR2 CALL TRN1 CALL BRR RET ; ; 90度左回転 ; L_TURN: LD A,SASETU LD (HOKO),A LD (MOTD),A CALL ACR2 CALL TRN1 CALL BRR RET ; ; 180度右回転 ; U_TURN: LD A,USETU LD (HOKO),A LD (MOTD),A CALL ACR2 CALL TRN2 CALL BRR RET ; ; 1/4周分定速度回転 ; TRN1: LD B,15 ;1/4周分定速回転理論値18 TRN11: LD A,0D5H OUT (CTC1),A OUT (CTC3),A LD A,(HL) OUT (CTC1),A OUT (CTC3),A TRN12: LD A,(IFR) OR A JP Z,TRN12 LD A,0 LD (IFR),A LD (IFL),A LD A,B OR A JP NZ,TRN11 RET ; ; 1/2周分定速度回転 ; TRN2: LD B,107 ;1/4周分定速回転108 TRN21: LD A,0D5H OUT (CTC1),A OUT (CTC3),A LD A,(HL) OUT (CTC1),A OUT (CTC3),A TRN22: LD A,(IFR) OR A JP Z,TRN22 LD A,0 LD (IFR),A LD (IFL),A LD A,B OR A JP NZ,TRN21 RET ; ; パルス発生のためのソフトタイマー ; TIMER: PUSH BC LD B,2 DJNZ $ POP BC RET ;***************************************************** ; 定速回転時横センサによる位置修正 ; 考え方 ;横センサ配置 PB0 PB1 PB2 PB3 PB4 PB5 PB6 PB7 ; 左 右 ; IN A,(SEN) ; CPL ; という動作を行ったとき ;左センサ 右センサ 意味 ;×○○× 6h ×○○× 60h 中央 ;×○×× 2h ×○×× 20h すこし右 ;○○×× 3h ○○×× 30h やや右 ;○××× 1h ○××× 10h かなり右 ;×××× 0 ×××× 0 はずれ ;××○× 4h ××○× 40h すこし左 ;××○○ 0Ch ××○○ 0C0h やや左 ;×××○ 8h ×××○ 80h かなり左 ;中央 何もしない (CTC1←28、CTC3←28) ;少し右 小修正 (CTC1←28、CTC3←29) ;やや右 中修正 (CTC1←28、CTC3←30) ;かなり右大修正 (CTC1←28、CTC3←32) ;はずれ 何もしない (CTC1←28、CTC3←28) ;ただし「壁あり→はずれ」時に壁データ修正イベント発生 ;少し左 小修正 (CTC1←29、CTC3←28) ;やや左 中修正 (CTC1←30、CTC3←28) ;かなり左大修正 (CTC1←32、CTC3←28) ;右壁あり:右壁センサで修正 ;右壁なし:左壁センサで修正 ;両方壁なし:修正しない ; ; ; 位置修正壁チェックルーチン ; WCHER: IN A,(SEN) ;右壁入力 CPL AND 11110000b JP Z,RWCHE ;右壁なしで左壁チェック BIT 6,A JP Z,WC1 BIT 5,A JP NZ,NONCR ;中央で修正なし WC1: CP 20H JP Z,LC1 ;少し右 CP 30H JP Z,LC2 ;やや右 CP 10H JP Z,LC3 ;かなり右 CP 40H JP Z,RC1 ;少し左 CP 0C0H JP Z,RC2 ;やや左 CP 80H JP Z,RC3 ;かなり左 JP NONCR ;いずれでもなければ修正なし RWCHE: IN A,(SEN) ;左壁入力 CPL AND 00001111b JP NZ,RWCHE2 JP NONCR RWCHE2: CP 6H JP Z,NONCR ;中央で修正なし CP 2H JP Z,LC1 ;少し右 CP 3H JP Z,LC2 ;やや右 CP 1H JP Z,LC3 ;かなり右 CP 4H JP Z,RC1 ;少し左 CP 0CH JP Z,RC2 ;やや左 CP 8 JP Z,RC3 ;かなり左 JP NONCR ;いずれでもなければ修正なし ; ; 右修正ルーチン ; RC1: LD A,0D5H OUT (CTC1),A OUT (CTC3),A LD A,28 ;左モータ回転速度 OUT (CTC1),A LD A,29 ;右モータ回転速度 OUT (CTC3),A RC11: LD A,(IFR) OR A JP Z,RC11 LD A,0 LD (IFR),A RET ; RC2: LD A,0D5H OUT (CTC1),A OUT (CTC3),A LD A,28 ;左モータ回転速度 OUT (CTC1),A LD A,30 ;右モータ回転速度 OUT (CTC3),A JP RC11 ; RC3: LD A,0D5H OUT (CTC1),A OUT (CTC3),A LD A,28 ;左モータ回転速度 OUT (CTC1),A LD A,33 ;右モータ回転速度 OUT (CTC3),A JP RC11 ; ; 修正なし ; ; NONCR: LD A,0D5H OUT (CTC1),A OUT (CTC3),A LD A,28 ;右左同じ回転数 OUT (CTC1),A OUT (CTC3),A NON1: LD A,(IFR) OR A JP Z,NON1 LD A,0 LD (IFR),A RET ; ; 左修正ルーチン ; LC1: LD A,ZENSHIN OUT (MOT),A LD A,0D5H OUT (CTC1),A OUT (CTC3),A LD A,29 ;左モータ回転速度 OUT (CTC1),A LD A,28 ;右モータ回転速度 OUT (CTC3),A LC11: LD A,(IFR) OR A JP Z,LC11 LD A,0 LD (IFR),A RET ; LC2: LD A,0D5H OUT (CTC1),A OUT (CTC3),A LD A,30 ;左モータ回転速度 OUT (CTC1),A LD A,28 ;右モータ回転速度 OUT (CTC3),A JP LC11 ; LC3: LD A,0D5H OUT (CTC1),A OUT (CTC3),A LD A,33 ;左モータ回転速度 OUT (CTC1),A LD A,28 ;右モータ回転速度 OUT (CTC3),A JP LC11 ; ;*************************************************** ; CPU INITIALIZE ROUTINE ;*************************************************** CPUINI: IM 2 LD A,81H LD I,A RET ; PIOINI: LD A,0CFh ;モード3 OUT (PBC),A LD A,11111111b OUT (PBC),A ;PB全部入力 LD A,0CFh OUT (PAC),A LD A,00000011b OUT (PAC),A ;PA7〜PA2出力、PA1PA0入力 RET ; CTCINI: LD A,0 OUT (CTC0),A LD A,15H ;CTC#0のイニシャライズ OUT (CTC0),A LD A,100 ;回転速度基準時間100標準 OUT (CTC0),A RET ;******************************************************** ; 割り込み処理 ;******************************************************** INTR0: EI RETI ; INTR1: PUSH AF ;左モータに1パルス出力 PUSH BC LD A,(HOKO) OR 01000000b LD B,A LD A,(MOTD) OR B LD (MOTD),A OUT (MOT),A AND 10111111b OUT (MOT),A LD A,1 LD (IFL),A POP BC POP AF EI RETI ; INTR2: EI RETI ; INTR3: PUSH AF ;右モータに1パルス出力 PUSH BC LD A,(HOKO) OR 00010000b LD B,A LD A,(MOTD) OR B LD (MOTD),A OUT (MOT),A AND 11101111b OUT (MOT),A LD A,1 LD (IFR),A POP BC POP AF DEC B EI RETI ;******************************************** ; 加減速パルス間隔データ ;******************************************** DEFB 148,148,148,148,148 DEFB 148,148,148,148,148 DEFB 148,148,148,148,148 DEFB 148,148,148,148,148 DEFB 148,148,148,148,148 DEFB 148,148,148,148,148 DEFB 148,148 MPDATA: DEFB 148,148,148 DEFB 148,136,124,112,104 DEFB 96,84,72,68,60 DEFB 56,56,52,48,48 DEFB 44,44,40,40,40 DEFB 40,36,36,36,36 DEFB 32,32,32,32,32 DEFB 30,30,30,30,30 DEFB 28,28,28,28,28 DEFB 26,26,26,26,26 DEFB 24,24,24,24,24 DEFB 24,24,24,24,24 ;******************************************* ; 壁データ記憶領域 ;******************************************* YOBI: DS 32 MAP: DS 256 YOBI2: DS 32 END