Cordic trig assembly code for PIC18f


Closed Thread
Results 1 to 40 of 55

Hybrid View

  1. #1
    Join Date
    Feb 2006
    Location
    Gilroy, CA
    Posts
    1,530


    Did you find this post helpful? Yes | No

    Default 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


    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:
    Name:  cordic-degrees-radians.PNG
Views: 26331
Size:  39.3 KB
    Attached Images Attached Images  
    Attached Files Attached Files
    Last edited by ScaleRobotics; - 21st April 2011 at 01:57. Reason: Update with cleaner code

  2. #2
    Join Date
    Feb 2006
    Location
    Gilroy, CA
    Posts
    1,530


    Did you find this post helpful? Yes | No

    Default

    Added this content to post 13
    Last edited by ScaleRobotics; - 5th February 2011 at 06:46. Reason: Added this content to post 13

  3. #3
    Join Date
    Aug 2006
    Location
    Look, behind you.
    Posts
    2,818


    Did you find this post helpful? Yes | No

    Default

    WOW, I have enough trouble doing trig on a calculator. Thank You both for this. Could you please assist us mathematicly challenged with a couple of lines of example code as to how to use this masterpiece? I get the part where my program has main: respondant to your goto main, but how do I use the functions? Or is that what post 8 is about?
    If you do not believe in MAGIC, Consider how currency has value simply by printing it, and is then traded for real assets.
    .
    Gold is the money of kings, silver is the money of gentlemen, barter is the money of peasants - but debt is the money of slaves
    .
    There simply is no "Happy Spam" If you do it you will disappear from this forum.

  4. #4
    Join Date
    Feb 2006
    Location
    Gilroy, CA
    Posts
    1,530


    Did you find this post helpful? Yes | No

    Default

    No problem, probably should have done that in the first place.

    Use the includes for trig.inc at the beginning of the program


    When you are doing an atan2 (you get the hypotenuse for free) load up values x and y.

    For instance
    Code:
    x = 25980
    y = 15000
    what do these numbers mean? Well for atan2 they mean a point at location x,y from origin. This function accepts numbers up to from -32,767 to 32,767

    now call your atan subroutine
    Code:
    call atan
    result, ang = 5461 or about 30 degrees radian (60 degrees deg).

    You will get your result in the variable called "ang" for angle and "x" for hypotenuse. Now hypotenuse will be in the same scale as your x and y coordinates, but the angle is represented by 0 = 0 and 32,767 = 180 degrees. I believe that -5461 will be 120 degrees. So to figure your angle, 1 degree = 46602/256 (or about 182) of these radians. Update, since radians start at 90 degrees, you have to do a little math to calc degrees.

    Now for sincos. This calculates the sin and cos simultaneously.

    Load you angle "ang" with an angle value using the funky radians from above

    Code:
    ang = 5461
    call sincos
    results, sin(60 degrees) = x = 25981 , cos(60 degrees) = y = 15004

    Now to make sense of your results, instead of using the strange radians from above, you get a result of -30,000 to 30,000. So converting the above results by dividing by 30,000, you get
    sin(60) = .8660333 and cos(60)= .5001333 . These results are pretty close to what I get on my calculator, .8660254 and .500000 respectively.
    Last edited by ScaleRobotics; - 13th February 2009 at 09:01.

  5. #5
    Join Date
    Feb 2006
    Location
    Gilroy, CA
    Posts
    1,530


    Did you find this post helpful? Yes | No

    Default

    Quote Originally Posted by scalerobotics View Post
    For instance
    Code:
    x = 25980
    y = 15000
    .....and that is 30 degrees
    My perspective was all wrong in the above post. I was supposed to be lying down and facing East, that is the radian way.

    to convert to degrees ......

    Imagine a circle with degree headings, but instead of starting at the top, and going clockwise, as the numbers get higher, we are going to do something completely different. Starting at 90 degrees, we are going to go counterclockwise while our radians rise. From zero to 32,767 is the arc from 90 degrees (through 0) to 270 degrees. To complete the circle, our arc continues from 32,768 to 65,535 as it travels from 270 degrees to 90 degrees in a counter clockwise rotation.

    So, as you can see there is just a little bit of manipulation to get the result in angle, but it really isn't too bad.

    Thanks Joe.
    Last edited by ScaleRobotics; - 13th February 2009 at 08:52.

  6. #6
    Join Date
    Feb 2006
    Location
    Gilroy, CA
    Posts
    1,530


    Did you find this post helpful? Yes | No

    Default

    For anyone else like me that forgot all they learned about radians.... This should help. These go from 0 to 65,535.
    Attached Images Attached Images  
    Last edited by ScaleRobotics; - 5th February 2011 at 17:05.

  7. #7
    Join Date
    Aug 2006
    Location
    Look, behind you.
    Posts
    2,818


    Did you find this post helpful? Yes | No

    Default

    Thank You Walter,
    It is definitely going to take some time to get my head around this, unfortunate side effect of growing up during the Vietnam war, where we were expected to become soldiers who got shipped there and returned in a nicely bagged and boxed container. My algebra teacher was a retired navel captain, who taught spit about the subject but spoke extensively about his career and travels in Tahiti. That notwithstanding I will get this. I am sure nobody who participates in this forum is going to point at me in the store and say "hey there is the dummy who couldn't understand that code Walter wrote . . . " <b>So Anyway</b> the numbers you plugged in represent . . .Units, of my choosing . . .and to use this
    Code:
    include"trig.inc"
    main:
    ang=5461
    x=0
    y=0
    
    asm
        call sin_cos
    endasm
        
    Lcdout $fe, 1   ' Clear LCD screen
    
    lcdout $FE,1,#x,",",#y
    lcdout $FE,$C0,#ang
    Correct? Figure I do not need to know how to build a truck to drive one.
    If you do not believe in MAGIC, Consider how currency has value simply by printing it, and is then traded for real assets.
    .
    Gold is the money of kings, silver is the money of gentlemen, barter is the money of peasants - but debt is the money of slaves
    .
    There simply is no "Happy Spam" If you do it you will disappear from this forum.

  8. #8
    Join Date
    Jul 2011
    Posts
    17


    Did you find this post helpful? Yes | No

    Default Re: Cordic trig assembly code for PIC18f

    hi ScaleRobotics , i am interested in your codes posted on #51. and i have a problem.
    ...
    ang=3000
    call sincos
    x=60395, y=39551
    ...
    for all ang values i get same numbers, i dont understan what is the problem? thank you

  9. #9
    Join Date
    Feb 2006
    Location
    Gilroy, CA
    Posts
    1,530


    Did you find this post helpful? Yes | No

    Default Re: Cordic trig assembly code for PIC18f

    That's weird. Maybe you can show us some of your code?
    http://www.scalerobotics.com

Similar Threads

  1. How much code space do PBP statements use.
    By Darrel Taylor in forum Code Examples
    Replies: 5
    Last Post: - 13th February 2009, 21:31
  2. Loop with two motor and 2 sensors
    By MrRoboto in forum mel PIC BASIC
    Replies: 4
    Last Post: - 8th December 2008, 23:40
  3. Making Program Code Space your playground...
    By Melanie in forum Code Examples
    Replies: 15
    Last Post: - 19th July 2008, 08:26
  4. 4 Chanel Dmx512 ready assembly code to PBP ?
    By syscoder in forum mel PIC BASIC Pro
    Replies: 10
    Last Post: - 21st March 2007, 23:55
  5. Your Suggestions: Assembly code material?
    By dw_picbasic in forum General
    Replies: 1
    Last Post: - 2nd February 2007, 17:33

Members who have read this thread : 1

You do not have permission to view the list of names.

Posting Permissions

  • You may not post new threads
  • You may not post replies
  • You may not post attachments
  • You may not edit your posts