Code:
;*******************************************************************************
; --- CORDIC TRIG LIBRARY ---
;http://www.chiefdelphi.com/media/papers/2016
; FILE NAME: trig.asm
; AUTHOR: Patrick Fairbank
; LAST MODIFIED: Jan. 16, 2008
;
; DESCRIPTION: This file contains functions implementing the COORDIC
; algorithm.
;
; USAGE: Add this file to your project.
;
; LICENSE: Users are free to use, modify, and distribute this code
; as they see fit.
;
; sin_cos: input ang, output x = sin and y = cos
;
;
; atan2_sqrt: input x and y coordinates
; Output the calculated angle and hypotenuse values
; as output: ang = angle x = hypotenuse
;
;******************************************************************************/
i var byte
j Var byte
quad var byte
x var word
y var word
ang var word
dy var word
dx var word
goto main
asm
;IDATA
; Table of arctan values
atans DW D'16384', D'9672', D'5110', D'2594', D'1302', D'652', D'326', D'163'
DW D'81', D'41', D'20', D'10', D'5', D'3', D'1'
;CODE
; Calculates the sine and cosine of the given angle
sin_cos:
; Set up the stack
movff FSR2L, POSTINC1
movff FSR1L, FSR2L
; Initialize _x to 18218
movlw 0x2a
movwf _x
movlw 0x47
movwf _x+1
; Initialize _y to 0
clrf _y
clrf _y+1
; Initialize _ang to passed parameter
movlw 0xfd
movff PLUSW2, _ang
movlw 0xfe
movff PLUSW2, _ang+1
; Initialize _quad to 0
clrf _quad
; Check if the angle is greater than 16383 (90°)
sc_check_greaterthan:
btfss _ang+1, 7
btfss _ang+1, 6
bra sc_check_lessthan
bra sc_adjust_quad2
; Check if the angle is less than -16384 (-90°)
sc_check_lessthan:
btfsc _ang+1, 7
btfsc _ang+1, 6
bra sc_setup_end
; If the angle is in quadrant 3, adjust it to quadrant 4
sc_adjust_quad3:
negf _ang
bc sc_negate_quad3
comf _ang+1
bra sc_adjust_end
; If the low byte negation causes a carry, negate the upper byte
sc_negate_quad3:
negf _ang+1
bra sc_adjust_end
; If the angle is in quadrant 2, adjust it to quadrant 1
sc_adjust_quad2:
comf _ang
comf _ang+1
; Toggle the sign bit and set the '_quad' flag
sc_adjust_end:
btg _ang+1, 7
setf _quad
; Multiply the angle by 2 to get better resolution
sc_setup_end:
bcf STATUS, 0
rlcf _ang
rlcf _ang+1
; Set up the main loop
sc_loop_start:
clrf _i
banksel atans
lfsr FSR0, atans
; The main loop label
sc_loop:
movff _x, _dy
movff _x+1, _dy+1
movff _i, _j
movf _j
bz sc_bs_x_done
; Loop to shift _dy right
sc_bs_x_loop:
bcf STATUS, 0
rrcf _dy+1
rrcf _dy
btfsc _x+1, 7
bsf _dy+1, 7
decfsz _j
bra sc_bs_x_loop
; Calculate what needs to be added to _x
sc_bs_x_done:
movff _y, _dx
movff _y+1, _dx+1
movff _i, _j
movf _j
bz sc_do_rotation
; Loop to shift _dx right
sc_bs_y_loop:
bcf STATUS, 0
rrcf _dx+1
rrcf _dx
btfsc _y+1, 7
bsf _dx+1, 7
decfsz _j
bra sc_bs_y_loop
; Perform adding operations on _x, _y and _ang
sc_do_rotation:
btfss _ang+1, 7
bra sc_sub_angle
; If _ang is negative
movf POSTINC0, W
addwf _ang
movf POSTINC0, W
addwfc _ang+1
movf _dx, W
addwf _x
movf _dx+1, W
addwfc _x+1
movf _dy, W
subwf _y
movf _dy+1, W
subwfb _y+1
bra sc_loop_bottom
; If _ang is positive
sc_sub_angle:
movf POSTINC0, W
subwf _ang
movf POSTINC0, W
subwfb _ang+1
movf _dx, W
subwf _x
movf _dx+1, W
subwfb _x+1
movf _dy, W
addwf _y
movf _dy+1, W
addwfc _y+1
; Increment the counter and exit the loop if done
sc_loop_bottom:
incf _i
movlw 0x0f
cpfseq _i
bra sc_loop
; Negate _x if it was initially in quadrant 2 or 3
sc_finished:
btfss _quad, 7
bra sc_output
negf _x
bc sc_negate_x
comf _x+1
bra sc_output
; If the low byte negation causes a carry, negate the upper byte
sc_negate_x:
negf _x+1
; Output the calculated _x and _y values
sc_output:
;movff _y, AARGB3
;movff _y+1, AARGB3+1
;movff _x, AARGB3+2
;movff _x+1, AARGB3+3
; Restore the stack to its previous state
movf POSTDEC1
movff INDF1, FSR2L
return
; Calculates the magnitude and direction of the given ordered pair
atan2_sqrt:
; Set up the stack
movff FSR2L, POSTINC1
movff FSR1L, FSR2L
; Initialize _x to passed parameter
movlw 0xfb
movff PLUSW2, _x
movlw 0xfc
movff PLUSW2, _x+1
; movff POSTINC2, _x
; movff POSTDEC2, _x+1
; Initialize _y to passed parameter
movlw 0xfd
movff PLUSW2, _y
movlw 0xfe
movff PLUSW2, _y+1
; movlw 0x03
; movff PLUSW2, _y+1
; movlw 0x02
; movff PLUSW2, _y
; Initialize _ang to 0
clrf _ang
clrf _ang+1
; Initialize _quad to 0
clrf _quad
; If the point is in quadrant 2 or 3, make _x positive and set flag
as_check_negative:
btfss _x+1, 7
bra as_shift_x
setf _quad
negf _x
bc as_negate_x
comf _x+1
bra as_shift_x
; If the low byte negation causes a carry, negate the upper byte
as_negate_x:
negf _x+1
; Divide the _x coordinate by 2 to prevent overflowing
as_shift_x:
bcf STATUS, 0
rrcf _x+1
rrcf _x
; Divide the _y coordinate by 2 to prevent overflowing
as_shift_y:
bcf STATUS, 0
rrcf _y+1
rrcf _y
btfsc _y+1, 6
bsf _y+1, 7
; Set up the main loop
as_loop_start:
clrf _i
banksel atans
lfsr FSR0, atans
; The main loop label
as_loop:
movff _x, _dy
movff _x+1, _dy+1
movff _i, _j
movf _j
bz as_bs_x_done
; Loop to shift _dy right
as_bs_x_loop:
bcf STATUS, 0
rrcf _dy+1
rrcf _dy
btfsc _x+1, 7
bsf _dy+1, 7
decfsz _j
bra as_bs_x_loop
; Calculate what needs to be added to _x
as_bs_x_done:
movff _y, _dx
movff _y+1, _dx+1
movff _i, _j
movf _j
bz as_do_rotation
; Loop to shift _dx right
as_bs_y_loop:
bcf STATUS, 0
rrcf _dx+1
rrcf _dx
btfsc _y+1, 7
bsf _dx+1, 7
decfsz _j
bra as_bs_y_loop
; Perform adding operations on _x, _y and _ang, shifting the atans right one
as_do_rotation:
movff POSTINC0, PRODL
movff POSTINC0, PRODH
bcf STATUS, 0
rrcf PRODH
rrcf PRODL
btfsc _y+1, 7
bra as_sub_angle
; If _y is positive
movf PRODL, W
addwf _ang
movf PRODH, W
addwfc _ang+1
movf _dx, W
addwf _x
movf _dx+1, W
addwfc _x+1
movf _dy, W
subwf _y
movf _dy+1, W
subwfb _y+1
bra as_loop_bottom
; If _y is negative
as_sub_angle:
movf PRODL, W
subwf _ang
movf PRODH, W
subwfb _ang+1
movf _dx, W
subwf _x
movf _dx+1, W
subwfb _x+1
movf _dy, W
addwf _y
movf _dy+1, W
addwfc _y+1
; Increment the counter and exit the loop if done
as_loop_bottom:
incf _i
movlw 0x0e
cpfseq _i
bra as_loop
; Multiply the _x value by 19898 and divide by 2^14 to scale it
as_scale_x:
movff _x, _dx
movff _x+1, _dx+1
movlw 0xba
mulwf _dx
movff PRODH, _x
movlw 0x4d
mulwf _dx+1
movff PRODH, _dy
movff PRODL, _x+1
movlw 0xba
mulwf _dx+1
movf PRODL, W
addwf _x, F
movf PRODH, W
addwfc _x+1, F
clrf WREG
addwfc _dy, F
movlw 0x4d
mulwf _dx
movf PRODL, W
addwf _x, F
movf PRODH, W
addwfc _x+1, F
clrf WREG
addwfc _dy, F
movlw 0x06
movwf _j
as_scale_bs_loop:
bcf STATUS, 0
rrcf _dy
rrcf _x+1
rrcf _x
decfsz _j
bra as_scale_bs_loop
; Check if the quadrant was originally changed
as_check_quad:
btfss _quad, 7
bra as_output
btfss _ang+1,7
bra as_adjust_quad1
; If the angle is in quadrant 4, adjust it to quadrant 3
as_adjust_quad4:
negf _ang
bc as_negate_quad4
comf _ang+1
bra as_adjust_end
; If the low byte negation causes a carry, negate the upper byte
as_negate_quad4:
negf _ang+1
bra as_adjust_end
; If the angle is in quadrant 1, adjust it to quadrant 2
as_adjust_quad1:
comf _ang
comf _ang+1
; Toggle the sign bit
as_adjust_end:
btg _ang+1, 7
; Output the calculated angle and hypotenuse values
as_output:
;movff _ang, AARGB3
;movff _ang+1, AARGB3+1
;movff _x, AARGB3+2
;movff _x+1, AARGB3+3
; Restore the stack to its previous state
movf POSTDEC1
movff INDF1, FSR2L
return
endasm
; Export the functions to the linker
'GLOBAL sin_cos
'GLOBAL atan2_sqrt
'END
I am using this as an includes file, but I am getting values so far from expected, that I can't figure out what is going on.
Bookmarks