Cordic trig assembly code for PIC18f
I have been trying to use cordic code that was originally written for a PIC18F device. I have made some changes to it to try to make it compatible with PicBasic Pro, but I am getting some unexpected results. I am compiling it to a PIC18F4620 device using 2.50b. It compiles without any errors, and prints the values to an LCD, but the results vary, and are unexpected. Any ideas would be greatly appreciated.
Here is the original code:
http://www.chiefdelphi.com/media/papers/2016
Here is my stab at making it compatible:
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.
Any help would be greatly appreciated.
3 Attachment(s)
Working base cordic trig code for sin cos tan atan atan2 and hypotenuse
Here is the base working code for the PIC18 cordic. It is 600 bytes, and performs atan2, hypotenuse, sin, cos, and tan. Times will change with different angles and coordinate entries, but here are a few stats from Codetimer.bas: http://www.picbasic.co.uk/forum/cont...te-XYZ-command
sincos: Simulaneous result of SIN and COS
Time: 185.65924 usec
OSC Freq: 48 Mhz
-------------------------
atan: ATAN2 result as well as hypotenuse
Time: 178.32620 usec
OSC Freq: 48 Mhz
-------------------------
And just for comparison, here is some timing for using a slower chip and different cordic code, compared to math.h
http://www.picbasic.co.uk/forum/atta...9&d=1296892329
And as a test, I ran the PBP code for SQR and ATN to compare it's speed.
Code:
x = x ATN y
y = SQR ang
took about 48.5 uS, but has much less precision.
PBP's SIN and COS function were much quicker at 4.16 uS, but again, much less precision than the cordic.
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:
Attachment 5148
2 Attachment(s)
16 bit trig for sin cos tan2 and hypotenuse
Here's one for people like me who think in degrees and not radians:
sincos example:
input ang in degrees.dd example: ang = 3000 (= 30.00 degrees)
call sincos
result: x = 15004 , Y = 25981
so, 15004/30000 = sin(ang) = 0.5001 and 25981/30000 = cos(ang) = 0.8660
atan2 example:
input x,y co-ordinates
x = 25981, y = 15000
call atan2
result: x = 29998 = hypotenuse or distance to x,y, y = angle to x,y in degrees.dd = 5998 (=59.98 degrees), ang = 5463 = radians 0 to 65535
Code:
'/*******************************************************************************
' TRIG.inc for PIC18 V 1.5
'/*******************************************************************************
'* FUNCTION NAME: sincos
'*
'* ARGUMENTS: ang (angle deg.dd format example: 35999 = 359.99 deg)
'*
'* RETURNS: x = sin(ang) , y = cos(ang)
'*
'* DESCRIPTION: The angle is given in degrees.dd (see above)
'* 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
'* x, and y as x=sin(ang), y = cos(ang).
'*
'* EXAMPLE: ang = 3000 (30.00 degrees) x = 15004 , Y = 25981
'* so, 15004/30000 = 0.5001 and 25981/30000 = 0.8660
'*
'*******************************************************************************/
'/*******************************************************************************
'* FUNCTION NAME: atan2
'*
'* ARGUMENTS: int x (x-coordinate)
'* int y (y-coordinate)
'*
'* RETURNS: atan2 of x,y:(angle to x,y) and the hypotenuse (distance) of x,y
'*
'* DESCRIPTION: Given an ordered pair of coordinates, the function
'* simultaneously calculates the atan2 (the direction of the
'* position as y degrees, and ang radians) and the square root of
'* the sum of the squares of the coordinates (the magnitude of
'* the position vector) as x
'*
'*
'* 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
'* x = 25980, y = 15000;
'* results:
'* ang = angle in radians 0 to 65535
'* x = distance = 30000 (same units as x and y)
'* y = 6000 (angle in degrees 60.00 degrees)
'*******************************************************************************/
;*******************************************************************************
; --- CORDIC TRIG LIBRARY ---
;http://www.chiefdelphi.com/media/papers/2016
; FILE NAME: trig.inc
; AUTHOR: Patrick Fairbank
; Last Modified Feb 15, 2012 - fixed bug in sincos which calculated wrong
; result for sin and cos in quadrants 2,3 and 4. Thanks Martin!
; FEB. 15, 2009 to make it PicBasic compatible and added degree conversion
; Modified by Walter Dunckel (Scale Robotics Inc.) with help from Darrel Taylor
; http://www.scalerobotics.com/79-cordic-for-picbasic.html
; DESCRIPTION: This file contains functions implementing the COORDIC
; 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"
; and add a line with main: directly below the include line
; Then fill x,y values for atan2, or fill ang value for sincos
; then either CALL sincos or CALL atan2
; LICENSE: Users are free to use, modify, and distribute this code
; as they see fit.
;
; sincos: input ang, output x = sin(ang) and y = cos(ang)
;
;
; atan2: input x and y coordinates
; Output the calculated angle and hypotenuse values
; as output: y = angle in degrees, ang = angle in radians, x = hypotenuse
;
;******************************************************************************/
' Example output using TRIG.inc using hserout
' HSEROUT ["Angle = ",dec a,", sin(ang) = ",sdec x,", cos(ang) = ",sdec y,13]
'Angle = 0, sin(ang) = 5, cos(ang) = 30000
'Angle = 10, sin(ang) = 5209, cos(ang) = 29544
'Angle = 20, sin(ang) = 10266, cos(ang) = 28188
'Angle = 30, sin(ang) = 15004, cos(ang) = 25981
'Angle = 40, sin(ang) = 19287, cos(ang) = 22981
'Angle = 50, sin(ang) = 22983, cos(ang) = 19285
'Angle = 60, sin(ang) = 25983, cos(ang) = 14998
'Angle = 70, sin(ang) = 28188, cos(ang) = 10264
'Angle = 80, sin(ang) = 29544, cos(ang) = 5207
'Angle = 90, sin(ang) = 30000, cos(ang) = 3
'Angle = 100, sin(ang) = 29543, cos(ang) = -5223
'Angle = 110, sin(ang) = 28189, cos(ang) = -10276
'Angle = 120, sin(ang) = 25974, cos(ang) = -15010
'Angle = 130, sin(ang) = 22973, cos(ang) = -19293
'Angle = 140, sin(ang) = 19274, cos(ang) = -22992
'Angle = 150, sin(ang) = 14991, cos(ang) = -25989
'Angle = 160, sin(ang) = 10249, cos(ang) = -28190
'Angle = 170, sin(ang) = 5192, cos(ang) = -29546
'Angle = 180, sin(ang) = -9, cos(ang) = -29999
'Angle = 190, sin(ang) = -5216, cos(ang) = -29544
'Angle = 200, sin(ang) = -10271, cos(ang) = -28184
'Angle = 210, sin(ang) = -15013, cos(ang) = -25977
'Angle = 220, sin(ang) = -19296, cos(ang) = -22976
'Angle = 230, sin(ang) = -22987, cos(ang) = -19275
'Angle = 240, sin(ang) = -25986, cos(ang) = -14992
'Angle = 250, sin(ang) = -28199, cos(ang) = -10258
'Angle = 260, sin(ang) = -29549, cos(ang) = -5195
'Angle = 270, sin(ang) = -30000, cos(ang) = 5
'Angle = 280, sin(ang) = -29544, cos(ang) = 5213
'Angle = 290, sin(ang) = -28188, cos(ang) = 10266
'Angle = 300, sin(ang) = -25981, cos(ang) = 15004
'Angle = 310, sin(ang) = -22981, cos(ang) = 19287
'Angle = 320, sin(ang) = -19283, cos(ang) = 22985
'Angle = 330, sin(ang) = -14998, cos(ang) = 25983
'Angle = 340, sin(ang) = -10260, cos(ang) = 28190
'Angle = 350, sin(ang) = -5207, cos(ang) = 29544
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 OverTrig
atan2:
asm
call atan2_sqrt
endasm
'convert to degrees.dd y is degrees
If ang < 16384 then y = 16383 - ang
if ang > 16383 then
y = 65535 - ang
y = y + 16383 'correct 90 degrees for radian
endif
y = y * 256 'divides radians to get degrees within 57ppm
y = div32 466 'degrees.dd is y, radians is ang
return
sincos:
'use angle as deg.dd for example 35999 is 359.99 degrees
if ang < 9001 then ang = 9000 - ang 'change degrees to radians
if ang > 9000 then ang = 45000 - ang 'change degrees to radians
ang = ang * 466
ang = div32 256
asm
call sin_cos
endasm
return
asm
; Calculates the sine and cosine of the given angle
sin_cos:
; 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:
return
; Calculates the magnitude and direction of the given ordered pair
atan2_sqrt:
; 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:
return
endasm
OverTrig: 'jump over code
TRIG for non-integer angles using a PIC16F684?
Hey, nerds! :) I'm relatively new to PICs, and right now I'm using the PIC16F684.
I'm trying to write a program for a mobile robot with two stepper motors for differential steering - to navigate it along a predefined path (or function). In order to do this, I need to be able to calculate trig values using Assembly code (yikes!). Not only that, but due to the physical geometry of my robot, I need to calculate the sine and cosine of multiples of 0.72 (non-integers).
Will this TRIG.INC file work with non-integers? If not, is there a way to get around this (DP shifting)?
Thanks for your help!
Samuel Aaron Ward
[email protected]
(740) 357-8016 cell
Re: Cordic trig assembly code for PIC18f
Hi !
I'm trying to write a program to calculate sunrise and sunset. The pic is 18F2550 and I'm using long variables and PBPL. I've tried to use the TRIG.INC but it seems that I cant use it in PBPL, Error: DIV32. Is there a way to rewrite the program so that it will work with PBPL?
I'm using Picbasic Pro 2.50 and MPASM 8.15.
Bernt-Ove