'/******************************************************************************* ' 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