Hi,
That's a good start; you're not ready for the navigation calculation code yet, but here's some food for thought. I've devised several Square Root routines, but I hope the one posted below is consistent with the first part of the code.
Firstly, a verification that we
do need 32-bit maths: Even without Pythagoras, the basic formula above contains
a2 and
b2 terms, so 16-bit (Word) calculations will limit
a and
b to a single byte each. Thus 10 cms would be the best resolution if the maximum range is to be 20 metres. Furthermore,
a2 - b2 can easily generate a negative result (and so can the original equation, if the "target" can move outside the length of the baseline), which potentially limits rhe resolution to a sign plus 7 bits (1: 127).
There's no point in going up to less than 32-bits (i.e. Double Words) but this is rather "overkill" - The maths can then resolve to 1 mm units, which keeps the scaling simple, but gives you quite a challenge for the hardware.
The Program/Calculation for A and B (i.e. along the "Y" axis of the two transducers) actually proved quite easy, since I had already documented code snippets for 32-bit subtraction and division (down to a 16-bit word result). Here it is,, with a few extra "hooks", in preparation for adding a Pythagorean calculation for "X". It should be easily fast enough; the simulator produces a result after around 5 seconds.
Code:
; Ultrasonic Triangulation ; AllyCat June 2019
#picaxe 08m2 ; And most others
#no_data
symbol AL = w1 ; Low word of Range A
symbol AH = w2 ; High word of Range A
symbol BL = w3 ; Low word of Range B
symbol BH = w4 ; High word of Range B
symbol DL = w5 ; Baseline spacing
symbol DH = w6 ; Auxiliary register
symbol rangeA = w1 ; Input values
symbol rangeB = w3 ;
symbol rangeD = w5 ; Transducer spacing
symbol sign = w4 ; Sign (flag) used for division subroutine (Overlays BH)
init:
rangeA = 13000 ; Test values
rangeB = 5000
rangeD = 12000 ; Transmitter spacing (baseline)
;* A = a2 - b2 + d2 {s / 2d s} ; Base formula (s = Signed functions)
;* A = a2 - b2 [s / d s] + d / 2 ; Revised formula
Yaxis:
DH = AL ; Store for X-Axis Pythagoras calculation
AH = rangeA ** rangeA ; High word of A squared
AL = rangeA * rangeA ; Low word of A squared
BH = rangeB ** rangeB ; High word of B squared
BL = rangeB * rangeB ; Low word of B squared
call subAB: ; Calculate AH:AL = AH:AL minus BH:BL
div32:
BL = rangeD ; Divisor
call sdiv ; Signed division 32 bits / 16 bits := 16 bits
result:
AL = AL + rangeD
if AL > 32767 then ; Negative value
AL = -AL / 2
AL = -AL
else
AL = AL / 2
endif
sertxd("Y= ",#AL,cr,lf) ; Result
DL = AL ; Store for later
Xaxis: ; ** Insert Pythagoras program and Additional subroutine here **
end
; SUBROUTINES:
sdiv: ; 32 bit Signed division
sign = 0 ; Use BH for loop counter and sign flag
if AH > 32767 then ; Negative input
sign = 1
negateA: ; Twos complement of AH:AL
AL = not AL + 1
AH = not AH
if AL = 0 then
inc AH
endif
endif
div31: ; Divide numerator (AH:AL) by divisor (BL)
for BH = sign to 63 step 4 ; Repeat for 16 bit positions
AH = AL / 32768 + AH + AH ; Add carry and shift numerator left (top bit lost)
AL = AL + AL ; Shift done
if AH >= BL then ; Skip if can't subtract
AH = AH - BL ; Subtract divisor, then..
AL = AL + 1 ; Add the flag into result (in low word)
endif
next BH ; Result in AL, remainder in AH
if sign = 65 then ; Negative result
AL = -AL
endif
return
subAB: ; Calculate AH:AL = AH:AL minus BH:BL
AH = AH - BH ; Subtract high words :
if AL < BL then ; Borrow if minuend < subtrahend
dec AH ; Borrow for low word
endif
AL = AL - BL ; Calculate low word
return
Of course we still need a solution for "X", which, notably cannot be "unique" because of symmetry about the baseline (mathematically, remember the square root of a real, positive number can be either positive or negative). Therefore, perhaps the "best" solution is to introduce a third (or more) transducer(s) at right angles to the first two. That can be solved in exactly the same way as for the first two transducers, to give a complete and unique solution. Or four transducers in a square or rectangular pattern permit up to
six calculations (four sides and two diagonals) which could be averaged for improved accuracy.
However, here is some additional program code to calculate "X", using Pythagoras. It requires more variable space and takes longer to compute, typically 15 seconds in the simulator.
Code:
Xaxis: ; Pythagoras
symbol rootL = w3 ; Low word for square root
symbol rootH = w4 ; High word for square root calculation
symbol bitsL = w5 ; Low word of bits for square root calc
symbol bitsH = w6 ; High word of bits for root calculation
BH = AL ** AL ; Square of Y-offset
BL = AL * AL
AH = DH ** DH ; High word of Hypotenuse
AL = DH * DH ; Low word
call subAB ; Calculate AH:AL = AH:AL minus BH:BL
squareroot:
rootH = 0 : rootL = 0 ; Initialise
bitsH = $4000 : bitsL = 0
;# Do While bits > number
do
if bitsH > AH then shiftbits
if bitsH < AH then bitsdone
if bitsL <= AL then bitsdone
shiftbits: ; Shift Right by two bits
call shiftbits2
loop
bitsdone:
do while bitsH <> bitsL ; Loop until both are zero (only 1 bit is ever set)
;# number = number - root - bits
subroot: ; Calculate AH:AL = AH:AL minus rootH:rootL
AH = AH - rootH ; Subtract high word
if AL < rootL then ; Borrow if minuend < subtrahend
dec AH ; Borrow for low word
endif
AL = AL - rootL ; Calculate low word
if AH > 32767 then addroot ; Already negative so skip to restore
subbits: ; Calculate AH:AL = AH:AL minus bitsH:bitsL
AH = AH - bitsH ; Subtract high words :
if AL < bitsL then ; Borrow if minuend < subtrahend
dec AH ; Borrow for low word
endif
AL = AL - bitsL ; Calculate low word
;# If number is negative then Restore subtraction
if AH > 32767 then ; Negative so restore number
addbits: ; Calculate A = A + bits
AL = AL + bitsL
AH = AH + bitsH
if AL < bitsL then ; Carry from low word
inc AH
endif
addroot: ; Calculate A = A + root
AL = rootL + AL
AH = rootH + AH
if AL < rootL then ; Carry from low word
inc AH
endif
;# root = root / 2
shiftroot:
rootL = rootL / 2
rootL = 32768 * rootH + rootL
rootH = rootH / 2
else
;# root = root / 2 + bits
rootL = rootL / 2
rootL = 32768 * rootH + rootL
rootH = rootH / 2
addbitsR: ; Calculate root = root + bits
rootL = bitsL + rootL
rootH = bitsH + rootH
if rootL < bitsL then ; Carry from low word
inc rootH
endif ; Carry from low word
endif
;# bits = bits / 4
call shiftbits2 ; Shift Right by two bits
loop
sertxd("X= ",#rootL," Rem= ",#AL,cr,lf)
end
; ADDITIONAL SUBROUTINE:
shiftbits2: ; Shift bits Right by two bits
bitsL = bitsL / 4
bitsL = 16384 * bitsH + bitsL
bitsH = bitsH / 4
return
I won't clutter up this thread with details and options for the Square Root function, but maybe will start a new "Snippets" thread in due course.
Cheers, Alan.