Cordic ATAN2 in 6502 (for fun)

6502_ATAN2.s

; DASM assembler

processor  6502

	seg.u   zpage
	ORG $0080


theta   ds 2
x	   ds 2
y	   ds 2
rt	  ds 2
i	   ds 1
ox	  ds 1
oy	  ds 1

HALF equ 32768
MAXL equ 8

	seg rom
	org $f000

Start	
	
	lda #<$190
	sta x
	lda #>$190
	sta x+1

	lda #<0
	sta y
	lda #>0
	sta y+1
	
	jsr atan2
	
	jmp Start
	
	
atan2
	lda x+1
	sta ox
	lda y+1
	sta oy
	
  ;x * 16
	asl x
	rol x+1
	asl x
	rol x+1
	asl x
	rol x+1
	asl x
	rol x+1

  ;y * 16
	asl y
	rol y+1
	asl y
	rol y+1
	asl y
	rol y+1
	asl y
	rol y+1
	
	lda #0
	sta theta
	sta theta+1
	
	lda y+1
	bpl .ypositive

  ; y = -y
	lda #0
	sec
	sbc y
	sta y
	lda #0
	sbc y+1
	sta y+1

  ; x = -x
	lda #0
	sec
	sbc x
	sta x
	lda #0
	sbc x+1
	sta x+1
	
.ypositive	

	lda x+1
	bpl .xpositive
	
  ; ytemp = y
	ldx y
	ldy y+1
	
  ; y = -x
	lda #0
	sec
	sbc x
	sta y
	lda #0
	sbc x+1
	sta y+1
	
  ; x = ytemp
	stx y
	sty y+1

.xpositive

  ;------------------
  ; 

	ldx #0
	
.cordicLoop	
	stx i
	
	lda y+1
	bpl .rotateNegative
	
  ;rotate positive
	
	lda x
	sta rt
	lda x+1
	sta rt+1
	ldx i
	beq .noshiftxp
	jsr asrn
.noshiftxp

	ldx y
	ldy y+1
	
	txa
	clc
	adc rt
	sta y
	tya
	adc rt+1
	sta y+1
	
	stx rt
	sty rt+1
	
	ldx i
	beq .noshiftyp
	jsr asrn
.noshiftyp

	lda x
	sec
	sbc rt
	sta x
	lda x+1
	sbc rt+1
	sta x+1

	ldx i
	lda theta
	sec
	sbc atantabL,x
	sta theta
	lda theta+1
	sbc atantabH,x
	sta theta+1

	inx
	cpx #MAXL
	bcc .cordicLoop
	bcs .done
	
.rotateNegative

	lda x
	sta rt
	lda x+1
	sta rt+1
	ldx i
	beq .noshiftxn 
	jsr asrn
.noshiftxn
	ldx y
	ldy y+1
	
	txa
	sec
	sbc rt
	sta y
	tya
	sbc rt+1
	sta y+1
	
	stx rt
	sty rt+1
	ldx i
	beq .noshiftyn   
	jsr asrn
.noshiftyn	
	lda x
	clc
	adc rt
	sta x
	lda x+1
	adc rt+1
	sta x+1

	ldx i
	lda theta
	clc
	adc atantabL,x
	sta theta
	lda theta+1
	adc atantabH,x
	sta theta+1
	
	inx
	cpx #MAXL
	bcs .done
	jmp .cordicLoop

.done	
  ; if original y was negative
	lda oy
	bpl .oyplus
	lda theta+1
	clc
	adc #$80
	sta theta+1
.oyplus	
	
  ; if original x was negative
	lda ox
	bpl .oxplus
	lda theta+1
	clc
	adc #$40
	sta theta+1
.oxplus

	rts

; arithmetic shift right xtl and xth by X
asrn
	lda rt+1
.shiftr
	cmp #$80
	ror
	ror rt
	dex
	bne .shiftr
	sta rt+1
	rts
	
	
; arithmetic shift left xtl and xth by X
asln
	lda rt+1
.shiftl
	asl rt
	rol
	dex
	bne .shiftl
	sta rt+1
	rts
	  
 
atantabL
	.byte <8192, <4836, <2555, <1297, <651, <326, <163, <81, <41, <20, <10, <5, <3, <1, <1
atantabH	
	.byte >8192, >4836, >2555, >1297, >651, >326, >163, >81, >41, >20, >10, >5, >3, >1, >1
	
	
 ORG $FFF8
 .word 0	  ; for supercharger
 .word 0	  ; nmi vector
 .word Start  ; start vector
 .word Start  ; brk vector