Cordic trig assembly code for PIC18f


+ Reply to Thread
Results 1 to 40 of 55

Hybrid View

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

    Default 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.
    Last edited by ScaleRobotics; - 6th February 2009 at 22:21.

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

    Default

    Here are the changes I have made to the assembly code to try to get it to work with PicBasic:


    commented out EXTERN AARGB3, UDATA_ACS, IDATA, CODE, edited variable descriptions so that variables are declared in PicBasic.

    Original Code:
    Code:
      ; Import the address the compiler uses to store 32-bit return values
        EXTERN AARGB3
    
    
        UDATA_ACS
    
    ; Variable declarations
    i       RES 1
    j       RES 1
    quad    RES 1
    x       RES 2
    y       RES 2
    ang     RES 2
    dy      RES 2
    dx      RES 2
    
    
        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
    I edited out where the result is moved to AARGB3, and just use the word variables _x, _y, and _ang to store the results. Commented out Globals and End. And of course added ASM and ENDASM.

    My code:
    Code:
    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
    
    
    -snip
    
      ; 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
    
    -snip
    
      ; 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
    Please let me know if you have any suggestions. It does compile without errors, but I do not get useful data out. When I input 5461 as the angle, I get the sin=29998, and cos=405. The result should be 15000, and 25980. But then if I run it again, a different value pops up for cos, that still does not get close to expected values.

    Thanks,

    Walter
    Last edited by ScaleRobotics; - 8th February 2009 at 18:35. Reason: left things out, and forgot a [/code]

  3. #3
    Join Date
    Jul 2003
    Location
    Colorado Springs
    Posts
    4,959

    Default

    Hi Walter,
    I thought we had this working already.

    Here's one thing I see...
    Code:
      ; Set up the main loop
    sc_loop_start:
      clrf i
      banksel atans
      lfsr FSR0, atans
    banksel will put it in the proper bank for a variable in RAM.
    But he's trying to use it with with a Label in FLASH.

    I'm sure it was just a mistake on his part because the lfsr is correct.
    And with his ASM code at the very beginning, it probably would have come up with bank 0 without him even knowing it.

    However, now that it's in an include file for PBP, that atans table will be at a much higher address in Flash and you'll end up in a different bank than where the variables are.

    Just comment it out.

    I was only 1/3 of the way thru when I found that.
    If it doesn't help, let me know, I'll keep looking for more.

    P.S. You should specify BANK0 for all the variables used with this routine.
    <br>
    Last edited by Darrel Taylor; - 9th February 2009 at 05:50. Reason: BANK0 VARS
    DT

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

    Default

    I thought we had this working already.
    Thanks Darrel! Yes, we had the cordic working for pic16 chips (can be found here http://www.picbasic.co.uk/forum/show...6307#post46307 ), but this one is more better. I feel the need for speed, and this one performs a hypotenuse length routine while it does the atan function. Also a bit more precise than the other one.

    I tried commenting out the banksel, and putting all the variables for the function in bank0, but I am still getting unexpected results.

    Thanks a lot for the help.

    Walter
    Last edited by ScaleRobotics; - 7th September 2011 at 15:44.

  5. #5
    Join Date
    Jul 2003
    Location
    Colorado Springs
    Posts
    4,959

    Default

    OK, next problem ...

    There's an issue with the indirect addressing.
    I don't use C, but I think it uses a "stack" structure to pass variables back and forth to functions.

    All the POSTINC's and PLUSW's etc, are moving things around to unknown locations because PBP doesn't use a stack.

    It's a little hard to follow (since I don't know how C works), so I thought I'd let you work on it too. Maybe you'll see something I don't (yet).

    If I figure out what it's doing with the FSR's I'll let you know.
    <br>
    DT

  6. #6
    Join Date
    Jul 2003
    Location
    Colorado Springs
    Posts
    4,959

    Default

    Oh! It wasn't as hard as I thought it would be.

    And yup, it's trying to pull the parameters off the stack, and would be overwriting the variables with random values from RAM.
    I assume you've set those variables before calling the routine.

    Commenting these should help...
    Code:
      ; Set up the stack
    ;  movff FSR2L, POSTINC1
    ;  movff FSR1L, FSR2L
    
    
      ; Initialize _ang to passed parameter
    ;  movlw 0xfd
    ;  movff PLUSW2, _ang
    ;  movlw 0xfe
    ;  movff PLUSW2, _ang+1<hr>; 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
    ADDED: OOPs, missed a couple ...
    Code:
      ; Restore the stack to its previous state
    ;  movf POSTDEC1
    ;  movff INDF1, FSR2L
    and there's another banksel atans in the as_loop_start: part.
    Last edited by Darrel Taylor; - 9th February 2009 at 09:48. Reason: Missed some
    DT

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

    Default Re: Cordic trig assembly code for PIC18f

    Hi Chris,

    Well, I might have to admit that the cordic may not be the best solution for a single axis tilt sensor. It is a natural for a dual or triple axis sensor. If you just have a single axis sensor, I might suggest a lookup table. Now I'm not really good at converting mathematical equations back and forth, so I can't say for sure if you are on the right track with the one you have. It doesn't resemble the ones I have used enough for me to tell. But I can show you some that I think would work. And, perhaps yours would work too. I just can't tell from here.

    Here is the app note I worked with before. As you can see, the arctan equation "theta = arctan(Ax/Az)" on page 4 is a pretty cool thing. Of course, this one requires a two axis sensor, but it keeps resolution up to about 1.3 degrees. It also fits in pretty well with are atan2 cordic.
    Attachment 4533

    Now for a single axis sensor, your resolution at near 90 degree angles goes down to about 9 degree accuracy. This seems to lend itself better to just a lookup table. But, if you really wanted to use the cordic, you could.

    For a single axis sensor, one equation for determining angle is arcsin (x) where x is acceleration in g's . Our cordic, can't do arcsin, but it can do arctan. We can use the following equation from Wikipedia:

    Click image for larger version

Name:	arcsin-to-arctan.png
Views:	694
Size:	1.1 KB
ID:	6268


    and solve for theta.

    atan(y) = atan2(y,1)

    But, in our case, we are feeding y with a value which at its peak, = 1g. So, whatever value your sensor gives for 1g, you should fill in for the "1" value. y will equal 2 arctan x/((1+sqr(1-x^2)) , then run it through the cordic. I just feel if its going to be off by up to 9 degrees (because we are using a single sensor) why not just use a lookup table.
    Last edited by ScaleRobotics; - 12th February 2012 at 16:47.
    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

Posting Permissions

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