The starting point of this positioning module is that we have three Infrared emitters located on three edges of a rectangle:
Our robot is located somewhere in this rectangle:
On the top of the robot a IR sensor rotates 360 degrees and detects the angle between the three IR transmitters:
Please realize that Angle 3 will not be used in the calculation, but will disturb an easy positioning, I will come to that in a later stage when I start programming the Arduino!
To calculate the position of the robot one must realize that the robot has to be on the interception of two circles (on one circle there is the robot and the IR emitter at (0,0) and one other IR emitter, on the second circle is also the robot and the (0,0) emitter and the remaining IR emitter):
Now it's time to do some mathematics!
To calculate the center of the right circle we first calculate it's radius:
R1=(XL/sin(Angle2))/2; where XL is the distance between the IR sources on the horizontal axis.
It will be obvious that the X position of the circles' centre will be halfway the bottom two IR emitters:
X1=XL/2
Now the Y position of the centre can be calculated using Pythagoras: (R^2=X^2+Y^2 --> Y^2=R^2-X^2 -->)
Y1=sqrt(R1^2-X1^2)
The same can be done for the other circle:
R2=(YL/sin(Angle1))/2
Y2=YL/2
X2=sqrt(R2^2-Y2^2)
OK, now we know the centres of the two circles, how to find the intersection of them? In fact that is simple; we have to find the equation of the line connecting both centres (Y=MX+N):
M=(Y2-Y1)/(X2-X1)
N=Y1+((Y1-Y2)*X1)/(X2-X1)
The robot will be at a line from (0,0) perpendicular to this line:
The directional coefficient of this new line will be:
DC=-1/M
The second line does not have a Y offset as it goes through (0,0), so it's function is Y=DC*X
We now make the two functions equal to each other to find the X value of the intersection of the two lines:
M*X+N=DC*X -->
Xintercept=-N/(M-DC)
The X position of the robot will be at twice the X position of the intercept:
So
XRobot=2*XIntercept
and the using this X position in the function of the last line gives:
YRobot=DC*XRobot