|
Actually, it is perfect!!!!
They do something weird to signify the angle. For actual angle z, degrees = z * 90/256, or z/2.844444444444 = degrees. Output/1000 = the fractional result.
Attached is part of their excel spread sheet showing inputs and outputs vs actuals.
<img src="http://www.picbasic.co.uk/forum/attachment.php?attachmentid=2134&d=1195310172">
Many thanks for your time,
Walter
Last edited by ScaleRobotics; - 17th November 2007 at 15:17.
|
That helps! Now the SIN and COS results all fall into place.For actual angle z, degrees = z * 90/256
But, still having problems with the ATAN function.
Can't seem to get anything meaningfull out of it.
And that spreadsheet won't run for me, so I can't tell what it's supposed to do.
Do you have any examples for that too?
<br>
DT
|
I have another excel example that I will post (when I get to my other computer).This one can't look at it either. But basically, it works like this:
Enter X and Y co-ordinates like x = 12631 and y = 11881, then perform atan.
<img src="http://www.picbasic.co.uk/forum/attachment.php?attachmentid=2136&d=1195349246">
The accuracy shows that when either x or y is really low, that errors in the way it simplifies the equation are a bit large. But with higher numbers, the accuracy is within one degree or so.
Result pops out as Z. Z is the angle to the co-ordinates. (if I remember right the answer was 134?), but it is in the same weird format. Z * 90 / 256 gets you back to a readable degree format, which if I remember right give you about 47 degrees. You will have to know what quadrant you are in to figure out if you add 90 degrees to result by figuring out if x or y or both are positive or negative.
I made the 20 mhz PIC do 10,000 of these calculations, and it took less than 5 seconds. Over 2000 per second! Pretty cool!
I wish I got this excited about trig 22 years ago, when I was taking the class ......
Last edited by ScaleRobotics; - 18th November 2007 at 01:35.
|
looking at the asm... i'm aware that my asm skills sucks, but, i don't understand why they used the line in red...
w already hold 0x00 and they reload it again for the next MOVWF? BTW i would use CLRF instead... no big deal, it remove 4 instructions... long run, it may decrease a little bit the calculation latency. OK OK no big improvement, but... i couldn't resist. maybe i'll find something else?Code:ATAN banksel _Z_H ; Z_H & Z_L is loaded with 0x0000 movlw 0x00 ; Y & X is the input movwf _Z_H ; X(i+1) = Xi - Yi*_Di*2^-i movlw 0x00 ; Y(I+1) = Yi + Xi*_Di*2^-i movwf _Z_L ; Z(i+1) = Zi - _Di*ATAN(2^-i) banksel _Di ; _Di = 1 if Yi < 0 ELSE _Di = 0 bsf RV ; Set Vectoring Mode goto CORDIC_Alg SINCOS banksel _Y_L ; Z_H & Z_L is the input movlw 0x00 ; Y_H & Y_L is loaded with 0x0000 movwf _Y_L ; X_H & X_L is loaded with 6070 (1/An constant) movlw 0x00 movwf _Y_H movlw LOW .6070 movwf _X_L ; X(i+1) = Xi - Yi*_Di*2^-i movlw HIGH .6070 ; Y(I+1) = Yi + Xi*_Di*2^-i movwf _X_H ; Z(i+1) = Zi - _Di*ATAN(2^-i) banksel _Di ; _Di = 0 if Zi < 0 ELSE _Di = 1 bcf RV ; Set Rotational Mode
so, once modified, the above code section should look like...
But yeah, i think we're not at the optimization stage yetCode:ATAN CHK?RP _Z_H ; Z_H & Z_L is loaded with 0x0000 ; Y & X is the input clrf _Z_H ; X(i+1) = Xi - Yi*_Di*2^-i ; Y(I+1) = Yi + Xi*_Di*2^-i clrf _Z_L ; Z(i+1) = Zi - _Di*ATAN(2^-i) CHK?RP _Di ; _Di = 1 if Yi < 0 ELSE _Di = 0 bsf RV ; Set Vectoring Mode goto CORDIC_Alg SINCOS CHK?RP _Y_L ; Z_H & Z_L is the input ; Y_H & Y_L is loaded with 0x0000 clrf _Y_L ; X_H & X_L is loaded with 6070 (1/An constant) clrf _Y_H movlw LOW .6070 movwf _X_L ; X(i+1) = Xi - Yi*_Di*2^-i movlw HIGH .6070 ; Y(I+1) = Yi + Xi*_Di*2^-i movwf _X_H ; Z(i+1) = Zi - _Di*ATAN(2^-i) CHK?RP _Di ; _Di = 0 if Zi < 0 ELSE _Di = 1 bcf RV ; Set Rotational Mode
Last edited by mister_e; - 18th November 2007 at 02:34.
Steve
It's not a bug, it's a random feature.
There's no problem, only learning opportunities.
|
Cordic Trig Include file for PIC16
Looks great! Good catch.
Here is a paste able version of the code to integrate into picbasic. (just takes up about 180 words). It loads the bytes needed for assembly. It uses X_ Y_ and Z_ word variables to load the bytes needed for the assembly routine.
Code:'**************************************************************** '* Name : trig-pic16.inc * '* Author : Walter Dunckel * '* Notice : Copyright (c) 2009 Scale Robotics Inc. * '* : All Rights Reserved * '* Date : 9/6/2011 * '* Version : 2.1 * '* Notes : Consolodated some variables to save ram and space * '* : * '* to convert degrees to radians Z_ * 256 / 90 * '* to convert radians to degrees Z_ * 90 / 256 * '* * '* to perform ATAN, enter values for X_ and Y_ : * '* X_ = 12631 * '* Y_ = 11881 * '* call ATAN * '* 'result will be in Z_ radians = 134 * '* Z_ * 90/256 = 47 degrees * '* * '* To find the result of Cos and Sin: * '*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 * '* 'results will be located here: cos(Z_) = X_ :sin(Z_) = Y_ * '* in thousandths, for instance 9999 ~ 1.000 * '**************************************************************** 'more info: http://www.picbasic.co.uk/forum/showthread.php?t=7502 'and PIC18 version http://www.picbasic.co.uk/forum/showthread.php?t=10528 X_ var word bank0 Y_ var word bank0 Z_ var word bank0 X_H VAR X_.byte1 X_L VAR X_.byte0 Y_H VAR Y_.byte1 Y_L VAR Y_.byte0 Z_H VAR Z_.byte1 Z_L VAR Z_.byte0 X_TMP_H VAR byte bank0 X_TMP_L VAR byte bank0 Y_TMP_H VAR byte bank0 Y_TMP_L VAR byte bank0 ITERAT_TMP VAR byte bank0 ITERATION VAR byte bank0 Di VAR byte bank0 COSCH VAR byte bank0 COSCL VAR byte bank0 Counter1 VAR byte bank0 SHIFT VAR byte bank0 '*********************************** goto jump_trig asm ;__________________________________________________________________________ FSR_ptr macro addr CHK?RP addr bcf STATUS,IRP btfsc STATUS,RP1 bsf STATUS,IRP movlw LOW addr movwf FSR endm #define Dir _Di,0 #define RV _Di,1 #define _C STATUS,0 #define _Z STATUS,2 Z_stepH_tbl addwf PCL,f dt 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00 Z_stepL_tbl addwf PCL,f dt 0x80,0x4C,0x28,0x14,0x0A,0x05,0x03,0x01 Z_NstepH_tbl addwf PCL,f dt 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF Z_NstepL_tbl addwf PCL,f dt 0x80,0xB4,0xD8,0xEC,0xF6,0xFB,0xFD,0xFF _ATAN CHK?RP _Z_H ; Z_H & Z_L is loaded with 0x0000 clrf _Z_H ; Y & X is the input clrf _Z_L ; X(i+1) = Xi - Yi*_Di*2^-i ; Y(I+1) = Yi + Xi*_Di*2^-i ; Z(i+1) = Zi - _Di*ATAN(2^-i) CHK?RP _Di ; _Di = 1 if Yi < 0 ELSE _Di = 0 bsf RV ; Set Vectoring Mode goto CORDIC_Alg _SINCOS CHK?RP _Y_L ; Z_H & Z_L is the input clrf _Y_L ; Y_H & Y_L is loaded with 0x0000 clrf _Y_H ; X_H & X_L is loaded with 6070 (1/An constant) movlw LOW .6070 movwf _X_L ; X(i+1) = Xi - Yi*_Di*2^-i movlw HIGH .6070 ; Y(I+1) = Yi + Xi*_Di*2^-i movwf _X_H ; Z(i+1) = Zi - _Di*ATAN(2^-i) CHK?RP _Di ; _Di = 0 if Zi < 0 ELSE _Di = 1 bcf RV ; Set Rotational Mode CORDIC_Alg CHK?RP _ITERATION clrf _ITERATION Rotation_loop BUFF_X_Y ; Buffers both X & Y into temporary registers CHK?RP _Y_L movf _Y_L, W ; Move Y --> T_TEMP movwf _Y_TMP_L movf _Y_H, W movwf _Y_TMP_H movf _X_L, W ; Move Y --> T_TEMP movwf _X_TMP_L movf _X_H, W movwf _X_TMP_H CHK?RP _Di ; Vectoring or Rotational Mode? bcf Dir btfsc RV goto Rot Vec btfss _Z_H, 7 ; Determine the state of _Di (1 OR -1) bsf Dir ; _Di=1 when Z_H is pos goto Cordic Rot btfsc _Y_H, 7 ; Determine the state of _Di (1 OR -1) bsf Dir ; _Di=1 when Y_H is neg Cordic movfw _ITERATION movwf _SHIFT ; Load number of shifts bsf _SHIFT,7 ; Shift right (Division) FSR_ptr _Y_TMP_L ; Load FSR call Shifter ; Execute shift btfsc Dir call COM_YTEMP ; Get the 2's complement of teh TEMP register SUM_X ; Calculate X(i+1) movf _Y_TMP_H, W addwf _X_H, F movf _Y_TMP_L, W addwf _X_L, F btfsc _C incf _X_H, F movfw _ITERATION movwf _SHIFT ; Load number of shifts bsf _SHIFT,7 ; Shift right (Division) FSR_ptr _X_TMP_L ; Load FSR call Shifter ; Execute shift btfss Dir call COM_XTEMP ; Get the 2's complement of teh TEMP register SUM_Y ; Calulate Y(i+1) movf _X_TMP_H, W addwf _Y_H, F movf _X_TMP_L, W addwf _Y_L, F btfsc _C incf _Y_H, F btfss Dir goto ADD_Z movlw HIGH Z_NstepH_tbl movwf PCLATH movfw _ITERATION ; Fetch appropriate value from table call Z_NstepH_tbl addwf _Z_H, F movlw HIGH Z_NstepL_tbl movwf PCLATH movfw _ITERATION call Z_NstepL_tbl addwf _Z_L, F btfsc _C incf _Z_H, F goto Rot_Loop_out ADD_Z movlw HIGH Z_stepL_tbl movwf PCLATH movfw _ITERATION call Z_stepH_tbl addwf _Z_H, F movlw HIGH Z_stepH_tbl movwf PCLATH movfw _ITERATION call Z_stepL_tbl addwf _Z_L, F btfsc _C incf _Z_H, F Rot_Loop_out incf _ITERATION,f btfsc _ITERATION,3 return goto Rotation_loop COM_YTEMP comf _Y_TMP_L, F ; Complememt Y_TMP comf _Y_TMP_H, F movlw 01H addwf _Y_TMP_L, F btfsc _C incf _Y_TMP_H, F return COM_XTEMP comf _X_TMP_L, F ; Complememt X_TMP comf _X_TMP_H, F movlw 01H addwf _X_TMP_L, F btfsc _C incf _X_TMP_H, F return Shifter ; Function for shifting 16bit signed REG i times CHK?RP _SHIFT ; left/right MUL/DIV movfw _SHIFT ; INPUTS: FSR & IRP are pointing to LOW REG andlw 0x0F ; SHIFT = d000 XXXX where XXXX = # of shifts btfsc _Z ; and d = dir of shift return ; check for zero interations movwf _Counter1 btfss _SHIFT,0x7 goto CORDIC_MUL CORDIC_DIV rrf INDF, F ; Divide INDF/2 INDF --> low byte bcf INDF, 7 decf FSR, F rrf INDF, F incf FSR, F btfsc _C bsf INDF, 7 ; Carry from the upper byte to the lower byte decf FSR, F bcf INDF, 7 ;Check to see if TEMP should be positive number btfsc INDF, 6 bsf INDF, 7 ;Was a negative number, force it negative again incf FSR, F decfsz _Counter1, F goto CORDIC_DIV return CORDIC_MUL bcf _C rlf INDF, F decf FSR, F rlf INDF, F bcf INDF, 7 btfsc _C bsf INDF, 7 incf FSR, F decfsz _Counter1, F goto CORDIC_MUL return ;______________________________________________________________________________ endasm jump_trig:
Last edited by ScaleRobotics; - 7th September 2011 at 18:09. Reason: Cleared up code a little, and added description
|
Then you can do some pretty cool stuff!
Like find the bearing to and distance to a lat-lon waypoint (solving for latitude adjustment of Longitude!)
Here is my ugly code. Needs to be cleaned up ... but it works for waypoints that are within 3.2000 degrees of eachother (200 miles or so). Angle still needs to be corrected depending which quadrant the waypoint lies.
Code:DEFINE __16F877A 1 ; list directive to define processor DEFINE OSC 20 ' Define Oscillator Speed DEFINE LOADER_USED 1 ' 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 X_H VAR byte X_L VAR byte Y_H VAR byte Y_L VAR byte Z_H VAR byte Z_L VAR byte X_TMP_H VAR byte X_TMP_L VAR byte Y_TMP_H VAR byte Y_TMP_L VAR byte ITERAT_TMP VAR byte ITERATION VAR byte Di VAR byte COSCH VAR byte COSCL VAR byte Counter1 VAR byte SHIFT VAR byte 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 pause 500 lcdout $FE,1,"Test" ' to convert degrees to radians Z_ * 256 / 90 ' to convert radians to degrees Z_ * 90 / 256 Start: x = 0 y = 0 z = 37 'input current latitude 37.xxx for longitude correction (lon smaller at higher latitude) lat_dif = 11881 'difference between current position and waypoint in latitude 1.1881 deg lon_dif = 15816 'difference between current position and waypoint in longitude 1.5816 deg i = z * 256 / 90 'changes z to the right format +256 to -256 'each 1/3 degree should equal about 1 radian need to figure out how to get this accuracy...... if (z*256)//90 >= 45 then i = i +1 'if answer is .5 or more, go to next highest integer (round number) Z_ = i 'put result in Z_ Call sincos i = X_ * lon_dif ' X_ = cos(lat_current) lon_dif = div32 10000 ' get the value of lon_dif * the fraction for lon correction (reduces lon_dif) X_ = lon_dif 'load lon_dif into X_ , prepare for ATAN function Y_ = lat_dif 'load lat_dif into Y_ , prepare for ATAN function call atan 'perform ATAN assembly function angle = Z_ angle_deg = Z_ *90/256 'puts angle in degrees into the variable 'Below code computes distance between waypoints 'Z_ is already loaded with angle from above result call sincos i = lon_dif * 10000 'get lon_dif ready to divide by fraction distance = div32 X_ 'divide by cos(Z_)gives latitude distance in d.dddd or really ddddd distance = distance * 69 'get miles units distance = div32 100 'hundreths of miles xx.xx miles lcdout $FE,1,#distance lcdout $FE,$C0,#angle_deg 'not corrected for quadrant, ie. + 180, or + 90, etc. only bare triangle angle end '***********************************
Last edited by ScaleRobotics; - 18th November 2007 at 16:49.
|
For anyone interested, the latest code was posted by Scalerobotics here:
http://sites.picbasic.net/index.php?...d=91&pageid=62
(the downloads don't seem to be working right now)
I believe the 16 bit code as posted on the above link did need the following minor tweaks:
Instead of:
lathome_min = lathome_min + 33*((((gpsdata[char_A+4] -48)*1000)+((gpsdata[char_A+5]-48)*100)+((gpsdata[char_A+7]-48)*10)+(gpsdata[char_A+8]-48))*5)//3
However, I believe it should read as follows so that the modulus is calculated first:
lathome_min = lathome_min + 33*(((((gpsdata[char_A+4] -48)*1000)+((gpsdata[char_A+5]-48)*100)+((gpsdata[char_A+7]-48)*10)+(gpsdata[char_A+8]-48))*5)//3)
Second:
latdest_min = latdest_min + (tempbyte*33) 'use remainder and add to latdest_min
should read as follows:
latdest_min = latdest_min*5/3 + (tempbyte*33) 'use remainder and add to latdest_min
Members who have read this thread : 1You do not have permission to view the list of names.
Posting Permissions
Bookmarks