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
16 bit trig for sin cos tan2 and hypotenuse
Bookmarks