|
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.Originally Posted by scalerobotics
If you do not believe in MAGIC, Consider how currency has value simply by printing it, and is then traded for real assets.
.
Gold is the money of kings, silver is the money of gentlemen, barter is the money of peasants - but debt is the money of slaves
.
There simply is no "Happy Spam" If you do it you will disappear from this forum.
|
Well now, that's better than all my blabbering! Looks like you got it down.
|
16 bit trig for sin cos tan2 and hypotenuse
Here's one for people like me who think in degrees and not radians:
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
Last edited by ScaleRobotics; - 15th February 2012 at 15:37. Reason: fixed sincos bug corrected in v1.5
|
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
[email protected]
(740) 357-8016 cell
|
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
|
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
|
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:
And the pic16 trig file: http://sites.picbasic.net/media/uhp2...trig_pic16.txtCode:'**************************************************************** '* 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
http://www.scalerobotics.com
Members who have read this thread : 1You do not have permission to view the list of names.
Posting Permissions
Bookmarks