+ Reply to Thread
Page 1 of 2 12 LastLast
Results 1 to 40 of 53
  1. #1
    Join Date
    Feb 2006
    Location
    Gilroy, CA
    Posts
    1,527

    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,527

    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,935

    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,527

    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,935

    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,935

    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
    Jul 2003
    Location
    Colorado Springs
    Posts
    4,935

    Default

    And then there's another problem. Or actually the same problem.

    Those banksel atans ...

    Somehow in C, the atans table that's in Flash by the DW statements, gets copied to RAM. Then it uses FSR0 to read the array. But it's strange that it uses the same label.

    I think you'll have to create a WORD array in PBP and load it with the values from the atans data.
    Either read the data or just have a few atans(0) = xxxx statements.

    If the array's in BANK0 then you still won't need the banksel statements.

    Mañana
    DT

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

    Default

    Thanks for all your work on this Darrel. I used all your suggestions, and the hypotenuse seems to work, but the angle and sin, cos do not. It seems to be doing something fishy because it is taking a few seconds to give me a result. Here is what I have done. If there are no glaring errors, perhaps I should try converting the cordic code that worked for the PIC16 to PIC18 code? This sounds like a very complex issue with the way the PIC18 is handling the addresses, so I very much appreciate the time you have spent on it.

    Code:
    define OSC 20
    
    include "TRIG.inc"
    
    main:
    define       LED     PortD.3
    
    'LCD defines taken out so that this code would fit in limited size 
    
    ADCON1=15
    
    TRISA=%00000011                         ' Set PORTA  
    TRISB=%00000000                         ' Set PortB
    TRISC=%00000000  
    TRISD=%00000100  
    '       Vdd     5 volts
    '       Vss     Ground
    '       Vo      20K potentiometer (or ground)
    '       DB0-3   No connect
    
    ang=5461
    x=0
    y=0
    
    asm
        call sin_cos
    endasm
        
    Lcdout $fe, 1   ' Clear LCD screen
    'toggle portd.3
            Pause 500       ' Wait for LCD to startup
    lcdout $FE,1,#x,",",#y
    lcdout $FE,$C0,#ang
    
    end
    
    'trig.inc here_________________________________________________
    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[14] 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 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
      ;CHK?RP _atans    ;was 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
      ;CHK?RP _atans    ;was 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
    Last edited by ScaleRobotics; - 10th February 2009 at 16:36.

  9. #9
    Join Date
    Jul 2003
    Location
    Colorado Springs
    Posts
    4,935

    Default

    There are 15 elements in the array (0-14).

    Might be overwriting another variable.
    <br>
    Last edited by Darrel Taylor; - 10th February 2009 at 17:15. Reason: .
    DT

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

    Talking

    Once again you saved the day!

    IT WORKS! I will clean it up a little bit and repost, so others may enjoy it. Looks to be about the same speed as the original author posted. Around 9,000 times per second on a 48mhz chip.
    Last edited by ScaleRobotics; - 10th February 2009 at 18:00.

  11. #11
    Join Date
    Jul 2003
    Location
    Colorado Springs
    Posts
    4,935

    Default

    IT WORKS!
    SWEET!

    You make it easy Walter.

    I know you just got it working, so you probably don't know this yet ...
    I'm curious about the accuracy.

    If I remember correctly, the last version could calculate a bearing within ~200 miles.

    What's the new resolution?
    <br>
    DT

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

    Default

    Resolution is pretty good!

    180 degrees is covered by 0 to 32,767.

    So thats .00549 degrees per unit. Now I have not tested the total error, but I have seen it vary from expected by .015 degrees or so, but that is pretty close!

    Sin and cos return between -30,000 and +30,000 for -1 and 1. One of the cool things about the cordic, is that it returns cos AND sin at the same time. I have heard that it is 90 times faster than math.h.

    Also, this allows atan2 for a point x, y and accepts values from 0 to 32,767, and gives the hypotenuse to x,y which for me is the distance. It also gets these values at the same time!

    This way my distance values should be spot on. I am still limited to about 200 miles between waypoints, the way I am measuring the change in x, and the change in y. But I could do a divide function to break the units into something it could handle. I just have not needed to do this for my application.

    What I was using it for was navigation between waypoints. The first cordic for PIC16 did a pretty good job. It was accurate for bearing by about half a degree or so. My distance calculations did a simple square and square root that gave me errors on the hypotenuse. For long distances, I got pretty reliable results, less than 1 percent off of predicted results, but close range I could be off by a lot more. I could have cleaned this up with some higher math, but I was running out of speed trying to read two NMEA sentences, convert to decimal degrees for lat/lon, and fly a model plane, at 5 hertz gps readings. I gave up one sentence, and all is working well with the PIC16. This will allow me to break into the PIC18 world, and I am sure it will handle both NMEA sentences, plus a few more things. Nice!
    Last edited by ScaleRobotics; - 11th February 2009 at 04:48.

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

    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:
    Click image for larger version

Name:	cordic-degrees-radians.PNG
Views:	5953
Size:	39.3 KB
ID:	5148
    Attached Images Attached Images  
    Attached Files Attached Files
    Last edited by ScaleRobotics; - 21st April 2011 at 01:57. Reason: Update with cleaner code

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

    Default

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

  15. #15
    Join Date
    Aug 2006
    Location
    Look behind you.
    Posts
    2,594

    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?
    Anti virus software seems a little like hiring the Mafia to protect you.

    Moderators are just regular members who get to put the KABAM on SPAM



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

    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.

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

    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.

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

    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.

  19. #19
    Join Date
    Aug 2006
    Location
    Look behind you.
    Posts
    2,594

    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.
    Anti virus software seems a little like hiring the Mafia to protect you.

    Moderators are just regular members who get to put the KABAM on SPAM



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

    Default

    Hey Joe,

    No you definitely do not need to know how it works. But if you find out, please let me know. The assembly code only seems to put me to sleep when I read it. Something about rotating angle, shifting bits zzzzz.....

    Your code will work, but it can be even simpler since it has the includes file in it. You don't have to get in to the asm endasm, as the sincos function is already there.

    Code:
    include"trig.inc"
    main:
    ang=5461
    x=0
    y=0
    
    call sincos
        
    Lcdout $fe, 1   ' Clear LCD screen
    
    lcdout $FE,1,#x,",",#y
    lcdout $FE,$C0,#ang
    It is that easy. Only a little bit harder if you want to do anything with the data returned in x and y (or ang and x for atan function)

  21. #21
    Join Date
    Aug 2006
    Location
    Look behind you.
    Posts
    2,594

    Default

    Quote Originally Posted by scalerobotics View Post
    You don't have to get in to the asm endasm, as the sincos function is already there.
    Funny, that was a copy paste, I don't know where the asm / endasm went, glad to know it can be omitted ( call sincos ). Just 1 (I hope) more question: Where does the number ang=5461 come from ? I apologize if I seem tedious, I really haven't a clue. Thanks.
    Anti virus software seems a little like hiring the Mafia to protect you.

    Moderators are just regular members who get to put the KABAM on SPAM



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

    Default

    That is the hardest part. Darn radians are 90 degrees off from the start.

    For example

    0 = 90 degrees
    5461 = 60 degrees
    10922 = 30 degrees
    16383 = 0 degrees (or North)
    21844 = 330 degrees
    32767 = 270 degrees
    38228 = 240 degrees

    and so on.....

    You definitely do not have to know this, but I think it is interesting:
    In the time it takes for a regular Sin, or Cos 16 bit function on a PIC18 to give a result, light travels 3000 kilometers
    The time it takes for this cordic 16 bit result, light travels 33.3 kilometers.
    How do you say "Your mileage may vary" when you use kilometers?
    Last edited by ScaleRobotics; - 13th February 2009 at 19:20.

  23. #23
    Join Date
    Aug 2006
    Location
    Look behind you.
    Posts
    2,594

    Default

    Quote Originally Posted by scalerobotics View Post
    That is the hardest part. Darn radians are 90 degrees off from the start.

    For example

    0 = 90 degrees
    5461 = 60 degrees
    10922 = 30 degrees
    16383 = 0 degrees (or North)
    21844 = 330 degrees
    32767 = 270 degrees
    38228 = 240 degrees

    and so on.....

    You definitely do not have to know this, but I think it is interesting:
    In the time it takes for a regular Sin, or Cos 16 bit function on a PIC18 to give a result, light travels 3000 kilometers
    The time it takes for this cordic 16 bit result, light travels 33.3 kilometers.
    How do you say "Your mileage may vary" when you use kilometers?
    Moving counter clockwise from the 3 o'clock position . . .I see for every 30 degrees the numbers change 5461 or 182.03333333333333333333333333333 per degree or 65532 for a full circle which is a number that fits nicely in a word variable.
    Anti virus software seems a little like hiring the Mafia to protect you.

    Moderators are just regular members who get to put the KABAM on SPAM



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

    Default

    Quote Originally Posted by Joe S. View Post
    Moving counter clockwise from the 3 o'clock position . . .I see for every 30 degrees the numbers change 5461 or 182.03333333333333333333333333333 per degree or 65532 for a full circle which is a number that fits nicely in a word variable.
    Well now, that's better than all my blabbering! Looks like you got it down.

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

    Default 16 bit trig for sin cos tan2 and hypotenuse

    Here's one for people like me who think in degrees and not radians:


    Hi, we are pleased to see that you are using our forum.

    We'd like you to become a member of our community. Membership is FREE, please REGISTER and view our forums without these annoying reminders!

    Once registered you may post on the forums, download from the file areas and use the WIKI without interruption.






    sincos example:
    input ang in degrees.dd example: ang = 3000 (= 30.00 degrees)
    call sincos
    result: x = 15004 , Y = 25981
    so, 15004/30000 = sin(ang) = 0.5001 and 25981/30000 = cos(ang) = 0.8660

    atan2 example:
    input x,y co-ordinates
    x = 25981, y = 15000
    call atan2
    result: x = 29998 = hypotenuse or distance to x,y, y = angle to x,y in degrees.dd = 5998 (=59.98 degrees), ang = 5463 = radians 0 to 65535

    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
    Attached Files Attached Files
    Last edited by ScaleRobotics; - 15th February 2012 at 15:37. Reason: fixed sincos bug corrected in v1.5

  26. #26

    Smile TRIG for non-integer angles using a PIC16F684?

    Hey, nerds! I'm relatively new to PICs, and right now I'm using the PIC16F684.

    I'm trying to write a program for a mobile robot with two stepper motors for differential steering - to navigate it along a predefined path (or function). In order to do this, I need to be able to calculate trig values using Assembly code (yikes!). Not only that, but due to the physical geometry of my robot, I need to calculate the sine and cosine of multiples of 0.72 (non-integers).

    Will this TRIG.INC file work with non-integers? If not, is there a way to get around this (DP shifting)?

    Thanks for your help!
    Samuel Aaron Ward
    aaron@zmm.com
    (740) 357-8016 cell

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

    Default

    Hello Samuel,

    Can you explain the significance of the .72, or maybe what you are calculating. I put on my nerd glasses, but it's not helping. There might not enough resolution in the PIC16 cordic version, but then, I do not have a grasp of what you are measuring, so maybe it would be fine.

    The PIC16 cordic is here http://www.picbasic.co.uk/forum/showthread.php?t=7502

    Thanks,
    Walter
    http://www.scalerobotics.com

  28. #28
    Join Date
    Mar 2003
    Location
    Commerce Michigan USA
    Posts
    803

    Default

    scalerobotics, the .72 probably represents 7.2 degrees per step for the motor... Stepper motors usually come in 1.8 and 7.2 degrees per step...

    Dave Purola,
    N8NTA

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

    Default

    Thanks Dave!

    In that case, the PIC16 should work for you Samuel. But it does need some modification to enable you to find the angle throughout 360 degrees. The PIC16 cordic only does 0 to 90 degrees. But the rest can be done pretty easily with PicBasic. I think it handles around 2000 iterations per second, but I may have been doing a few more equations when I measured that.

    Here is some code that works on a Pic16F877a:
    Code:
    '****************************************************************
    '*  Name    : UNTITLED.BAS                                      *
    '*  Author  : Walter Dunckel                                    *
    '*  Notice  : Copyright (c) 2009 Scale Robotics Inc.            *
    '*          : All Rights Reserved                               *
    '*  Date    : 10/7/2009                                         *
    '*  Version : 1.0                                               *
    '*  Notes   :                                                   *
    '*          :                                                   *
    '****************************************************************
    DEFINE __16F877A 1				; list directive to define processor
    DEFINE OSC 20                    ' Define Oscillator Speed                    
    DEFINE LOADER_USED 0
    include "trig-pic16.inc"   
    
    ' Shutdown comparators
    ADCON1 = 7                        ; Turn of all A/D
    CMCON  = 7
    ' Set the port directions
    ' 0=output 1=input
    TRISA=%00000011                         ' Set PORTA  0-1-3 used for AD
    TRISB=%11011100                         ' Set PortB
    TRISC=%10000000  
    TRISD=%00000000                       ' Set PortC USART RX as an input
    porta.2 = 0 
    porta.3 = 0
    
    x           var     word
    y           var     word
    z           var     word
    i           var     word
    lon_dif     var     word
    lat_dif     var     word
    angle       var     word
    angle_deg   var     word
    distance    var     word
    j           var     word
    
    ;---------[change these to match your hardware]------------------------------
    DEFINE LCD_DREG PORTD             ; Set LCD Data port                B C   D
    DEFINE LCD_DBIT 0                 ; Set starting Data bit (0 or 4)   4 0
    DEFINE LCD_RSREG PORTA            ; Set LCD Register Select port     A A
    DEFINE LCD_RSBIT 3                ; Set LCD Register Select bit      3 2
    DEFINE LCD_EREG PORTA             ; Set LCD Enable port              A A
    DEFINE LCD_EBIT 1                 ; Set LCD Enable bit               1 5
    DEFINE LCD_BITS 4                 ; Set LCD bus size (4 or 8 bits)   4 4
    DEFINE LCD_LINES 2                ; Set number of lines on LCD       2 2
    '----------------------------------------------------------------------------
    clear
    lcdout $FE,1
    ' to convert degrees to radians Z_ * 256 / 90
    ' to convert radians to degrees Z_ * 90 / 256
    Start:
    x = 0
    y = 0
    z = 0  
    
    Z_ = 90                    'put angle in Z_ in degrees
    Z_ = (Z_ * 255)/90     'convert degrees to radians
    Call sincos                 'perform sin and cos on Z_ value
                                   'cos(Z_) = X_ :sin(Z_) = Y_
    
    lcdout $FE,1,"cos Z=",#X_                                          
    lcdout $FE,$C0,"sin Z=",#Y_ 
    end
    And the pic16 trig file: http://sites.picbasic.net/media/uhp2...trig_pic16.txt
    http://www.scalerobotics.com

  30. #30
    Join Date
    Aug 2003
    Location
    Australia
    Posts
    550

    Default

    Nice work, but I am also challenged with maths.
    Will this allow me to do point rotation?

    Say I have an origin x=0 & y=0, and another point x=4, y=4.
    Can I find out the angle of the second coordinates where the first set is the origin,
    and then rotate the second point say 180 degrees around the origin ?
    Art.

  31. #31
    Join Date
    Aug 2003
    Location
    Australia
    Posts
    550

    Default

    Uh-oh.. just including the line:
    include "trig-pic16.inc"
    causes this:

    Error C:\PROGRA~1\MPLAB\PROJECT\SCROLL.ASM 213 : [235] Opcode Expected Instead of 'dt'
    Error C:\PROGRA~1\MPLAB\PROJECT\SCROLL.ASM 216 : [235] Opcode Expected Instead of 'dt'
    Error C:\PROGRA~1\MPLAB\PROJECT\SCROLL.ASM 219 : [235] Opcode Expected Instead of 'dt'
    Error C:\PROGRA~1\MPLAB\PROJECT\SCROLL.ASM 222 : [235] Opcode Expected Instead of 'dt'
    Error C:\PROGRA~1\MPLAB\PROJECT\SCROLL.ASM 237 : [226] Numeric Constant or Symbol Name Expected
    Error C:\PROGRA~1\MPLAB\PROJECT\SCROLL.ASM 239 : [226] Numeric Constant or Symbol Name Expected
    Error C:\PROGRA~1\MPLAB\PROJECT\SCROLL.ASM 269 : [235] Opcode Expected Instead of 'movfw'
    Error C:\PROGRA~1\MPLAB\PROJECT\SCROLL.ASM 283 : [235] Opcode Expected Instead of 'movfw'
    Error C:\PROGRA~1\MPLAB\PROJECT\SCROLL.ASM 301 : [235] Opcode Expected Instead of 'movfw'
    Error C:\PROGRA~1\MPLAB\PROJECT\SCROLL.ASM 306 : [235] Opcode Expected Instead of 'movfw'
    Error C:\PROGRA~1\MPLAB\PROJECT\SCROLL.ASM 316 : [235] Opcode Expected Instead of 'movfw'
    Error C:\PROGRA~1\MPLAB\PROJECT\SCROLL.ASM 321 : [235] Opcode Expected Instead of 'movfw'
    Error C:\PROGRA~1\MPLAB\PROJECT\SCROLL.ASM 349 : [235] Opcode Expected Instead of 'movfw'
    Warn C:\PROGRA~1\MPLAB\PROJECT\SCROLL.ASM 934 : [102] Code Crosses Boundary @ 800h

    I'm using a 16F877 too.

  32. #32
    Join Date
    Jul 2003
    Location
    Colorado Springs
    Posts
    4,935

    Default

    Use MPASM for the assembler.
    <br>
    DT

  33. #33
    Join Date
    Aug 2003
    Location
    Australia
    Posts
    550

    Default

    Thanks Darrel,
    I'm working on hardware now, but I'll try again as soon as possible.
    Any idea on my first question?

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

    Default

    Quote Originally Posted by Art View Post
    Nice work, but I am also challenged with maths.
    Will this allow me to do point rotation?

    Say I have an origin x=0 & y=0, and another point x=4, y=4.
    Can I find out the angle of the second coordinates where the first set is the origin,
    and then rotate the second point say 180 degrees around the origin ?
    Art.
    Hi Art,

    You can definitely find the angle in the first part of your question. As far as rotation, I have to think a little more. Should be possible though.

    Are you rotating one of your LED displays? If so, are we talking full rotation, or just 90, 180, 270 degrees?

  35. #35
    Join Date
    Aug 2003
    Location
    Australia
    Posts
    550

    Default

    Ideally, to potentially be able to do full rotation of a pixel.
    Yes, I have a matrix LED display in mind
    I wouldn't care whether or not it is equally divided into 360 degrees.
    I already have made routines that make it easy to draw to the screen.

    I can do this in C on the Sony PSP like this
    Code:
    xx = 240;
    yy = 30;
    x = xx - 240;
    y = yy - 136;
    
    theta = rotation;
    thetab = 360 - theta;
    thetab = (3.141592 / 180) * theta;
    
    xnew = cos(thetab)*x - sin(thetab)*y;
    ynew = sin(thetab)*x + cos(thetab)*y;
    
    xnew = xnew + 240;
    ynew = ynew + 136;
    Where x-240, y-136 are coordinates for the centre of the screen,
    x-240,y-30 is the arbitrary point I want to rotate (the one given is a point directly above the centre of the screen),
    rotation is the real angle 0-359 (0 can also be 360).
    xnew and ynew are the new (rotated) coordinates.
    I don't know if this works with microcontrollers even if you do use a C compiler.

  36. #36
    Join Date
    Aug 2003
    Location
    Australia
    Posts
    550

    Default

    Quote Originally Posted by Darrel Taylor View Post
    Use MPASM for the assembler.
    <br>
    It seems that this line becomes offensive:
    Code:
    '@ DEVICE LVP_OFF,BOD_OFF,HS_OSC				'
    Which is how I set configuration.
    Art.
    Last edited by Art; - 29th January 2010 at 12:41.

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

    Default

    You need to use a different syntax for MPASM.

    like this:
    Code:
    Example: To set the PIC for Internal Oscillator allowing use of the OSC pins as I/O...
    
    @ __config _INTRC_OSC_NOCLKOUT
    
    Only one config statement is allowed when using MPASM, so multiple definitions follow-on from each other each connected to it’s previous buddy with an &.
    
    Example: Typical Settings for a 16F628…
    
    @ __config _INTRC_OSC_NOCLKOUT & _WDT_ON & _PWRTE_ON & _MCLRE_OFF & _BODEN_ON & _LVP_OFF & _CP_ALL & _DATA_CP_ON
    More (much more) here: http://www.picbasic.co.uk/forum/showthread.php?t=543

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

    Default

    Quote Originally Posted by Art View Post
    Ideally, to potentially be able to do full rotation of a pixel.
    Yes, I have a matrix LED display in mind
    I wouldn't care whether or not it is equally divided into 360 degrees.
    I already have made routines that make it easy to draw to the screen.

    I can do this in C on the Sony PSP like this
    Code:
    xx = 240;
    yy = 30;
    x = xx - 240;
    y = yy - 136;
    
    theta = rotation;
    thetab = 360 - theta;
    thetab = (3.141592 / 180) * theta;
    
    xnew = cos(thetab)*x - sin(thetab)*y;
    ynew = sin(thetab)*x + cos(thetab)*y;
    
    xnew = xnew + 240;
    ynew = ynew + 136;
    Where x-240, y-136 are coordinates for the centre of the screen,
    x-240,y-30 is the arbitrary point I want to rotate (the one given is a point directly above the centre of the screen),
    rotation is the real angle 0-359 (0 can also be 360).
    xnew and ynew are the new (rotated) coordinates.
    I don't know if this works with microcontrollers even if you do use a C compiler.
    Yes, it looks like you could use the same type of math. The cordic lets you compute sin and cos in a single operation. It is very quick with this type of thing, and can perform about 2000 of these per second.

    So you would just have to solve:



    as you have in your above equation. The PIC18 cordic is a little better, and I documented it more. But if you still want to do it with your PIC16, I am here to help.

    Wikipedia talks a little bit about rotation here: http://en.wikipedia.org/wiki/Rotation_%28mathematics%29
    Attached Images Attached Images  
    Last edited by ScaleRobotics; - 31st January 2010 at 05:20.

  39. #39
    Join Date
    Aug 2003
    Location
    Australia
    Posts
    550

    Default

    I'm not quite ready to do it yet, only because I'm in the middle of something else,
    but regarding the LED displays, I've got a blitter routine handling sprites now.
    It would be really nice to see if it could handle rotation of a sprite (pixel by pixel).

    Thanks for your offer to help, I'll surely take you up on it very soon

  40. #40
    Join Date
    Apr 2010
    Location
    Gavle, Sweden
    Posts
    3

    Default Re: Cordic trig assembly code for PIC18f

    Hi !
    I'm trying to write a program to calculate sunrise and sunset. The pic is 18F2550 and I'm using long variables and PBPL. I've tried to use the TRIG.INC but it seems that I cant use it in PBPL, Error: DIV32. Is there a way to rewrite the program so that it will work with PBPL?
    I'm using Picbasic Pro 2.50 and MPASM 8.15.
    Bernt-Ove

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 : 6

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