USE	Labdefs.s12

Outzero:	CMPB	#7
		BGT	return1
		PSHD
		LDAA	#1
		CMPB	#0
loop1:		BEQ	break1
		LSLA
		DECB
		BRA	loop1
break1:		COMA
		ANDA	DCShadow
		STAA	DCShadow
		STAA	DrillControl
		PULD
return1:	RTS



Outone:		CMPB	#7
		BGT	return2
		PSHD
		LDAA	#1
		CMPB	#0
loop2:		BEQ	break2
		LSLA
		DECB
		BRA	loop2
break2:		ORAA	DCShadow
		STAA	DCShadow
		STAA	DrillControl
		PULD
return2:	RTS



MotorStart:	PSHB
		LDAB	DCShadow
		BITB	#%00000100
		BNE	return3
		LDAB	#2	; turn on engine
		JSR	Outone
		JSR	Delay
		JSR	Delay
		JSR	Delay
		JSR	Delay
return3:	PULB
		RTS



MotorStop:	PSHB
		LDAB	#2
		JSR	Outzero
		PULB
		RTS



DrillDown:	PSHB
		LDAB	#3
		JSR	Outone
		PULB
		RTS



DrillUp:	PSHB
		LDAB	#3
		JSR	Outzero
		JSR	Delay
		PULB
		CMPB	#0
		RTS



Alarm:		PSHB
AlarmLoop:	CMPB	#0
		BEQ	AlarmRet
		PSHB
		LDAB	#4
		JSR	Outone
		JSR	Delay	; 1000 ms delay
		JSR	Delay
		JSR	Delay
		JSR	Delay
		JSR	Outzero
		JSR	Delay	; 500 ms delay
		JSR	Delay
		PULB
		DECB
		BRA	AlarmLoop
AlarmRet:	PULB
		RTS



Step:		LDAB	DrillStatus
		BITB	#%00000010
		BEQ	NotInTop ; inte i toppläge
		LDAB	#1
		JSR	Outone
		LDAB	#0
		JSR	Outone
		JSR	Delay
		JSR	Delay
		JSR	Outzero
		LDAB	#1
		RTS
NotInTop:	LDAB	#2
		JSR	Alarm
		LDAB	#0
		RTS



DrillDownTest:	LDAB	#20
loopD:		LDAA	DrillStatus
		BITA	#%00000100
		BNE	IsDown
		JSR	Delay
		DECB
		BNE	loopD
		LDAB	#2
		JSR	Alarm
		LDAB	#0
		RTS
IsDown:		LDAB	#1
		RTS



DrillHole:	JSR	DrillDown
		JSR	DrillDownTest
		JSR	DrillUp
		RTS



RefPos:		LDAA	DrillStatus
		BITA	#%00000001
		BNE	RefPosRet
		JSR	Step
		CMPB	#0
		BNE	RefPos
		RTS
RefPosRet:	LDAB	#1
		RTS



Nstep:		CMPB	#0
		BEQ	NstepRet
		DECB
		PSHB
		JSR	Step
		CMPB	#0
		PULB
		BNE	Nstep
		LDAB	#0
		RTS
NstepRet:	LDAB	#1
		RTS



DoAuto:		LDX	#Pattern
		JSR	Auto
		RTS



Auto:		JSR	RefPos
		BEQ	AutoRet
		JSR	MotorStart
AutoLoop:	LDAB	,X
		INX
		CMPB	#$FF
		BEQ	AutoRet
		JSR	Nstep
		BEQ	AutoRet
		JSR	DrillHole
		BNE	AutoLoop
AutoRet:	JSR	MotorStop
		RTS