Code:
'/*******************************************************************************
'* FUNCTION NAME: sin_cos
'*
'* ARGUMENTS: int angle (angle in 16-bit binary radians)
'*
'* RETURNS: x = sin, y = cos (and can give tan: remember tan=sin/cos)
'*
'* DESCRIPTION: The angle is given in 16-bit radians (on a scale of -32,768
'* to 32,767). The function simultaneously calculates the sine
'* and cosine of the angle as fractions of 30,000 (where 30,000
'* equates to 1 and -30,000 equates to -1) and returns them in
'* a sin_cos_struct.
'*
'* EXAMPLE: ang = 5461 'radians (30 degrees);
'* gosub sincos;
'* result: x = 15000, y = 25980 'radians
'* For "decimal" divide by 3 to get 5000 (.5000) and 8660 (.8660)
'* To figure Tan result, use radians (x * 10000), then
'* tan = div32 y, result will be 5773 (for .5773)
'*******************************************************************************/
'/*******************************************************************************
'* FUNCTION NAME: atan2_sqrt
'*
'* ARGUMENTS: int y (y-coordinate)
'* int x (x-coordinate)
'*
'* RETURNS: x = atan2 y,x , or atan if x = 30000 radians (1.0000 decimal)
'* for atan: (atan2 y,1 = atan y)
'* y = hypotenuse
'* atan2 or atan results will be in radians, see chart
'*
'* DESCRIPTION: Given an ordered pair of coordinates, the function
'* simultaneously calculates the atan2 (the direction of the
'* position vector in 16-bit radians) and the square root of
'* the sum of the squares of the coordinates (the magnitude of
'* the position vector) and returns them in an
'* atan2_sqrt_struct.
'*
'* NOTES: (1) The accuracy of the returned values increases as the
'* sizes of x and y increase. Consider multiplying both by a
'* scaling factor before calling the function.
'* (2) The function will fail for x and y values that result in
'* magnitues greater than 32,767 (the size of a signed int).
'*
'* EXAMPLE: atan2_sqrt_struct bar;
'* int x = 25980, y = 15000;
'* gosub atan;
'* for the angle in radians: x = 5461
'* for the hypotenuse: y = 30000
'*******************************************************************************/
Code:
;*******************************************************************************
; --- CORDIC TRIG LIBRARY ---
; http://www.chiefdelphi.com/media/papers/2016
; FILE NAME: trig.inc
; AUTHOR: Patrick Fairbank
; LAST MODIFIED: FEB. 1, 2011 to make it cleaner
; Modified by Walter Dunckel (Scale Robotics Inc.) with help from Darrel Taylor
; http://www.scalerobotics.com/cordic.html
; DESCRIPTION: This file contains functions implementing the CORDIC
; algorithm, or how to get a 16 bit sin, cos, tan2 and hypotenuse result
;
; USAGE: Add this file to your PicBasic Pro project using INCLUDE "TRIG.inc"
; Then fill x,y values for atan2, or fill ang value for sincos
; then either GOSUB sincos or GOSUB atan
; LICENSE: Users are free to use, modify, and distribute this code
; as they see fit.
;
;******************************************************************************/
i var byte BANK0
j Var byte BANK0
quad var byte BANK0
x var word BANK0
y var word BANK0
ang var word BANK0
dy var word BANK0
dx var word BANK0
atans var word[15] BANK0
atans(0) = 16384
atans(1) = 9672
atans(2) = 5110
atans(3) = 2594
atans(4) = 1302
atans(5) = 652
atans(6) = 326
atans(7) = 163
atans(8) = 81
atans(9) = 41
atans(10) = 20
atans(11) = 10
atans(12) = 5
atans(13) = 3
atans(14) = 1
goto OverAtan
sincos:
asm
; Initialize _x to 18218
movlw 0x2a
movwf _x
movlw 0x47
movwf _x+1
; Initialize _y to 0
clrf _y
clrf _y+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
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:
endasm
return 'Done with sincos , return
;######################################################################
; Calculates the magnitude and direction of the given ordered pair
;atan_sqrt:
atan:
asm
; 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
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:
endasm
return
OverAtan:
Working base cordic trig code for sin cos tan atan atan2 and hypotenuse
Bookmarks