New Compass Sensor
1. The
6100 analog compass sensor
This is our second realization of a
compass-sensor. The design is based on a different sensor which
we got from the Swiss firm PEWATRON. Non-Europeans can find a very similar
sensor at Dinsmore's. The data-sheet tells us that the
structure of the sensor contains a miniaturrotor hold on saphire
bearings, the custom-made Halleffect-IC in combination with the
magnet. The sensor is damped in order to eliminate overshooting.
The sensor is constructed to operate in a vertical position with
leads down. It is designed to measure the horizontal field of the
earth field. External field and magnetic parts affect the correct
reading.
Here some specifications of the
6100 analog type:
| Supply voltage | 5 VDC |
| Input current | 18..19mA |
| Voltage swing | 1,5 V |
| Current draw max. | 4 mA |
| Return time from 90 degrees | ~2,5 sec |
The output-voltage in function of
the direction:
2. The layout of the RCX-compass sensor
The design is made of two identical sensor-drivers. This sensor must be read by two independant RCX-sensor inputs. So, there is no common ground. For correct and stable results the 6100 has to be powered by stabilized means. The OpAmp is used as a non-inverting amplifier. This has the advantage of a very high input impedance. The amplification-ratio is about 2. R4 and R8 are used to adjust the reference-potential of the LM 324. The design presents perfect linearity as the following graphics demonstrate:
Here some experimental results of our design:
So, for good accuracy the sensor should not be mounted in proximity to disturbing magnetic or metallic objects. As the field influence becomes weaker in function of the square of the distance, some cm to such objects should be sufficient.
3. PC-Board
4.
Programming
There are two different
approaches to this programming challenge. The first possibility
is to program the RCX to render the arctan of a certain value
given by sin/cos. The second way is to devide the output-curve in
different sectors and choose the most linear part either of the
sine or the cosine-curve in order to produce good accuracy. In
this case the RCX has to be programmed with an arccos function.We
decided for the first option. There are actually two programs:
the first should be run to let the RCX determine the minimum and
maximum raw-values of the sensor-inputs. During the execution of
the program Start_up the compass-sensor should turn in every
direction. The second program Manage_Compass first adjusts the curve of the sensor_2
values to the curve of sensor_1. This is done by the function:
f(v)=a*v+b, where a=(max1-min1)/(max2-min2) and b=max1-a*max2.
After this, the program checks for a couple of given values the
quadrant of the sin-cos functions where the point is to be found.
Then the program calculates the tangens as the ratio x1/x2, where
x1=abs(value1-midth) and x2=abs(value2-midth). As you only have
to determine the tangens for angles below 45 degrees, x1 and x2
are exchanged, if the ratio is greater than 1. The program
decides upon the case. The arctan is computed somehow tricky as
an approximation of 
.
One might be astonished about the
complicated calculations. But they are the result of the fact
that, equipped with the original firmware, the RCX may only treat
integer variables of two bytes length.
{...programs...}
#define(start_up,0)
{...tasks...}
#define(clicks,1)
{...sensortypes...}
#define(compass_type,3)
{...variables.....}
#define(min1,0)
#define(max1,1)
#define(min2,3)
#define(max2,4)
beginoftask(main)
setsensortype(sensor_1,compass_type)
setsensortype(sensor_2,compass_type)
setsensormode(sensor_1,raw_mode,0)
setsensormode(sensor_2,raw_mode,0)
starttask(clicks)
cleartimer(timer_1)
setvar(max1,con,-32000)
setvar(max2,con,-32000)
setvar(min1,con,32000)
setvar(min1,con,32000)
wait(con,500)
{wait 5 sec for power-up}
while(timer,timer_1,LT,con,150)
{15 seconds}
setvar(value1,senval,sensor_1)
setvar(value2,senval,sensor_2)
if(var,value1,GT,var,max1)
setvar(max1,var,value1)
endif()
if(var,value1,LT,var,min1)
setvar(min1,var,value1)
endif()
if(var,value2,GT,var,max2)
setvar(max2,var,value2)
endif()
if(var,value2,LT,var,min2)
setvar(min2,var,value2)
endif()
endwhile()
stoptask(clicks)
endoftask()
beginoftask(clicks)
loop(con,forever)
playsystemsound(click_sound)
wait(con,1500)
{wait 15 seconds}
endloop()
endoftask()
{.....programs.....}
#define(manage_compass,1)
{....tasks....}
#define(read_compass,1)
{....subroutines....}
#define(a*X,1)
#define(compute_angle,2)
{...variables.....}
#define(min1,0)
#define(max1,1)
#define(delta1,2)
#define(min2,3)
#define(max2,4)
#define(delta2,5)
#define(h,6)
#define(p,7)
#define(q,8)
#define(r,9)
#define(A,10)
#define(B,11)
#define(res,12)
#define(x,13)
#define(value1,14)
#define(value2,15)
#define(val2,16)
#define(midth,17)
#define(quadrant,18)
#define(x1,19)
#define(x2,20)
#define(Tan,21)
#define(Phi,22)
#define(case,23)
#define(i,24)
#define(y,25)
#define(f_y,26)
#define(arctan,27)
#define(alpha,28)
{...constants....}
#define(q1,1)
#define(q2,2)
#define(q3,3)
#define(q4,4)
#define(case1,1)
#define(case2,2)
{===========================================}
{===========================================}
{===========================================}
beginoftask(main)
setvar(delta1,var,max1)
subvar(delta1,var,min1)
setvar(delta2,var,max2)
subvar(delta2,var,min2)
{determine the rounded midth}
setvar(midth,var,max1)
sumvar(midth,var,min1)
mulvar(midth,con,10)
divvar(midth,con,2)
sumvar(midth,con,5)
divvar(midth,con,10)
{...................calculate a=d1/d2 and b=max1-a*max2.......}
{...................this is the adjusting function for the
curve2...}
{...example: min1=659;
max1=915; delta1=256................}
{...................min2=664; max2=909;
delta2=245................}
mulvar(delta1,con,100) {256*100=25600}
setvar(p,var,delta1) {25600}
divvar(p,var,delta2) {25600 div 245=104}
setvar(A,var,p) {104}
mulvar(A,con,100)
{104*100=10400}
mulvar(p,var,delta2)
{104*245=25480}
mulvar(p,con,-1) {-25480}
sumvar(p,var,delta1) {-25480+25600=120}
mulvar(p,con,100) {120*100=12000}
divvar(p,var,delta2) {12000 div 245=48}
sumvar(A,var,p) {10400+48=10448}
{...now we have A. 4 digits precise, no comma...}
setvar(x,var,max2)
gosub(a*X) {function, the result
9497 is in var res}
{which is 10 times the real result}
setvar(B,var,res) {9497}
mulvar(B,con,-1) {-9497}
setvar(h,var,max1) {915}
mulvar(h,con,10) {915*10=9150}
sumvar(B,var,h) {-9497+9150=-347;
e.a. 10 times the real B}
{....now we are able to
read compass values and adjust them}
starttask(read_compass)
endoftask()
{===========================================}
beginoftask(read_compass)
loop(con,forever)
setvar(value1,senval,sensor_1)
setvar(val2,senval,sensor_2)
{......now
adjust........}
setvar(x,var,val2)
gosub(a*x)
sumvar(res,var,B) {res=a*X+B, 10 times the true result}
divvar(res,con,10) {res=true adjusted result}
setvar(value2,var,res)
gosub(compute_angle)
endloop()
endoftask()
{============================================}
beginofsub(a*X)
{.....le'ts take x=909 as an example.....}
setvar(q,var,A) {10448}
divvar(q,con,1000) {10448 div 1000=10}
setvar(res,var,q)
{10}
mulvar(res,var,x) {10*909=9090}
mulvar(q,con,-1000)
{10*(-1000)=-10000}
sumvar(q,var,A) {-10000+10448=448}
setvar(r,var,q) {448}
divvar(q,con,100)
{448 div 100=4}
setvar(h,var,q) {4}
mulvar(h,var,x)
{4*909=3636}
mulvar(q,con,-100) {4*100=-400}
sumvar(r,var,q) {448-400=48}
setvar(q,var,r) {48}
divvar(q,con,10)
{48 div 10=4}
setvar(p,var,q)
{4}
mulvar(p,var,x)
{4*909=3636}
divvar(p,con,10) {3636
div 10=363}
sumvar(h,var,p) {3636+363=3999}
mulvar(q,con,-10)
{4*(-10)=-40}
sumvar(r,var,q) {48-40=8}
mulvar(r,var,x) {8*909=7272}
divvar(r,con,100) {7272
div 100=72}
sumvar(h,var,r)
{3999+72=4071}
divvar(h,con,10)
{4071 div 10=407}
sumvar(res,var,h)
{9090+407=9497}
endofsub()
beginofsub(compute_angle)
if(var,value1,GT,var,midth)
if(var,value2,GT,var,midth)
setvar(quadrant,con,q1)
else()
setvar(quadrant,con,q2)
endif()
else()
if(var,value2,GT,var,midth)
setvar(quadrant,con,q4)
else()
setvar(quadrant,con,q3)
endif()
endif()
setvar(x1,var,value1) {x1:=abs(value1-midth)}
subvar(x1,var,midth)
absvar(x1,var,x1)
setvar(x2,var,value2) {x2:=abs(value2-midth)}
subvar(x2,var,midth)
absvar(x2,var,x2)
if(var,x1,GT,var,x2)
setvar(case,con,case1)
setvar(i,var,x1) {exchange x1 and x2}
setvar(x1,var,x2)
setvar(x2,var,i)
else()
setvar(case,con,case2)
endif()
setvar(tan,var,x1) {tangens}
mulvar(tan,con,100)
divvar(tan,var,x2)
setvar(arctan,con,0) {clear}
setvar(i,con,0)
{arctan as integral of f(y)=1/(1+y*y)}
while(var,i,LT,var,tan)
sumvar(i,con,1)
setvar(y,var,i)
mulvar(y,var,y) {square, now we have 4 to
5 digits}
divvar(y,con,10) {div 100, rounded}
sumvar(y,con,5)
divvar(y,con,10)
sumvar(y,con,100) {we take f(y)=10000/(100+y*y/100)}
setvar(f_y,con,10000)
divvar(f_y,var,y)
sumvar(arctan,var,f_y)
endwhile()
divvar(arctan,con,314) {arctan/pi}
mulvar(arctan,con,180)
divvar(arctan,con,10) {/100 rounded}
sumvar(arctan,con,5)
divvar(arctan,con,10)
if(var,quadrant,eq,con,q1)
if(var,case,eq,con,case1)
setvar(phi,con,90)
subvar(phi,var,arctan)
else()
setvar(phi,var,arctan)
endif()
endif()
if(var,quadrant,eq,con,q2)
if(var,case,eq,con,case1)
setvar(phi,con,90)
sumvar(phi,var,arctan)
else()
setvar(phi,con,180)
subvar(phi,var,arctan)
endif()
endif()
if(var,quadrant,eq,con,q3)
if(var,case,eq,con,case1)
setvar(phi,con,270)
subvar(phi,var,arctan)
else()
setvar(phi,con,180)
sumvar(phi,var,arctan)
endif()
endif()
if(var,quadrant,eq,con,q4)
if(var,case,eq,con,case1)
setvar(phi,con,270)
sumvar(phi,var,arctan)
else()
setvar(phi,con,360)
subvar(phi,var,arctan)
endif()
endif()
setvar(alpha,var,phi)
endofsub()
