Navigate2.1b
This code calculates the relative (X,Y) position based on (1) a direction or heading and (2) motion,expressed in the form of wheel displacement. A second routine calculates the direction and the distance to a gridpoint that was specified as the target.
It depends partly on the division/multiplication routines as presented in Atmels appnote 200 although I made minor modifications to them to suit the requirements of this program.
Please read the comments in the .asm file for more information.
The robot sees the world as a 2-dimensional grid with it's starting point (i.e.) charger station
as coordinate 0,0. The Y-axis is the 0 degree line (north) on the compass module.
A target can be specified as a set of X-Y coordinates or as a distance plus a compass angle.
The latter type will be internally converted to it's X-Y coordinates.
The unit of measurement is a state-change on the wheelencoder. This is appr 0,9162 mm
The program uses 16-bit signed numbers to avoid extensive floating point programming.
sin & cos are multiplied by 32768. This number is chosen as it delivers optimal precision
under the selected criteria. Only tan is multiplied by 512 to make the result fit in a 16 bit
signed number.
The final results are divided by these amounts to achieve the correct outcome.
Design limits:
A signed 2-dimensional grid that uses 2-byte wide variables to store X and Y coordinates
imposes a maximum distance in any direction from the point of origin.
This number equals 2^15 or 32768 units. (300,22m)
To make the calculations with the desired precision, the use of 4-byte variables is required.
Variables:
T = target
Tx = target X-coordinate
Ty = target Y-coordinate
Px(n) = map-point X-coordinate of next point to be reached
Py(n) = map-point Y-coordinate ,,
D(n) = travelled distance in current interval
@(n) = alfa(n), compass angle for current interval
Px(n-1) = map-point X-coordinate of last point reached
Py(n-1) = map-point Y-coordinate ,,
ODO(n) = current ODO-reading
ODO(n-1) = last ODO-reading (saved on the last position calculation)
Formulae
Grid position:
Px(n) = (D(n) x (sin @(n)*32768))/32768 + Px(n-1)
Py(n) = (D(n) x (cos @(n)*32768))/32768 + Py(n-1)
Heading to target:
Tx - Px(n)/Ty - Py(n)= tan (compass angle to destination)
This will give two solutions that differ 180deg.
We can use the sign of Tx - Px(n) & Ty - Py(n) to distinguish
If Tx - Px(n)>0 AND Ty - Py(n)>0 angle = 0-90
If Tx - Px(n)>0 AND Ty - Py(n)<0 angle = 90-180
If Tx - Px(n)<0 AND Ty - Py(n)<0 angle = 180-270
If Tx - Px(n)<0 AND Ty - Py(n)>0 angle = 270-360
This can also be used to limit the search range to one quadrant.
;distance to target
;(Tx - Px(n)*32768) / (sin (angle to destination)*32768)
; or
;(Ty - Py(n)*32768) / (cos (angle to destination)*32768)
;dividend must be a 32bit number to keep the resolution
Machine coding
Target lists
Tx,Ty 4 bytes for each target, maximum 8 consequetive targets
Current grid point storage
Px(n),Py(n) 4 bytes
Last grid point storage
Px(n-1),Py(n-1) 4 bytes
compass reports angles in 10th of degrees. angles are rounded off to nearest degree
31,1 till 31,5 =31
31,6 till 31,9 =32
add 5 to reported angle, then divide by ten using avr200.asm.
the result is a 16-bit offset to the location (address) where the required value can be found.
Goniotable:
sin/cos * 32768, one word per degree. if sin or cos equals 1, rounded off to 32767
tan * 512. tan 90 and tan 270 are infinite. rounded off to 32767
To increase precision, straight line moves are recalculated using the original starting point.
When the compass heading remains the same, the wheel displacement is added to
navodols00/navodors00. the originating grid position is also saved.
to detemine if the heading remains the same, we use the raw compass course + or - a configurable
number (navcompdiff) preset at 16 (+-1.6 degree)
this fairly large margin also partially accounts for sudden deviations of the compass.
when the compass angle changes beyond the treshold, the position that is previous to the new
course is calculated first.
This code calculates the relative (X,Y) position based on (1) a direction or heading and (2) motion,expressed in the form of wheel displacement. A second routine calculates the direction and the distance to a gridpoint that was specified as the target.
It depends partly on the division/multiplication routines as presented in Atmels appnote 200 although I made minor modifications to them to suit the requirements of this program.
Please read the comments in the .asm file for more information.
The robot sees the world as a 2-dimensional grid with it's starting point (i.e.) charger station
as coordinate 0,0. The Y-axis is the 0 degree line (north) on the compass module.
A target can be specified as a set of X-Y coordinates or as a distance plus a compass angle.
The latter type will be internally converted to it's X-Y coordinates.
The unit of measurement is a state-change on the wheelencoder. This is appr 0,9162 mm
The program uses 16-bit signed numbers to avoid extensive floating point programming.
sin & cos are multiplied by 32768. This number is chosen as it delivers optimal precision
under the selected criteria. Only tan is multiplied by 512 to make the result fit in a 16 bit
signed number.
The final results are divided by these amounts to achieve the correct outcome.
Design limits:
A signed 2-dimensional grid that uses 2-byte wide variables to store X and Y coordinates
imposes a maximum distance in any direction from the point of origin.
This number equals 2^15 or 32768 units. (300,22m)
To make the calculations with the desired precision, the use of 4-byte variables is required.
Variables:
T = target
Tx = target X-coordinate
Ty = target Y-coordinate
Px(n) = map-point X-coordinate of next point to be reached
Py(n) = map-point Y-coordinate ,,
D(n) = travelled distance in current interval
@(n) = alfa(n), compass angle for current interval
Px(n-1) = map-point X-coordinate of last point reached
Py(n-1) = map-point Y-coordinate ,,
ODO(n) = current ODO-reading
ODO(n-1) = last ODO-reading (saved on the last position calculation)
Formulae
Grid position:
Px(n) = (D(n) x (sin @(n)*32768))/32768 + Px(n-1)
Py(n) = (D(n) x (cos @(n)*32768))/32768 + Py(n-1)
Heading to target:
Tx - Px(n)/Ty - Py(n)= tan (compass angle to destination)
This will give two solutions that differ 180deg.
We can use the sign of Tx - Px(n) & Ty - Py(n) to distinguish
If Tx - Px(n)>0 AND Ty - Py(n)>0 angle = 0-90
If Tx - Px(n)>0 AND Ty - Py(n)<0 angle = 90-180
If Tx - Px(n)<0 AND Ty - Py(n)<0 angle = 180-270
If Tx - Px(n)<0 AND Ty - Py(n)>0 angle = 270-360
This can also be used to limit the search range to one quadrant.
;distance to target
;(Tx - Px(n)*32768) / (sin (angle to destination)*32768)
; or
;(Ty - Py(n)*32768) / (cos (angle to destination)*32768)
;dividend must be a 32bit number to keep the resolution
Machine coding
Target lists
Tx,Ty 4 bytes for each target, maximum 8 consequetive targets
Current grid point storage
Px(n),Py(n) 4 bytes
Last grid point storage
Px(n-1),Py(n-1) 4 bytes
compass reports angles in 10th of degrees. angles are rounded off to nearest degree
31,1 till 31,5 =31
31,6 till 31,9 =32
add 5 to reported angle, then divide by ten using avr200.asm.
the result is a 16-bit offset to the location (address) where the required value can be found.
Goniotable:
sin/cos * 32768, one word per degree. if sin or cos equals 1, rounded off to 32767
tan * 512. tan 90 and tan 270 are infinite. rounded off to 32767
To increase precision, straight line moves are recalculated using the original starting point.
When the compass heading remains the same, the wheel displacement is added to
navodols00/navodors00. the originating grid position is also saved.
to detemine if the heading remains the same, we use the raw compass course + or - a configurable
number (navcompdiff) preset at 16 (+-1.6 degree)
this fairly large margin also partially accounts for sudden deviations of the compass.
when the compass angle changes beyond the treshold, the position that is previous to the new
course is calculated first.
| CODE |
| ;-------------------------------------------------------------------------- ;Navigation.asm ; ;version 2.1b ;(c) L Gijssel ;nov 2005 ;lgijssel@neon-networking.nl ; ;What does it do? ;Navigation system for a small robot platform. For this purpose, the program creates a virtual ;two-dimensional grid (X,Y) using the ODO-ticks as the basic units. The program runs entirely ;in AVR assembler and should on anything beyond a Mega16. ;It depends partly on the division/multiplication routines as presented in Atmels appnote 200 ;although I made minor modifications to them to suit the requirements of this program. ; ;How does it work? ;This code calculates the relative position based on (1) a direction or heading and (2) motion, ;expressed in the form of wheel displacement. It also calculates the direction and the ;distance to a gridpoint that was specified as the target. The calculation method used is ;quite similar to what sailors did in earlier days. Now don't come argueing that I should have ;developed a system using satellite guidance. These systems still only work well when outdoors ;and their accuracy is +/-3 to 5m. What we have here is designed for indoor use and with an ;accuracy that, altough far from 100% should still be better than that. ;Requirements: ; Odometry signal to register wheel displacement. ; A robot should ideally be fitted with two wheel sensors. ; ; Compass or other device to read the current compass heading. ; I am using a Devantech CMPS03 module with I2C interface for this. ; ;You may need to hack the way you enter this data into the program. ;I left my proprietary solution to give an idea of how this can be done. ; ;Performance: ;The range that can be covered with this solution is defined by the fact that the grid ;position is stored as a 16bit signed number. (32767 till -32768 for X and Y) ;If 1 grid increment resembles 1cm, this allows a range of over 327m in any direction. ;After a reset, the robot assumes grid position 00,00 but could be preset to any position. ; ;The resolution of the system is equal to the smallest wheel displacement that is seen by ;the ODO system. For my case this distance is 1cm. Wheel encoders with more divisions will ;increase accuracy but reduce the range. ; ;The system does not make use of floating point numbers nor does it calculate sin/cos values. ;The goniometric data that is used comes from a table in flash where each entry was multiplied ;by a fixed number. Sin/cos were multiplied by 32768 (2^15) Tan was multiplied by 512 (2^9) ;After making the required calculations, the multiplication factor is rolled back sending ;everything behind the comma to Oblivion. ; ;The Goniometric data table has one entry per degree: 360 entries for sin/cos/tan. ;Each entry is a 16bit word. The table-size is a second source of inaccuracy but once again, ;this is a trade-off between performance and available resources. ;The table could be extended to contain half degrees as well but this assumes you have lots of ;flash. Besides, there is no real point in it (at least not for me) as the compass I am using ;has an accuracy of about 3 degrees. ; ;These limitations makes the system pretty fast but you should not expect a 99,999% accuracy. ;When used with an understanding of the calculation method used and taking it's limitations ;into account, it should be possible to come pretty near to the point where you intended to go! ; ;Usage guidelines ;1: Everything behind the comma is lost! ;For optimal precision it is better to make calculations with large numbers whenever possible. ;This is because we do not take digits behind the comma into account. The error will be larger ;when you are calculating 10 times a wheel displacement of 3 units as opposed to making this ;calculation once for 30 units. ; ;2:Drive straight! ;when driving in a straight line, (that is: remaining on the same heading +/- x degrees) the ;program accumulates the distance and recalculates it's grid position from where it started ;the straight line. ; ;3:Turn on the spot! ;The biggest errors occur when the robot is moving in a curved line. The program needs to make ;many calculations over a relatively short distance and these errors will add up. ;This can be prevented by turning on the spot. The wheel displacements will then be opposite ;and practically zero eachother out. ; ;Possible improvements: ;The code can still be made leaner & meaner. First goal was stability and a working result. ;- Target lists will be added to give the robot a row of coordinates to move to. The program ;knows only one target but this can be altered by software when it has been reached. ;- Interaction with motor control is rather application specific and hence not a part of this ;code. Another application should take care of that using the input of the navigation system. ; ;Known issues: ;Overflow will occur in the calculation of the distance & angle to target when the robot has to ;traverse a distance that is more than 32767 grid positions. This is because the distance is ;saved as a 16bit signed number. Remember your highschool math!? ; ;Unknown issues: ;Huuh, no idea really but I assume there will be some. Please report any errors or ;improvements that you add to this code so that I can correct/improve it. An updated release ;may be posted when appropriate. ; ;That brings me to the following: ;I cannot accept liability when you hack this code into your fathers car and drive up the ;nearst lamppost. Actually, I do not accept any liabitlity for minor or major disasters ;that may occur as a direct or indirect result of the use of this code. ; ;Consider this code free for non commercial use, to be used at your own risk. ; ; ;About the code ;The system consists of two separate routines: ;NAV01 ; Calculates a grid position based on compass angle and wheel displacement ; ;NAVcdh ; Calculates angle and distance to a specified grid coordinate (Target) ; ;The start at NAV00 is proprietary for my own implementation. I run this code in a loop that ;excutes twice a second or so. The compass is read two t0ovf ticks before the code is started. ;You can use another approach to enter the correct values into the variables and run the routines ;as often as you like. Remember the Usage guidelines! ; ;The use of RAM memory is substantial (56 bytes). This is also because I am using my CPU registers ;for many other purposes where many others seem to dedicate a large part of their MCU to just one ;single task. RAM is used to off-load the data from the registers and still keep track. ;I am using a Mega32 (2k RAM) ;Due to the fact that all CPU registers are used for a variety of applications, I preferred to ;keep the names for them at the Atmel default. No fancy register naming for me! ;Anyone may feel free to rename them in the source if this makes him/her happier;-) ; ; .equ navcycles = 2;for testing ;.equ navcycles = 70;normal value, run twice per second .equ navcompdiff=16;differnce of +- n/10 degrees .equ NAVbase = (RAMstart + 0x1A0);could be anywhere .equ navloop = (NAVbase + 0) .equ navcmpsdata00 = (NAVbase + 1) .equ navcmpsdata01 = (NAVbase + 2) .equ navcmpsdata02 = (NAVbase + 3) .equ navcmpsdata03 = (NAVbase + 4) .equ navcmpsdata04 = (NAVbase + 5) ;average distance traversed: .equ navdist00H = (NAVbase + 6) .equ navdist00L = (NAVbase + 7) ;00=current odo, 01=previous .equ navodoleft00H = (NAVbase + 8) .equ navodoleft00L = (NAVbase + 9) .equ navodoright00H = (NAVbase + 10) .equ navodoright00L = (NAVbase + 11) .equ navodoleft01H = (NAVbase + 12) .equ navodoleft01L = (NAVbase + 13) .equ navodoright01H = (NAVbase + 14) .equ navodoright01L = (NAVbase + 15) ;00=current angle, 01=angle to target .equ navsin00H = (NAVbase + 16) .equ navsin00L = (NAVbase + 17) .equ navsin01H = (NAVbase + 18) .equ navsin01L = (NAVbase + 19) .equ navcos00H = (NAVbase + 20) .equ navcos00L = (NAVbase + 21) .equ navcos01H = (NAVbase + 22) .equ navcos01L = (NAVbase + 23) .equ navtan00H = (NAVbase + 24) .equ navtan00L = (NAVbase + 25) .equ navtan01H = (NAVbase + 26) .equ navtan01L = (NAVbase + 27) ;grid positions, current(00) and previous(01) .equ navgridX00H = (NAVbase + 28) .equ navgridX00L = (NAVbase + 29) .equ navgridY00H = (NAVbase + 30) .equ navgridY00L = (NAVbase + 31) .equ navgridX01H = (NAVbase + 32) .equ navgridX01L = (NAVbase + 33) .equ navgridY01H = (NAVbase + 34) .equ navgridY01L = (NAVbase + 35) ;extra grid storage for recalculation of larger distances .equ navgridX02H = (NAVbase + 36) .equ navgridX02L = (NAVbase + 37) .equ navgridY02H = (NAVbase + 38) .equ navgridY02L = (NAVbase + 39) ;distance travelled in a straight line; only 16bits (max 32m straight ahead) .equ navdist02H = (NAVbase + 40) .equ navdist02L = (NAVbase + 41) ;compass reading for straight line .equ navcmpss01 = (NAVbase + 42) .equ navcmpss02 = (NAVbase + 43) ;control variables for straight line calculations .equ navctrl02 = (NAVbase + 44) ;Target lists & ctrl variables .equ navctrl03 = (NAVbase + 45) .equ navTX00H = (NAVbase + 46) .equ navTX00L = (NAVbase + 47) .equ navTY00H = (NAVbase + 48) .equ navTY00L = (NAVbase + 49) ;temp for X/Y distance to target .equ navTscratch00L = (NAVbase + 50) .equ navTscratch00H = (NAVbase + 51) .equ navTscratch01L = (NAVbase + 52) .equ navTscratch01H = (NAVbase + 53) .equ navdist03H = (NAVbase + 54) .equ navdist03L = (NAVbase + 55) NAVinit: ldi YL,low(NAVbase) ldi YH,high(NAVbase) ldi r16,56 clr r13;always zero NAVinit01: st Y+,r13 dec r16 brne NAVinit01 ldi r16,(navcycles/2) sts navloop,r16 ret NAV00: lds r16,navloop cp r16,r13 breq NAV01 cpi r16,2;2 cycles =14,22ms, this should be more than sufficient to read cmps brne NAVexit call NAVgetCMPS;initiate I2C update of compass reading ret NAV01:;navigation routine entry point ;when navloop=0 it must be reset to the starting value ldi r16,navcycles sts navloop,r16 call TWIchkCMPS;check if compass has been read andi r24,$80;if $80: data ready, exit otherwise brne NAV02 ldi r16,1;retry after 7ms sts navloop,r16 NAVexit: ret NAV02:;data in Q ldi YL,low(navcmpsdata00) ldi YH,high(navcmpsdata00) call TWIreadCMPS ;compass bearing is now in navcmpsdata01,02 01=high, 02=low byte. ;The CMPS03 reports the angle as a 2byte number between 0 and 3599. ;Example: 45 degrees equals 450. ; ;check if the compass differs from last reading call navtstS ;check if we have moved, exit if not call NAVchkmove breq NAVexit NAV03:;we have moved, gather data to calculate new position lds ZH,navcmpsdata01 lds ZL,navcmpsdata02 adiw ZH:ZL,5 ldi r18,10;set up divisor to divide Z-contents by ten (unsigned) ldi r19,0;the remainder can be ignored call div16u;with regards to atmel appnote200 ;adjust for 16bit organization of flashmemory lsl ZL rol ZH sts navcmpsdata03,ZH sts navcmpsdata04,ZL rcall goniolookup;find goniometric values for current compass angle rcall navdistance ;calculates the distance traversed. returns a 16bit positive number ;result is in navdist00L,H and also in r16,r17 ;when navctrl02 = 0, navdist02L,H are cleared ;when navctrl02 > 0, navdist02L,H are updated lds r14,navctrl02 and r14,r14 cpse r14,r13 rjmp NAV05 ;calculating a new grid position uses compass angle and traversed distance ;there are two versions of this routine: ;NAV04 when navctrl02 = 0, course is not identical so the most recent grid position is used ;NAV05 when navctrl02 > 0, we are on the same course as in the previous interval. ;in this special case the grid position where we started driving straight is used ;the first case uses navdist00 with navgrid01 as starting point. ;updates navgrid00,01,02 and resets navdist02 ;in the second case navdist02 is used with navgrid02 as starting point. ;navgrid02 remains the same as long as we are driving straight ;updates navgrid00,01 NAV04: ;calculate new grid X-position when navctrl02 = 0 lds r16,navdist00L lds r17,navdist00H lds r18,navsin00L lds r19,navsin00H rcall mpy16s;with regards to atmel appnote200 ;perform division by 32768. fastest solution is to use the high bytes with one shift left. ;this equals multiplying by 65536 divided by 2 clc rol r19;(required to adjust r20's lsb, r18 is not at all significant) rol r20 rol r21 ;result is now in r20,r21 lds r18,navgridX01L lds r19,navgridX01H add r18,r20 adc r19,r21 sts navgridX00L,r18;update current postion sts navgridX00H,r19 sts navgridX01L,r18;update previous postion sts navgridX01H,r19 sts navgridX02L,r18;update long range startpostion sts navgridX02H,r19 ;calculate new grid Y-position when navctrl02 = 0 lds r16,navdist00L lds r17,navdist00H lds r18,navcos00L lds r19,navcos00H rcall mpy16s ;perform division by 32768. fastest solution is to use the high bytes with one shift left. ;this equals x65536/2 clc rol r19;(to adjust r20,lsb) rol r20 rol r21 ;result is now in r20,r21 lds r18,navgridY01L lds r19,navgridY01H add r18,r20 adc r19,r21 sts navgridY00L,r18;update current postion sts navgridY00H,r19 sts navgridY01L,r18;update previous position sts navgridY01H,r19 sts navgridY02L,r18;update long range startpostion sts navgridY02H,r19 ret NAV05: ;calculate new grid X-position when navctrl02 > 0 lds r16,navdist02L lds r17,navdist02H lds r18,navsin00L lds r19,navsin00H rcall mpy16s;with regards to atmel appnote200 ;perform division by 32768. fastest solution is to use the high bytes with one shift left. ;this equals multiplying by 65536 divided by 2 clc rol r19;(required to adjust r20's lsb, r18 is not at all significant) rol r20 rol r21 ;result is now in r20,r21 lds r18,navgridX02L lds r19,navgridX02H add r18,r20 adc r19,r21 sts navgridX00L,r18;update current postion sts navgridX00H,r19 sts navgridX01L,r18;update previous postion sts navgridX01H,r19 ;calculate new grid Y-position when navctrl02 > 0 lds r16,navdist02L lds r17,navdist02H lds r18,navcos00L lds r19,navcos00H rcall mpy16s ;perform division by 32768. fastest solution is to use the high bytes with one shift left. ;this equals x 65536/2 clc rol r19;(to adjust r20,lsb, r18 is not significant) rol r20 rol r21 ;result is now in r20,r21 lds r18,navgridY01L lds r19,navgridY01H add r18,r20 adc r19,r21 sts navgridY00L,r18;update current postion sts navgridY00H,r19 sts navgridY01L,r18;update previous position sts navgridY01H,r19 ret ;********************** NAV services below this line ***************************** NAVgetCMPS: ;initiate reading of the compass bearing ldi ZL,low((NAVgetbearing)*2) ldi ZH,high((NAVgetbearing)*2) call TWIwrdfCMPS ret ;Qlen=5 bytes, SLA+W, reg 2, SLA+R, bearing NAVgetbearing: .db 5,$C0,2,$C1 NAVchkmove: ;this routine checks the low bytes of the ODO-system. ;exit with zero-flag set if they are unchanged ;exit with new ODOvalues in navodoleft/right when we have moved, zero-flag cleared ;only the lower two bytes of ODO are copied. ;distances traversed must never be more than 32767cm per interval. sts navctrl03,r13;clear control variable lds r15,ODOleft00 lds r16,navodoleft00L cpse r15,r16 rcall NAVmove01;update if left=changed lds r15,ODOright00 lds r16,navodoright00L cpse r15,r16 rcall NAVmove02;update if right=changed lds r16,navctrl03 andi r16,$C0 ret NAVmove01: ;update byte00 sts navodoleft01L,r16 sts navodoleft00L,r15 ;update byte01 lds r15,(ODOleft00+1) lds r16,navodoleft00H sts navodoleft01H,r16 sts navodoleft00H,r15 lds r16,navctrl03 sbr r16,$80 sts navctrl03,r16 ret NAVmove02: ;update byte00 sts navodoright01L,r16 sts navodoright00L,r15 ;update byte01 lds r15,(ODOright00+1) lds r16,navodoright00H sts navodoright01H,r16 sts navodoright00H,r15 lds r16,navctrl03 sbr r16,$40 sts navctrl03,r16 ret div16u: ;*************************************************************************** ;* AVR appnote 200 ;* "div16u" - 16/16 Bit Unsigned Division ;* ;* This subroutine divides the two 16-bit numbers ;* "ZH:ZL" (dividend) and "r19:r18" (divisor). ;* The result is placed in "ZH:ZL" and the remainder in ;* "r12:r11". ;* ;* Number of words :19 ;* Number of cycles :235/251 (Min/Max) ;* Low registers used :2 (r11,r12) ;* High registers used :5 (ZL/ZL,ZH/ZH,r18,r19, ;* r20) ;* ;*Modified to return result in Z-reg ;*************************************************************************** ;***** Subroutine Register Variables ; r11 ; r12 ; r18 ; r19 ; r20 ;save registers that may be used elsewhere push r20 div16u0: clr r11;clear remainder Low byte sub r12,r12;clear remainder High byte and carry ldi r20,17;init loop counter d16u_1: rol ZL ;shift left dividend rol ZH dec r20 ;decrement counter brne d16u_2 ;if done pop r20 ret ;return d16u_2: rol r11;shift dividend into remainder rol r12 sub r11,r18;remainder = remainder - divisor sbc r12,r19; brcc d16u_3 ;if result negative add r11,r18;restore remainder adc r12,r19 clc ;clear carry to be shifted into result rjmp d16u_1 ;else d16u_3: sec ;set carry to be shifted into result rjmp d16u_1 goniolookup: ;finds sin/cos/tan that belong to current angle and saves them ;on entry, Z contains the offset, also in navcmpsdata03/04 ;lookup sin @ ldi r16,low((sin00)*2) ldi r17,high((sin00)*2) add ZL,r16 adc ZH,r17 lpm r16,Z+ sts navsin00L,r16 lpm r17,Z+ sts navsin00H,r17 ;lookup cos @ lds ZH,navcmpsdata03 lds ZL,navcmpsdata04 ldi r16,low((cos00)*2) ldi r17,high((cos00)*2) add ZL,r16 adc ZH,r17 lpm r16,Z+ sts navcos00L,r16 lpm r17,Z+ sts navcos00H,r17 ;lookup tan @ lds ZH,navcmpsdata03 lds ZL,navcmpsdata04 ldi r16,low((tan00)*2) ldi r17,high((tan00)*2) add ZL,r16 adc ZH,r17 lpm r16,Z+ sts navtan00L,r16 lpm r17,Z+ sts navtan00H,r17 ret goniolookup2: ;finds sin/cos that belong to target angle and saves them in navsin01,navcos01 ;on entry, Z contains the offset, also in navcmpsdata03/04 ;lookup sin @ ldi r16,low((sin00)*2) ldi r17,high((sin00)*2) add ZL,r16 adc ZH,r17 lpm r16,Z+ sts navsin01L,r16 lpm r17,Z+ sts navsin01H,r17 ;lookup cos @ lds ZH,navcmpsdata03 lds ZL,navcmpsdata04 ldi r16,low((cos00)*2) ldi r17,high((cos00)*2) add ZL,r16 adc ZH,r17 lpm r16,Z+ sts navcos01L,r16 lpm r17,Z+ sts navcos01H,r17 ret navdistance: ;calculate the average distance traversed lds r16,navodoleft01L lds r17,navodoleft01H lds r18,navodoleft00L lds r19,navodoleft00H sub r18,r16 sbc r19,r17 sts navdist00L,r18 sts navdist00H,r19 lds r16,navodoright01L lds r17,navodoright01H lds r18,navodoright00L lds r19,navodoright00H sub r18,r16 sbc r19,r17 lds r16,navdist00L lds r17,navdist00H add r16,r18 adc r17,r19 brpl navdistance01 ;perform two's complement when the result is minus com r16 com r17 subi r16,low(-1) sbci r17,high(-1) navdistance01:;get average (divide by 2) lsr r17 ror r16 sts navdist00L,r16 sts navdist00H,r17 lds r14,navctrl02 and r14,r14 breq navdistance02;navdist02 is cleared when lds r18,navdist02L lds r19,navdist02H add r18,r16 adc r19,r17 sts navdist02L,r18 sts navdist02H,r19 ret navdistance02: sts navdist02L,r13 sts navdist02H,r13 ret ;*************************************************************************** ;* ;* "mpy16s" - 16x16 Bit Signed Multiplication ;* ;* This subroutine multiplies signed the two 16-bit register variables ;* r19:r18 and r17:r16. ;* The result is placed in r21:r20:r19:r18. ;* The routine is an implementation of Booth's algorithm. If all 32 bits ;* in the result are needed, avoid calling the routine with ;* -32768 ($8000) as multiplicand ;* ;* Number of words :16 + return ;* Number of cycles :210/226 (Min/Max) + return ;* Low registers used :None ;* High registers used :7 (r18,r19,r16/r18,r17/r19, ;* r20,r21,r22) ;* ;*************************************************************************** ;***** Subroutine Register Variables ;r16 ;multiplicand low byte ;r17 ;multiplicand high byte ;r18 ;multiplier low byte ;r19 ;multiplier high byte ;r18 ;result byte 0 (LSB) ;r19 ;result byte 1 ;r20 ;result byte 2 ;r21 ;result byte 3 (MSB) ;r22 ;loop counter mpy16s: clr r21 ;clear result byte 3 sub r20,r20;clear result byte 2 and carry ldi r22,16;init loop counter m16s_1: brcc m16s_2 ;if carry (previous bit) set add r20,r16; add multiplicand Low to result byte 2 adc r21,r17; add multiplicand High to result byte 3 m16s_2: sbrc r18,0;if current bit set sub r20,r16; sub multiplicand Low from result byte 2 sbrc r18,0;if current bit set sbc r21,r17; sub multiplicand High from result byte 3 asr r21 ;shift right result and multiplier ror r20 ror r19 ror r18 dec r22 ;decrement counter brne m16s_1 ;if not done, loop more ret navtstS:;compass compare ;check if compass data remains within certain boundaries ;used for straight-line calculations lds r16, navcmpsdata02;low byte lds r24, navcmpss02 lds r17, navcmpsdata01;high byte lds r25, navcmpss01 sub r24,r16 sbc r25,r17 ;result is in r24:25 brpl navtstS01 ;negative result: invert first com r24 com r25 subi r24,low(-1) sbci r25,high(-1) navtstS01: cpi r25,0 breq navtstS03 navtstS02:;exit if result is not near enough: update compass course, set navctrl02=0 sts navcmpss01,r17 sts navcmpss02,r16 sts navctrl02,r13 ret navtstS03:;high byte = equal cpi r24, (navcompdiff) brge navtstS02 ;result < limit sts navcmpss01,r17;store course, inc navctrl02 sts navcmpss02,r16 lds r16,navctrl02 inc r16 sts navctrl02,r16 ret ;calculate distance & heading to target NAVcdh: ;Tx - Px(n)/Ty - Py(n)= tan (compass angle to destination) lds r16,navgridX01L lds r17,navgridX01H lds r18,navTX00L lds r19,navTX00H sub r18,r16 sbc r19,r17 sts navTscratch00H,r19 sts navTscratch00L,r18 lds r16,navgridY01L lds r17,navgridY01H lds r18,navTY00L lds r19,navTY00H sub r18,r16 sbc r19,r17 ;calculate tan@ ;we must multiply the dividend 512 so that the result will have ;the same resolution as used for the stored tan-data. sts navTscratch01H,r19;divisor sts navTscratch01L,r18 mov r20,r18 mov r21,r19 ;setup dividend clr r16 lds r17,navTscratch00L;dividend x 2^8 while stored at +8bits lds r18,navTscratch00H clr r19 sbrc r18,7 com r19;invert r19 when dividend < 0 lsl r16;not needed but correct rol r17;multiply by 2 once more: 2^9 rol r18 rol r19 navca09: call div32s ;result in r16(L),r17(H) sts navtan01L,r16;this is the tan of the heading to the target sts navtan01H,r17; ;determine sign of Ty - Py(n)/Tx - Px(n) lds r19,navTscratch01H cp r19,r13 brpl navca00 ;Y<0, test if X >0 lds r18,navTscratch00H cp r18,r13 brpl navca03 ;------------------------ navca02:;X<0,Y<0 ;search range 180-270 ldi ZL,low((tan00+180)*2) ldi ZH,high((tan00+180)*2) rjmp navca10 ;------------------------ navca00:;Y>0, test if X >0 lds r18,navTscratch00H cp r18,r13 brmi navca01 ;X>0,Y>0 ;search range 0-90 ldi ZL,low((tan00+0)*2) ldi ZH,high((tan00+0)*2) rjmp navca10 ;------------------------ navca03:;X>0,Y<0 ;search range 90-180 ldi ZL,low((tan00+90)*2) ldi ZH,high((tan00+90)*2) rjmp navca10 ;------------------------ navca01:;X<0,Y>0 ;search range 270-360 ldi ZL,low((tan00+270)*2) ldi ZH,high((tan00+270)*2) navca10:;Lookup tan ;find nearest matching value. ;registers used: ;r10-r11 smallest difference found ;r14,r15 value to be tested ;r16,r17 value to be compared ;r18,r19 scratch space ;Z-reg pointer in flash ;r20 loopcounter ;initiate loop by reading the first value clr r20 inc r20 lpm r14,Z+ lpm r15,Z+ mov r19,r15 mov r18,r14 sub r18,r16 sbc r19,r17 brpl navca11 com r18 com r19 subi r18,low(-1) sbci r19,high(-1) navca11: mov r11,r19 mov r10,r18 navca12: inc r20 lpm r14,Z+ lpm r15,Z+ mov r19,r15 mov r18,r14 sub r18,r16 sbc r19,r17 brpl navca13 com r18 ;two complement inverse com r19 subi r18,low(-1) sbci r19,high(-1) navca13: cp r19,r11;compare msb brlo navca11;branch if result-msb is smaller cp r18,r10 brlo navca11;branch if result-lsb is smaller ;result is the same or higher than previous > USE PREVIOUS RESULT ;adjust Z-reg: 4 places back & offset from start of table subi ZL,low((tan00+2)*2) sbci ZH,high((tan00+2)*2) sts navcmpsdata03,ZH;temp store for offset sts navcmpsdata04,ZL call goniolookup2;modified to lookup sin & cos ;calculate distance to target ;(Tx - Px(n)*32768) / (sin (angle to destination)*32768) ; or ;(Ty - Py(n)*32768) / (cos (angle to destination)*32768) ;dividend must be a 32bit number to keep the resolution ;use sin unless it is zero. due to the multiplication by 32768, msb of sin > 0 unless sin=0 lds r21,navsin01H cp r21,r13 breq navca16 ;use sin lds r20,navsin01L;setup divisor lds r21,navsin01H lds r19,navTscratch00H;setup dividend lds r18,navTscratch00L clr r17 clr r16 clc sbrc r19,7 sec ror r19;adjust for 2^15 ror r18 ror r17 rjmp navca17 navca16: lds r21,navcos01H lds r20,navcos01L;setup divisor lds r19,navTscratch01H;setup dividend lds r18,navTscratch01L clr r17 clr r16 clc sbrc r19,7 sec ror r19;adjust for 2^15 ror r18 ror r17 navca17: call div32s ;result is in r17:16 sbrc r17,7 rcall navinverse;invert result when minus. Distance should be absolute (i.e. positive) sts navdist03L,r16 sts navdist03H,r17 ret navinverse: com r16 com r17 subi r16,low(-1) sbci r17,high(-1) ret ;*************************************************************************** ;* ;* "div32s" - 32/16 Bit Signed Division ;* ;* This subroutine divides two signed numbers. dividend 32bit, divisor 16 bit ;* "r19:r16" (dividend) and "r21:r20" (divisor). ;* The result is placed in "r19:r16" and the remainder in ;* "r15:r14". ;* ;* Number of words : ;* Number of cycles : (Min/Max) ;* Low registers used :3 (r12,r14,r15) ;* High registers used :7 (r16 tm r22) ;* Adapted for 32 bit dividend ;* LG nov 2005 ;*************************************************************************** ;***** Subroutine Register Variables ; r12 ;sign register ; r14 ;remainder low byte ; r15 ;remainder high byte ; r16 ;result byte 0 ; r17 ;result byte 1 ; r18 ;result byte 2 ; r19 ;result byte 3 ; r16 ;dividend byte 0 ; r17 ;dividend byte 1 ; r18 ;dividend byte 2 ; r19 ;dividend byte 3 ; r20 ;divisor low byte ; r21 ;divisor high byte ; r22 ;loop counter div32s: mov r12,r19;move dividend High to sign register eor r12,r21;xor divisor High with sign register sbrs r19,7;if MSB in dividend set rjmp d32s_1 com r19 ; change sign of dividend com r18 com r17 com r16 subi r16,low(-1) sbci r17,high(-1) sbci r18,high(-1) sbci r19,high(-1) d32s_1: sbrs r21,7;if MSB in divisor set rjmp d32s_2 com r21 ;change sign of divisor com r20 subi r20,low(-1) sbci r21,high(-1) d32s_2: clr r14;clear remainder Low byte sub r15,r15;clear remainder High byte and carry ldi r22,33;init loop counter d32s_3: rol r16 ;shift left dividend rol r17 rol r18 rol r19 dec r22 ;decrement counter brne d32s_5 ;if done sbrs r12,7 ; if MSB in sign register set rjmp d32s_4 com r19; change sign of result com r18 com r17 com r16 subi r16,low(-1) sbci r17,high(-1) sbci r18,high(-1) sbci r19,high(-1) d32s_4: ret ; return d32s_5: rol r14;shift dividend into remainder rol r15 sub r14,r20;remainder = remainder - divisor sbc r15,r21; brcc d32s_6 ;if result negative add r14,r20;restore remainder adc r15,r21 clc ;clear carry to be shifted into result rjmp d32s_3 ; else d32s_6: sec ;set carry to be shifted into result rjmp d32s_3 |
;table that contains sin/cos/tan info
;sin * 32768, cos *32768, tan * 512
sin00:
.dw 0
.dw 572
.dw 1144
.dw 1715
.dw 2286
.dw 2856
.dw 3425
.dw 3993
.dw 4560
.dw 5126
.dw 5690
.dw 6252
.dw 6813
.dw 7371
.dw 7927
.dw 8481
.dw 9032
.dw 9580
.dw 10126
.dw 10668
.dw 11207
.dw 11743
.dw 12275
.dw 12803
.dw 13328
.dw 13848
.dw 14365
.dw 14876
.dw 15384
.dw 15886
.dw 16384
.dw 16877
.dw 17364
.dw 17847
.dw 18324
.dw 18795
.dw 19261
.dw 19720
.dw 20174
.dw 20622
.dw 21063
.dw 21498
.dw 21926
.dw 22348
.dw 22763
.dw 23170
.dw 23571
.dw 23965
.dw 24351
.dw 24730
.dw 25102
.dw 25466
.dw 25822
.dw 26170
.dw 26510
.dw 26842
.dw 27166
.dw 27482
.dw 27789
.dw 28088
.dw 28378
.dw 28660
.dw 28932
.dw 29197
.dw 29452
.dw 29698
.dw 29935
.dw 30163
.dw 30382
.dw 30592
.dw 30792
.dw 30983
.dw 31164
.dw 31336
.dw 31499
.dw 31651
.dw 31795
.dw 31928
.dw 32052
.dw 32166
.dw 32270
.dw 32365
.dw 32449
.dw 32524
.dw 32588
.dw 32643
.dw 32688
.dw 32723
.dw 32748
.dw 32763
.dw 32767 ;actually 32768 but this would represent a negative number
.dw 32763
.dw 32748
.dw 32723
.dw 32688
.dw 32643
.dw 32588
.dw 32524
.dw 32449
.dw 32365
.dw 32270
.dw 32166
.dw 32052
.dw 31928
.dw 31795
.dw 31651
.dw 31499
.dw 31336
.dw 31164
.dw 30983
.dw 30792
.dw 30592
.dw 30382
.dw 30163
.dw 29935
.dw 29698
.dw 29452
.dw 29197
.dw 28932
.dw 28660
.dw 28378
.dw 28088
.dw 27789
.dw 27482
.dw 27166
.dw 26842
.dw 26510
.dw 26170
.dw 25822
.dw 25466
.dw 25102
.dw 24730
.dw 24351
.dw 23965
.dw 23571
.dw 23170
.dw 22763
.dw 22348
.dw 21926
.dw 21498
.dw 21063
.dw 20622
.dw 20174
.dw 19720
.dw 19261
.dw 18795
.dw 18324
.dw 17847
.dw 17364
.dw 16877
.dw 16384
.dw 15886
.dw 15384
.dw 14876
.dw 14365
.dw 13848
.dw 13328
.dw 12803
.dw 12275
.dw 11743
.dw 11207
.dw 10668
.dw 10126
.dw 9580
.dw 9032
.dw 8481
.dw 7927
.dw 7371
.dw 6813
.dw 6252
.dw 5690
.dw 5126
.dw 4560
.dw 3993
.dw 3425
.dw 2856
.dw 2286
.dw 1715
.dw 1144
.dw 572
.dw 0
.dw -572
.dw -1144
.dw -1715
.dw -2286
.dw -2856
.dw -3425
.dw -3993
.dw -4560
.dw -5126
.dw -5690
.dw -6252
.dw -6813
.dw -7371
.dw -7927
.dw -8481
.dw -9032
.dw -9580
.dw -10126
.dw -10668
.dw -11207
.dw -11743
.dw -12275
.dw -12803
.dw -13328
.dw -13848
.dw -14365
.dw -14876
.dw -15384
.dw -15886
.dw -16384
.dw -16877
.dw -17364
.dw -17847
.dw -18324
.dw -18795
.dw -19261
.dw -19720
.dw -20174
.dw -20622
.dw -21063
.dw -21498
.dw -21926
.dw -22348
.dw -22763
.dw -23170
.dw -23571
.dw -23965
.dw -24351
.dw -24730
.dw -25102
.dw -25466
.dw -25822
.dw -26170
.dw -26510
.dw -26842
.dw -27166
.dw -27482
.dw -27789
.dw -28088
.dw -28378
.dw -28660
.dw -28932
.dw -29197
.dw -29452
.dw -29698
.dw -29935
.dw -30163
.dw -30382
.dw -30592
.dw -30792
.dw -30983
.dw -31164
.dw -31336
.dw -31499
.dw -31651
.dw -31795
.dw -31928
.dw -32052
.dw -32166
.dw -32270
.dw -32365
.dw -32449
.dw -32524
.dw -32588
.dw -32643
.dw -32688
.dw -32723
.dw -32748
.dw -32763
.dw -32768
.dw -32763
.dw -32748
.dw -32723
.dw -32688
.dw -32643
.dw -32588
.dw -32524
.dw -32449
.dw -32365
.dw -32270
.dw -32166
.dw -32052
.dw -31928
.dw -31795
.dw -31651
.dw -31499
.dw -31336
.dw -31164
.dw -30983
.dw -30792
.dw -30592
.dw -30382
.dw -30163
.dw -29935
.dw -29698
.dw -29452
.dw -29197
.dw -28932
.dw -28660
.dw -28378
.dw -28088
.dw -27789
.dw -27482
.dw -27166
.dw -26842
.dw -26510
.dw -26170
.dw -25822
.dw -25466
.dw -25102
.dw -24730
.dw -24351
.dw -23965
.dw -23571
.dw -23170
.dw -22763
.dw -22348
.dw -21926
.dw -21498
.dw -21063
.dw -20622
.dw -20174
.dw -19720
.dw -19261
.dw -18795
.dw -18324
.dw -17847
.dw -17364
.dw -16877
.dw -16384
.dw -15886
.dw -15384
.dw -14876
.dw -14365
.dw -13848
.dw -13328
.dw -12803
.dw -12275
.dw -11743
.dw -11207
.dw -10668
.dw -10126
.dw -9580
.dw -9032
.dw -8481
.dw -7927
.dw -7371
.dw -6813
.dw -6252
.dw -5690
.dw -5126
.dw -4560
.dw -3993
.dw -3425
.dw -2856
.dw -2286
.dw -1715
.dw -1144
.dw -572
cos00:
.dw 32767 ;actually 32768 but this would represent a negative number
.dw 32763
.dw 32748
.dw 32723
.dw 32688
.dw 32643
.dw 32588
.dw 32524
.dw 32449
.dw 32365
.dw 32270
.dw 32166
.dw 32052
.dw 31928
.dw 31795
.dw 31651
.dw 31499
.dw 31336
.dw 31164
.dw 30983
.dw 30792
.dw 30592
.dw 30382
.dw 30163
.dw 29935
.dw 29698
.dw 29452
.dw 29197
.dw 28932
.dw 28660
.dw 28378
.dw 28088
.dw 27789
.dw 27482
.dw 27166
.dw 26842
.dw 26510
.dw 26170
.dw 25822
.dw 25466
.dw 25102
.dw 24730
.dw 24351
.dw 23965
.dw 23571
.dw 23170
.dw 22763
.dw 22348
.dw 21926
.dw 21498
.dw 21063
.dw 20622
.dw 20174
.dw 19720
.dw 19261
.dw 18795
.dw 18324
.dw 17847
.dw 17364
.dw 16877
.dw 16384
.dw 15886
.dw 15384
.dw 14876
.dw 14365
.dw 13848
.dw 13328
.dw 12803
.dw 12275
.dw 11743
.dw 11207
.dw 10668
.dw 10126
.dw 9580
.dw 9032
.dw 8481
.dw 7927
.dw 7371
.dw 6813
.dw 6252
.dw 5690
.dw 5126
.dw 4560
.dw 3993
.dw 3425
.dw 2856
.dw 2286
.dw 1715
.dw 1144
.dw 572
.dw 0
.dw -572
.dw -1144
.dw -1715
.dw -2286
.dw -2856
.dw -3425
.dw -3993
.dw -4560
.dw -5126
.dw -5690
.dw -6252
.dw -6813
.dw -7371
.dw -7927
.dw -8481
.dw -9032
.dw -9580
.dw -10126
.dw -10668
.dw -11207
.dw -11743
.dw -12275
.dw -12803
.dw -13328
.dw -13848
.dw -14365
.dw -14876
.dw -15384
.dw -15886
.dw -16384
.dw -16877
.dw -17364
.dw -17847
.dw -18324
.dw -18795
.dw -19261
.dw -19720
.dw -20174
.dw -20622
.dw -21063
.dw -21498
.dw -21926
.dw -22348
.dw -22763
.dw -23170
.dw -23571
.dw -23965
.dw -24351
.dw -24730
.dw -25102
.dw -25466
.dw -25822
.dw -26170
.dw -26510
.dw -26842
.dw -27166
.dw -27482
.dw -27789
.dw -28088
.dw -28378
.dw -28660
.dw -28932
.dw -29197
.dw -29452
.dw -29698
.dw -29935
.dw -30163
.dw -30382
.dw -30592
.dw -30792
.dw -30983
.dw -31164
.dw -31336
.dw -31499
.dw -31651
.dw -31795
.dw -31928
.dw -32052
.dw -32166
.dw -32270
.dw -32365
.dw -32449
.dw -32524
.dw -32588
.dw -32643
.dw -32688
.dw -32723
.dw -32748
.dw -32763
.dw -32768
.dw -32763
.dw -32748
.dw -32723
.dw -32688
.dw -32643
.dw -32588
.dw -32524
.dw -32449
.dw -32365
.dw -32270
.dw -32166
.dw -32052
.dw -31928
.dw -31795
.dw -31651
.dw -31499
.dw -31336
.dw -31164
.dw -30983
.dw -30792
.dw -30592
.dw -30382
.dw -30163
.dw -29935
.dw -29698
.dw -29452
.dw -29197
.dw -28932
.dw -28660
.dw -28378
.dw -28088
.dw -27789
.dw -27482
.dw -27166
.dw -26842
.dw -26510
.dw -26170
.dw -25822
.dw -25466
.dw -25102
.dw -24730
.dw -24351
.dw -23965
.dw -23571
.dw -23170
.dw -22763
.dw -22348
.dw -21926
.dw -21498
.dw -21063
.dw -20622
.dw -20174
.dw -19720
.dw -19261
.dw -18795
.dw -18324
.dw -17847
.dw -17364
.dw -16877
.dw -16384
.dw -15886
.dw -15384
.dw -14876
.dw -14365
.dw -13848
.dw -13328
.dw -12803
.dw -12275
.dw -11743
.dw -11207
.dw -10668
.dw -10126
.dw -9580
.dw -9032
.dw -8481
.dw -7927
.dw -7371
.dw -6813
.dw -6252
.dw -5690
.dw -5126
.dw -4560
.dw -3993
.dw -3425
.dw -2856
.dw -2286
.dw -1715
.dw -1144
.dw -572
.dw 0
.dw 572
.dw 1144
.dw 1715
.dw 2286
.dw 2856
.dw 3425
.dw 3993
.dw 4560
.dw 5126
.dw 5690
.dw 6252
.dw 6813
.dw 7371
.dw 7927
.dw 8481
.dw 9032
.dw 9580
.dw 10126
.dw 10668
.dw 11207
.dw 11743
.dw 12275
.dw 12803
.dw 13328
.dw 13848
.dw 14365
.dw 14876
.dw 15384
.dw 15886
.dw 16384
.dw 16877
.dw 17364
.dw 17847
.dw 18324
.dw 18795
.dw 19261
.dw 19720
.dw 20174
.dw 20622
.dw 21063
.dw 21498
.dw 21926
.dw 22348
.dw 22763
.dw 23170
.dw 23571
.dw 23965
.dw 24351
.dw 24730
.dw 25102
.dw 25466
.dw 25822
.dw 26170
.dw 26510
.dw 26842
.dw 27166
.dw 27482
.dw 27789
.dw 28088
.dw 28378
.dw 28660
.dw 28932
.dw 29197
.dw 29452
.dw 29698
.dw 29935
.dw 30163
.dw 30382
.dw 30592
.dw 30792
.dw 30983
.dw 31164
.dw 31336
.dw 31499
.dw 31651
.dw 31795
.dw 31928
.dw 32052
.dw 32166
.dw 32270
.dw 32365
.dw 32449
.dw 32524
.dw 32588
.dw 32643
.dw 32688
.dw 32723
.dw 32748
.dw 32763
tan00:
.dw 0
.dw 9
.dw 18
.dw 27
.dw 36
.dw 45
.dw 54
.dw 63
.dw 72
.dw 81
.dw 90
.dw 100
.dw 109
.dw 118
.dw 128
.dw 137
.dw 147
.dw 157
.dw 166
.dw 176
.dw 186
.dw 197
.dw 207
.dw 217
.dw 228
.dw 239
.dw 250
.dw 261
.dw 272
.dw 284
.dw 296
.dw 308
.dw 320
.dw 332
.dw 345
.dw 359
.dw 372
.dw 386
.dw 400
.dw 415
.dw 430
.dw 445
.dw 461
.dw 477
.dw 494
.dw 512
.dw 530
.dw 549
.dw 569
.dw 589
.dw 610
.dw 632
.dw 655
.dw 679
.dw 705
.dw 731
.dw 759
.dw 788
.dw 819
.dw 852
.dw 887
.dw 924
.dw 963
.dw 1005
.dw 1050
.dw 1098
.dw 1150
.dw 1206
.dw 1267
.dw 1334
.dw 1407
.dw 1487
.dw 1576
.dw 1675
.dw 1786
.dw 1911
.dw 2054
.dw 2218
.dw 2409
.dw 2634
.dw 2904
.dw 3233
.dw 3643
.dw 4170
.dw 4871
.dw 5852
.dw 7322
.dw 9770
.dw 14662
.dw 29332
.dw 32767 ;infinite, rounded to maximum
.dw -29332
.dw -14662
.dw -9770
.dw -7322
.dw -5852
.dw -4871
.dw -4170
.dw -3643
.dw -3233
.dw -2904
.dw -2634
.dw -2409
.dw -2218
.dw -2054
.dw -1911
.dw -1786
.dw -1675
.dw -1576
.dw -1487
.dw -1407
.dw -1334
.dw -1267
.dw -1206
.dw -1150
.dw -1098
.dw -1050
.dw -1005
.dw -963
.dw -924
.dw -887
.dw -852
.dw -819
.dw -788
.dw -759
.dw -731
.dw -705
.dw -679
.dw -655
.dw -632
.dw -610
.dw -589
.dw -569
.dw -549
.dw -530
.dw -512
.dw -494
.dw -477
.dw -461
.dw -445
.dw -430
.dw -415
.dw -400
.dw -386
.dw -372
.dw -359
.dw -345
.dw -332
.dw -320
.dw -308
.dw -296
.dw -284
.dw -272
.dw -261
.dw -250
.dw -239
.dw -228
.dw -217
.dw -207
.dw -197
.dw -186
.dw -176
.dw -166
.dw -157
.dw -147
.dw -137
.dw -128
.dw -118
.dw -109
.dw -100
.dw -90
.dw -81
.dw -72
.dw -63
.dw -54
.dw -45
.dw -36
.dw -27
.dw -18
.dw -9
.dw 0
.dw 9
.dw 18
.dw 27
.dw 36
.dw 45
.dw 54
.dw 63
.dw 72
.dw 81
.dw 90
.dw 100
.dw 109
.dw 118
.dw 128
.dw 137
.dw 147
.dw 157
.dw 166
.dw 176
.dw 186
.dw 197
.dw 207
.dw 217
.dw 228
.dw 239
.dw 250
.dw 261
.dw 272
.dw 284
.dw 296
.dw 308
.dw 320
.dw 332
.dw 345
.dw 359
.dw 372
.dw 386
.dw 400
.dw 415
.dw 430
.dw 445
.dw 461
.dw 477
.dw 494
.dw 512
.dw 530
.dw 549
.dw 569
.dw 589
.dw 610
.dw 632
.dw 655
.dw 679
.dw 705
.dw 731
.dw 759
.dw 788
.dw 819
.dw 852
.dw 887
.dw 924
.dw 963
.dw 1005
.dw 1050
.dw 1098
.dw 1150
.dw 1206
.dw 1267
.dw 1334
.dw 1407
.dw 1487
.dw 1576
.dw 1675
.dw 1786
.dw 1911
.dw 2054
.dw 2218
.dw 2409
.dw 2634
.dw 2904
.dw 3233
.dw 3643
.dw 4170
.dw 4871
.dw 5852
.dw 7322
.dw 9770
.dw 14662
.dw 29332
.dw 32767 ;infinite, rounded to maximum
.dw -29332
.dw -14662
.dw -9770
.dw -7322
.dw -5852
.dw -4871
.dw -4170
.dw -3643
.dw -3233
.dw -2904
.dw -2634
.dw -2409
.dw -2218
.dw -2054
.dw -1911
.dw -1786
.dw -1675
.dw -1576
.dw -1487
.dw -1407
.dw -1334
.dw -1267
.dw -1206
.dw -1150
.dw -1098
.dw -1050
.dw -1005
.dw -963
.dw -924
.dw -887
.dw -852
.dw -819
.dw -788
.dw -759
.dw -731
.dw -705
.dw -679
.dw -655
.dw -632
.dw -610
.dw -589
.dw -569
.dw -549
.dw -530
.dw -512
.dw -494
.dw -477
.dw -461
.dw -445
.dw -430
.dw -415
.dw -400
.dw -386
.dw -372
.dw -359
.dw -345
.dw -332
.dw -320
.dw -308
.dw -296
.dw -284
.dw -272
.dw -261
.dw -250
.dw -239
.dw -228
.dw -217
.dw -207
.dw -197
.dw -186
.dw -176
.dw -166
.dw -157
.dw -147
.dw -137
.dw -128
.dw -118
.dw -109
.dw -100
.dw -90
.dw -81
.dw -72
.dw -63
.dw -54
.dw -45
.dw -36
.dw -27
.dw -18
.dw -9