;selfwork ちょろQ 2011/02/17 retestd を改良
;停止時逆転を挿入　2011/04/24 smartA2
;停止時逆転を修正　2011/04/25 smartA3
;BKDRIVE挿入　2011/06/19
;;停止確認program 2011/06/19


include 16f84.h
.osc hs
.wdt off

inp_0 = ra.0 ;portA_1bit
inp_1 = ra.1
inp_2 = ra.2
inp_3 = ra.3 ;portA_3bit

org 0ch
tim0 ds 1 ;tim0
tim1 ds 1 ;tim1〜tim2 を変数として使う
tim2 ds 1
count ds 1 ;宣言
countf ds 1 ;前進カウンター
countb ds 1 ;後退カウンター
org 0
goto start
start
clr ra
clr rb
mov !ra,#01111b ;port ra input
mov !rb,#0 ;port rb output

main
call start_flsh ;フラッシュ　スタート


ruoop
mov w,#00000001b ;前進スタート
mov rb,w
mov tim0,#26
call frdrive


ropc mov w,#00000110b ;左曲がり後退
mov rb,w
mov tim0,#25
call bkdrive ;BKDRIVE挿入

ropd mov w,#00000101b ;右曲がり前進　修正
mov rb,w
mov tim0,#30
call frdrive

mov w,#00000101b ;左曲り前進
mov rb,w
mov tim0,#40
call frdrive

roopb mov w,#00001001b ;右曲がり前進
mov rb,w
mov tim0,#40
call frdrive

fropa mov w,#00000110b ;右曲がり後退
mov rb,w
mov tim0,#35
call bkdrive ;BKDRIVE挿入

fropb mov w,#00000101b ;右曲り前進　　修正
mov rb,w
mov tim0,#50
call frdrive

fropc mov w,#00001001b ;左曲り前進
mov rb,w
mov tim0,#50
call frdrive


goto ruoop



stop mov w,#00000000b ;停止
mov rb,w
mov tim0,#7
call wait

goto stop ;ropc


wait
wa0 clr tim1
wa1 mov tim2,80
wa2 nop
djnz tim2,wa2
djnz tim1,wa1
djnz tim0,wa0
ret

frdrive
frdrive0 clr tim1
frdrive1 mov tim2,#50
frdrive2 call frsens_ck
djnz tim2,frdrive2
djnz tim1,frdrive1
djnz tim0,frdrive0
ret


bkdrive
bkdrive0 clr tim1
bkdrive1 mov tim2,#50
bkdrive2 call bksens_ck
djnz tim2,bkdrive2
djnz tim1,bkdrive1
djnz tim0,bkdrive0
ret



frsens_ck snb inp_0
call sub_0
snb inp_1
call sub_1


bksens_ck snb inp_2
call sub_2
snb inp_3
call sub_3
ret

;センサー0　　後　左　検出を検出時の処理
sub_0 mov w,#00000110b ;後退　右曲　ハンドル
mov rb,w
mov tim0,#10 ;wait 10 set
call wait
mov w,#00000010b ;後退　右曲り 直進後退に修正
mov rb,w
mov tim0,#10 ;wait10
call bkdrive
goto ruoop ;ruoop 前進　へ行く ( 直し済み )


sub_1 mov w,#00001010b ;センサー1　前　の右　を検出した時の処理
mov rb,w ;左ハンドル
mov tim0,#10 ;wait set
call wait
mov w,#00000110b ;ハンドルを右に切り換える
mov rb,w ;左ハンドル
mov tim0,#10 ;wait set
call wait
goto ropc ;goto ropb　後退 ( 直し済み )

sub_2 mov w,#00001001b ;センサー２　前左　検出時の処理 　修正
mov rb,w
mov tim0,#25 ;前進　右ハンドル
call wait
mov w,#00000101b ;センサー２　前左　検出時の処理 修正
mov rb,w
mov tim0,#15 ;後退　右ハンドル
call frdrive
goto ropd ;前進 ( 直し済み )


sub_3 mov w,#00000101b ;センサー３　後左　検出時の処理 修正
mov rb,w
mov tim0,#25
call frdrive ;前進
mov w,#00000101b ;センサー３　後右　検出時の処理
mov rb,w
mov tim0,#15 ;wait set
call frdrive
goto roopb ;ropb 後退　へ行く


start_flsh
mov count,#10
st_fls mov rb,#10000000b
mov tim0,#5
call wait
mov rb,#00000000b
mov tim0,#5
call wait
djnz count,st_fls
ret

bkstp mov w,#00000010b ;frdrive
mov rb,w
mov tim0,#15
call wait
mov w,#00000001b ;bkdrive
mov tim0,#5
call wait
ret

frstp mov w,#00000001b
mov rb,w
mov tim0,#15
call wait
mov w,#00000010b
mov rb,w
mov tim0,#5
call wait
ret



frsens
snb inp_0
call fsub_0 ;sub_0へ行く
snb inp_1
call fsub_1 ;sub_1へ行く
ret


bksens
snb inp_2
call bsub_2 ;sub_2へ行く
snb inp_3
call bsub_3 ;sub_3へ行く
ret

frnt_brk mov w,#00000010b ;前進時ブレーキソフト
mov rb,w
mov tim0,#5
call wait
ret



fsub_0 mov rb,#10000000b
goto fsub_0

fsub_1 mov rb,#01000000b
goto fsub_1

bsub_2 mov rb,#00100000b
goto bsub_2

bsub_3 mov rb,#00010000b
goto bsub_3

---

---