Commodore 1571 firmware
Motor and stepper control
.page
.subttl 'lccend.src'
;
;
;
; motor and stepper control
;
;
; irq into controller every 8.224 ms
end
lda t1hl2 ; set irq timer
sta t1hc2
;
lda dskcnt
and #$10 ; test write proctect
cmp lwpt
sta lwpt ; change ?
;<><><><><><><><><><><><><><><><><><><><><><><><><><><><><><>
jmp ptch21 ; do auto on code *** rom ds 85 ***
nop ; fill
nop ; fill
nop ; fill
; beq end002 ; no
; lda #1 ; yes, set flag
; sta wpsw
;
rtch21 ; return
;<><><><><><><><><><><><><><><><><><><><><><><><><><><><><><>
end002 lda phase ; test for phase offset
beq end40
;
cmp #2
bne end003
;
lda #0 ; phase <-- 0
sta phase
beq end40
;
end003 sta steps
lda #2 ; phase <-- 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
dec acltim2 ; timer2 *rom-05ds 85*
bne end30
nop ; fill
jsr motoff ; off the motor, ok...
;
; 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 #&g;ssacl
sta nxtst+1
;
dostep lda steps
bpl stpin
;
;---------rom05-bc 84--------
;stpout inc steps
; ldx dskcnt
; dex
; jmp stp
stpout jmp patch9 ; check track 0 (photo sensor)
nop ;
nop ;
nop ;
pppppp jmp stp ; goto step
;----------------------------------
;
short lda steps ; step end ?
bne dostep ; no
;
lda #<setle ; settle
sta nxtst
lda #>setle
sta nxtst+1
lda #5 ; settle time (5*8=40ms)
sta aclstp
jmp end33
;
setle dec aclstp ; settle end ?
bne end33 ; no
;
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