'/******************************************************************************* '* 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 '*******************************************************************************/ ;******************************************************************************* ; --- 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 'both input and output - see above y var word BANK0 'both input and output - see above ang var word BANK0 'input angle, see radian chart to decifer dy var word BANK0 dx var word BANK0 atans var word[15] BANK0 'array for initial cordic guesses 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: