Update docs

docs/LinearHallEncoder.md

@@ -45,13 +45,16 @@ We can see that in the first movement (positive rotation), the green is out of p
 
 ![Sinwave figure](cosSinEncoderDiagram.png)
 ### 3. Encoding the position 
-1. Get the angle of one periodic signal
+1. Get the absolute angle within a period
 
-Since the 2 signals correspond to a cos and sin signals, it is possible to compute the angle inside the period using arctan2 function. 
+Since the 2 signals correspond to a cos and sin signals, it is possible to compute the angle inside the period using arctan2 function. However, we have more than one period, it is so necessary to increment a position.
+$$\theta= atan2(a,b)$$
 
 2. Incremental position
-- increment periods 
-- start from known position (one end)
+
+To increment the position, it is necessary to start from 0 at a known postion. For that the motor is moved in open loop to one end and the position is set to 0.
+Then we need to sum all the delta of movement at each measure sample.
+$$\phi_t=\phi_{t-1} + (\theta_t - \theta_{t-1})mod(-\pi;\pi)$$
 
 ## Coding the solution
 1. Get the angle in the perdiod
@@ -61,14 +64,35 @@ Beforehand, the maximum and minimum peak of the signals need to be found. It can
 Then the arctan function can be applied. It is preferable to use arctan2 as it will give an angle within the 4 quadrants (-π,π). Whereas arctan give an angle between (-π/2,π/2). [Wikipedia](https://en.wikipedia.org/wiki/Atan2)
 
 ```C++
-float norm(float x, float in_min, float in_max)
+
+float LinearHallSensor::Callback()  // Return the estimated position of the sensor
+{
+    A = norm(analogRead(CH1),minCh1, maxCh1);   //read analog values and normalise between [-1;1]
+    B = norm(analogRead(CH2),minCh2, maxCh2);
+
+    theta = atan2(A,B); // Compute the absolute angle in the period
+
+    phi = phi + dist_angle(theta, theta_prev);  // increment the difference
+
+    theta_prev = theta; // save fot nex time
+
+    return phi;
+}
+
+float norm(float x, float in_min, float in_max) //return the input value normalised between [-1;1]
 {
     return (float)(x + 1.0) * (2.0) / (float)(in_max - in_min) -1.0;
 }
 
-A = norm(analogRead(CH1),minCh1, maxCh1);
-B = norm(analogRead(CH2),minCh2, maxCh2);
+float dist_angle(float newAngle, float prevAngle) // return the difference modulo [-pi;pi]
+{
+    float diff = newAngle - prevAngle;
+    while (diff < (-M_PI))
+        diff += 2 * M_PI;
+    while (diff > M_PI)
+        diff -= 2 * M_PI;
+    return diff;
+}
 
-angle = atan2(A,B);
 ```