lcc.end

.PAGE 'LCC.END'
;
;
;
;   MOTOR AND STEPPER CONTROL
;
;
;   IRQ INTO CONTROLLER EVERY 10 MS
END
	LDA T1HL2
	STA T1HC2
;
	LDA DSKCNT
;
END001
	AND #$10        ; TEST WRITE PROCTECT
	CMP LWPT
	STA LWPT        ; CHANGE ?
	BEQ END002      ; NO
;
	LDA #1          ; YES, SET FLAG
	STA WPSW
;
END002	LDA PHASE       ; TEST FOR PHASE OFFSET
	BEQ END40
;
	CMP #2
	BNE END003
;
	LDA #0
	STA PHASE
	BEQ END40
;
END003	STA STEPS
	LDA #2
	STA PHASE
	JMP DOSTEP
;
END40	LDX CDRIVE      ; WORK ON ACTIVE DRIVE ONLY
	BMI END33X      ; NO ACTIVE DRIVE
;
	LDA DRVST       ; TEST IF MOTOR ON
	TAY
	CMP #$20        ; TEST IF ANYTHING TO DO
	BNE END10       ; SOMETHING HERE
;
END33X	JMP END33       ; MOTOR JUST RUNNING
;
END10	DEC ACLTIM      ; DEC TIMER
	BNE END30
;
	TYA             ; TEST IF ACEL
	BPL END20
;
;
	AND #$7F        ; OVER, CLEAR ACEL BIT
	STA DRVST
;
END20	AND #$10        ; TEST IF TIME OUT STATE
	BEQ END30
;
	LDA DSKCNT
	AND #$FF-$04    ; TURNOFF MOTOR
	STA DSKCNT
;
;
	LDA #$FF        ; NO ACTIVE DRIVE NOW
	STA CDRIVE
;
	LDA #0          ; DRIVE INACTIVE
	STA DRVST       ; CLEAR ON BIT AND TIMOUT
	BEQ END33X
;
END30	TYA             ; TEST IF STEP NEEDED
	AND #$40
	BNE END30X      ; STEPPING
;
	JMP END33
;
;
END30X	JMP (NXTST)     ;GOTO PROPER STEPPER STATE
;
INACT	LDA STEPS       ; GET ABS(STEPS)
	BPL INAC10
;
	EOR #$FF
	CLC
	ADC #1
;
INAC10	CMP MINSTP      ; TEST IF WE CAN ACCEL
	BCS INAC20      ; TOO SMALL
;
	LDA #<SHORT     ;SHORT STEP MODE
	STA NXTST
	LDA #>SHORT
	STA NXTST+1
	BNE DOSTEP
;
INAC20	; CALC THE # OF RUN STEPS
	SBC AS
	SBC AS
	STA RSTEPS
;
	LDA AS
	STA ACLSTP      ; SET  # OF ACCEL STEPS
	LDA #<SSACL
	STA NXTST
	LDA #>SSACL
	STA NXTST+1
;
DOSTEP	LDA STEPS
	BPL STPIN
;
STPOUT	INC STEPS
	LDX DSKCNT
	DEX
	JMP STP
;
SHORT	LDA STEPS
	BNE DOSTEP
;
	LDA #<SETLE
	STA NXTST
	LDA #>SETLE
	STA NXTST+1
	LDA #5          ; SETTLE TIME
	STA ACLSTP
	JMP END33
;
SETLE	DEC ACLSTP
	BNE END33
;
	LDA DRVST
	AND #$FF-$40
	STA DRVST
;
	LDA #<INACT
	STA NXTST
	LDA #>INACT
	STA NXTST+1
	JMP END33
;
STPIN	DEC STEPS
	LDX DSKCNT
	INX
;
STP	TXA
	AND #3
	STA TMP
	LDA DSKCNT
	AND #$FF-$03    ; MASK OUT OLD
	ORA TMP
	STA DSKCNT
	JMP END33
;
SSACL	                ; SUB ACEL FACTOR
	SEC
	LDA T1HL2
	SBC AF
	STA T1HC2
;
	DEC ACLSTP
	BNE SSA10
;
	LDA AS
	STA ACLSTP
;
	LDA #<SSRUN
	STA NXTST
	LDA #>SSRUN
	STA NXTST+1
;
SSA10	JMP DOSTEP
;
SSRUN	DEC RSTEPS
	BNE SSA10
;
	LDA #<SSDEC
	STA NXTST
	LDA #>SSDEC
	STA NXTST+1
	BNE SSA10
;
SSDEC	                ; DECEL
	LDA T1HL2
	CLC
	ADC AF
	STA T1HC2
;
	DEC ACLSTP
	BNE SSA10
;
	LDA #<SETLE     ; GOTO SETTLE MODE
	STA NXTST
	LDA #>SETLE
	STA NXTST+1
;
	LDA #5
	STA ACLSTP      ; SETTLE TIMER
;
;
END33	LDA PCR2        ; DISABLE S.O. TO 6502
	AND #$FF-$02
	STA PCR2
;
	RTS
;
;
;
.END