diff --git a/src/linearHallSensor.cpp b/src/linearHallSensor.cpp
index 560f0b3..637e6ea 100644
--- a/src/linearHallSensor.cpp
+++ b/src/linearHallSensor.cpp
@@ -1,223 +1,128 @@
 #include "linearHallSensor.h"
 
-float norm(float x, float in_min, float in_max)
+float norm(float x, float in_min, float in_max, float out_min = -1.0, float out_max = 1.0)
 {
-    return (float)(x + 1.0) * (2.0) / (float)(in_max - in_min) -1.0;
+    return (float)(x - in_min) * (out_max - out_min) / (float)(in_max - in_min) + out_min;
 }
 
-LinearHallSensor::LinearHallSensor(uint8_t ch1, uint8_t ch2)
+LinearHallSensor::LinearHallSensor(uint8_t ch1, uint8_t ch2, float minPos, float maxPos)
 {
     _analogPin1 = ch1;
     _analogPin2 = ch2;
+    _minPositionEndValue = minPos;
+    _maxPositionEndValue = maxPos;
 }
 
 void LinearHallSensor::init(BLDCMotor motor)
 {
-    int senseA = analogRead(A0);
-    int senseB = analogRead(A1);
+    MotionControlType prevController = motor.controller;
+    motor.controller = MotionControlType::angle_openloop;
+    float prevVoltageLimit = motor.voltage_limit;
+    motor.voltage_limit = 0.8;
 
-    int minA = senseA, maxA = senseA, minB = senseB, maxB = senseB;
+    uint32_t senseA = analogRead(_analogPin1);
+    uint32_t senseB = analogRead(_analogPin2);
 
-    // Swipe motor 2 times
-    for (float i = -0.55; i <= 2.25; i = i + 0.005)
+    _minCh1 = senseA, _maxCh1 = senseA;
+    _minCh2 = senseB, _maxCh2 = senseB;
+
+    // Swipe motor
+    for (float i = _minPositionEndValue; i <= _maxPositionEndValue; i = i + 0.005)
     {
         motor.move(i);
-        senseA = analogRead(A0);
-        senseB = analogRead(A1);
-        if (senseA > maxA)
-            maxA = senseA;
-        else if (senseA < minA)
-            minA = senseA;
-        if (senseB > maxB)
-            maxB = senseB;
-        else if (senseB < minB)
-            minB = senseB;
         delay(1);
+        senseA = analogRead(_analogPin1);
+        senseB = analogRead(_analogPin2);
+        if (senseA > _maxCh1)
+            _maxCh1 = senseA;
+        else if (senseA < _minCh1)
+            _minCh1 = senseA;
+        if (senseB > _maxCh1)
+            _maxCh1 = senseB;
+        else if (senseB < _minCh2)
+            _minCh2 = senseB;
     }
-
     delay(100);
-
-    for (float i = 2.25; i >= -0.55; i = i - 0.005)
+    for (float i = _maxPositionEndValue; i >= _minPositionEndValue; i = i - 0.005)
     {
         motor.move(i);
-        senseA = analogRead(A0);
-        senseB = analogRead(A1);
-        if (senseA > maxA)
-            maxA = senseA;
-        else if (senseA < minA)
-            minA = senseA;
-        if (senseB > maxB)
-            maxB = senseB;
-        else if (senseB < minB)
-            minB = senseB;
         delay(1);
+        senseA = analogRead(_analogPin1);
+        senseB = analogRead(_analogPin2);
+        if (senseA > _maxCh1)
+            _maxCh1 = senseA;
+        else if (senseA < _minCh1)
+            _minCh1 = senseA;
+        if (senseB > _maxCh1)
+            _maxCh1 = senseB;
+        else if (senseB < _minCh2)
+            _minCh2 = senseB;
     }
 
-    _maxCh1 = maxA;
-    _maxCh2 = maxB;
-    _minCh1 = minA;
-    _minCh2 = minB;
+    // Min offset with 50 values average
+    float sum = 0.0;
+    for (uint8_t i = 0; i <= 49; i++)
+    {
+        _offset = 0.0;
+        sum += readSensorCallback();
+    }
+    _offset = -sum / 50.0;
 
-    // Intialise the state machine variables
-    _state = 0; // Set the state machine to 0
-    _prevCh1 = analogRead(_analogPin1);
-    _prevCh2 = analogRead(_analogPin2);
-    _directionCh1 = Direction::UNKNOWN;
-    _directionCh2 = Direction::UNKNOWN;
+    Serial.println("\n\rmaxA: " + String(_maxCh1) + "\tminA: " + String(_minCh1) + "\tmaxB: " + String(_maxCh2) + "\tminB: " + String(_minCh2));
+    Serial.println("Offset: " + String(_offset));
 
-    //measure offset angle
-    float normCh1 = norm(_prevCh1, _minCh1, _maxCh1);
-    float normCh2 = norm(_prevCh2, _minCh2, _maxCh2);
-    float pos = atan2(normCh1, normCh2);
+    // Detect max angle
+    for (float i = _minPositionEndValue; i <= _maxPositionEndValue; i = i + 0.01)
+    {
+        motor.move(i);
+        delay(1);
+        _maxSensor = readSensorCallback();
+    }
+    Serial.println("current motor angle: " + String(motor.electricalAngle()));
+    delay(250);
 
-    _offset =  M_PI_2 - pos; 
+    // take average value of 50 values
+    sum = 0.0;
+    for (uint8_t i = 0; i <= 49; i++)
+        sum += readSensorCallback();
+    _maxSensor = sum / 50.0;
 
+    Serial.println("Max sensor: " + String(_maxSensor));
 
-    Serial.println("maxA: " + String(maxA) + "\tminA: " + String(minA) + "\tmaxB: " + String(maxB) + "\tminB: " + String(minB));
-    Serial.println("Current Ch1:" + String(_prevCh1) + "\tCh2:" + String(_prevCh2));
+    // Recovering previous controller type and go in the middle
+    for (float i = _maxPositionEndValue; i >= _minPositionEndValue/2.0; i = i - 0.01)
+    {
+        motor.move(i);
+        delay(1);
+        readSensorCallback();
+    }
+    motor.controller = prevController;
+    motor.voltage_limit = prevVoltageLimit;
 }
 
+float dist_angle(float newAngle, float prevAngle)
+{
+    float diff = newAngle - prevAngle;
+    while (diff < (-M_PI))
+        diff += 2 * M_PI;
+    while (diff > M_PI)
+        diff -= 2 * M_PI;
+    return diff;
+}
 
 float LinearHallSensor::readSensorCallback()
 {
     // Update variables
-    float currentCh1 = analogRead(_analogPin1);
-    float currentCh2 = analogRead(_analogPin2);
+    uint32_t currentCh1 = analogRead(_analogPin1);
+    uint32_t currentCh2 = analogRead(_analogPin2);
 
     //  Normalise variable between -1 and 1
-    float normCh1 = norm(currentCh1, _minCh1, _maxCh1);
-    float normCh2 = norm(currentCh2, _minCh2, _maxCh2);
+    float normCh1 = norm((float)currentCh1, _minCh1, _maxCh1);
+    float normCh2 = norm((float)currentCh2, _minCh2, _maxCh2);
 
     _currentPosition = atan2f(normCh2, normCh1);
-    Serial.print("Current angle: " + String(_currentPosition));
+    _estimatePosition += dist_angle(_currentPosition, _prevPosition);
 
-    float temp1 = normCh1 + sin(_offset);
-    normCh1 = temp1 * floorf(temp1);
-
-    float temp2 = normCh2 + cos(_offset);
-    normCh2 = temp2 * floorf(temp2);
-
-    _currentPosition = atan2f(normCh2, normCh1);
-    Serial.print("\tNew angle: " + String(_currentPosition)+ '\r');
-
-    return _currentPosition;
-}
-
-float LinearHallSensor::readSensorCallbackStateMachine()
-{
-    // Update variables
-    float currentCh1 = analogRead(_analogPin1);
-    float currentCh2 = analogRead(_analogPin2);
-
-    if (currentCh1 > _prevCh1)
-        _directionCh1 = Slope::INCREASING;
-    else if (currentCh1 < _prevCh1)
-        _directionCh1 = Slope::DECREASING;
-    else
-        _directionCh1 = Slope::EQUAL;
-
-    if (currentCh2 > _prevCh2)
-        _directionCh2 = Slope::INCREASING;
-    else if (currentCh2 < _prevCh2)
-        _directionCh2 = Slope::DECREASING;
-    else
-        _directionCh2 = Slope::EQUAL;
-
-    //  Normalise variable between -1 and 1
-    float normCh1 = norm(currentCh1, _minCh1, _maxCh1);
-    float normCh2 = norm(currentCh2, _minCh2, _maxCh2);
-
-    //  Update previous var
-    _prevCh1 = currentCh1;
-    _prevCh2 = currentCh2;
-
-    // State machine
-    switch (_state)
-    {
-    case 0:
-        _currentPosition = 0.0;
-        if (_directionCh1 == Slope::INCREASING)
-            _state = 1;
-        break;
-    case 1:
-        if(_directionCh1 != Slope::EQUAL)
-            _currentPosition++;
-
-        if (_directionCh1 == Slope::DECREASING)
-            _state = 2;
-        else if (normCh1 >= 0.8 && _directionCh1 == Slope::INCREASING)
-            _state = 3;
-        break;
-    case 2:
-        if(_directionCh1 != Slope::EQUAL)
-            _currentPosition--;
-
-        if (_directionCh1 == Slope::INCREASING)
-            _state = 1;
-        else if (_directionCh1 == Slope::DECREASING && normCh2 <= 0.2)
-            _state = 8;
-        break;
-    case 3:
-        if(_directionCh2 != Slope::EQUAL)
-            _currentPosition++;
-
-        if (_directionCh2 == Slope::INCREASING)
-            _state = 4;
-        else if (_directionCh2 == Slope::DECREASING && normCh2 <= 0.2)
-            _state = 5;
-        break;
-    case 4:
-        if(_directionCh2 != Slope::EQUAL)
-            _currentPosition--;
-
-        if (_directionCh2 == Slope::DECREASING)
-            _state = 3;
-        else if (_directionCh2 == Slope::INCREASING && normCh1 <= 0.8)
-            _state = 2;
-        break;
-    case 5:
-        if(_directionCh1 != Slope::EQUAL)
-            _currentPosition++;
-        if (_directionCh1 == Slope::INCREASING)
-            _state = 6;
-        else if (_directionCh1 == Slope::DECREASING && normCh1 <= 0.2)
-            _state = 7;
-        break;
-    case 6:
-        if(_directionCh1 != Slope::EQUAL)
-            _currentPosition--;
-
-        if (_directionCh1 == Slope::DECREASING)
-            _state = 5;
-        else if (_directionCh1 == Slope::INCREASING && normCh2 >= 0.2)
-            _state = 4;
-        break;
-    case 7:
-        if(_directionCh2 != Slope::EQUAL)
-            _currentPosition++;
-
-        if (_directionCh2 == Slope::DECREASING)
-            _state = 8;
-        else if (_directionCh2 == Slope::INCREASING && normCh2 >= 0.8)
-            _state = 1;
-        break;
-    case 8:
-        if(_directionCh2 != Slope::EQUAL)
-            _currentPosition--;
-
-        if (_directionCh2 == Slope::INCREASING)
-            _state = 7;
-        else if (_directionCh2 == Slope::DECREASING && normCh1 >= 0.2)
-            _state = 6;
-        break;
-
-    default:
-        break;
-    }
-
-    Serial.print("\rState: " +     String(_state) + 
-    "\tSlope Ch1: " + String(_directionCh1) + "\tnormCh1: " + String(normCh1)+
-    "\tSlope Ch2: " + String(_directionCh2) + "\tnormCh2: " + String(normCh2)+
-    "\tPosition: " + String(_currentPosition));
-    return _currentPosition;
+    _prevPosition = _currentPosition;
+    return (_estimatePosition + _offset)/4.0;
 }
diff --git a/src/linearHallSensor.h b/src/linearHallSensor.h
index 531fbb2..28663b7 100644
--- a/src/linearHallSensor.h
+++ b/src/linearHallSensor.h
@@ -4,12 +4,6 @@
 #include "Arduino.h"
 #include "BLDCMotor.h"
 
-enum Slope : int8_t{
-    INCREASING    = 1,
-    DECREASING    = -1,
-    EQUAL     = 0
-};
-
 /**
  * @brief Linear Hall sensor class
  * 
@@ -21,8 +15,10 @@ class LinearHallSensor
          Linear Hall Sensor class constructor
         @param ch1 Analog pin for channel 1
         @param ch2 Analog pin for channel 2
+        @param minPos Minimum position in Openloop
+        @param maxPos Maximum position in Openloop
         */ 
-        LinearHallSensor(uint8_t ch1, uint8_t ch2);
+        LinearHallSensor(uint8_t ch1, uint8_t ch2, float minPos, float maxPos);
 
 
         /**
@@ -42,27 +38,23 @@ class LinearHallSensor
         float readSensorCallback();
 
     private:
-        uint8_t _analogPin1;
-        uint8_t _analogPin2;
+        uint32_t _analogPin1; 
+        uint32_t _analogPin2;
         
-        uint16_t _maxCh1;
-        uint16_t _maxCh2;
-        uint16_t _minCh1;
-        uint16_t _minCh2;
+        //  max values read from the ADC on channels during init
+        uint32_t _maxCh1;
+        uint32_t _maxCh2;
+        uint32_t _minCh1;
+        uint32_t _minCh2;
 
+        float _prevPosition;
         float _currentPosition;
-        float _offset;
+        float _estimatePosition;    // current position + offset
+        float _offset;              // offset angle measured at init
+        float _maxSensor;           // Max angle value measured (so command from 0 to that value)
 
-        float _minPositionEndValue;
+        float _minPositionEndValue; // min pos in open loop mode
         float _maxPositionEndValue;
-
-        uint16_t _prevCh1;
-        uint16_t _prevCh2;
-
-        int8_t _directionCh1;
-        int8_t _directionCh2;
-
-        uint8_t _state;
 };
 
 #endif
\ No newline at end of file
diff --git a/src/main.cpp b/src/main.cpp
index 0911a69..beab5d7 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -2,19 +2,14 @@
 #include <SimpleFOC.h>
 #include <linearHallSensor.h>
 
+BLDCMotor motor = BLDCMotor(4);
+BLDCDriver3PWM driver = BLDCDriver3PWM(7, 8, 9);
 
-
-BLDCMotor motorX = BLDCMotor(4);
-BLDCDriver3PWM driverX = BLDCDriver3PWM(3, 5, 6);
-
-BLDCMotor motorY = BLDCMotor(4);
-BLDCDriver3PWM driverY = BLDCDriver3PWM(7, 8, 9);
-
-LinearHallSensor linearSensor = LinearHallSensor(A0, A1);
+LinearHallSensor linearSensor = LinearHallSensor(A0, A1, -0.75, 2.35);
 
 void initSensor()
 {
-  linearSensor.init(motorY);
+  linearSensor.init(motor);
 }
 
 float callback()
@@ -24,63 +19,10 @@ float callback()
 
 GenericSensor sensor = GenericSensor(callback, initSensor);
 
-
 float targetX = 0.0;
 float targetY = 0.0;
 
-float target = 6.3;
-
-void setup()
-{
-  // X MOTR STUFF
-  driverX.voltage_power_supply = 5.0;
-  driverX.init();
-  motorX.linkDriver(&driverX);
-  motorX.foc_modulation = FOCModulationType::SpaceVectorPWM;
-  motorX.controller = MotionControlType::angle_openloop;
-  motorX.voltage_limit = 0.6;
-  // motorX.PID_velocity.P = 0.001f;
-  // motorX.PID_velocity.I = 20;
-  // motorX.PID_velocity.D = 0;
-  // motorX.LPF_velocity.Tf = 0.01f;
-  // motorX.P_angle.P = 20;
-  motorX.velocity_limit = 10;
-
-  // *************** Y MOTOR STUFF ******************
-  driverY.voltage_power_supply = 5.0;
-  driverY.init();
-  motorY.linkDriver(&driverY);
-  motorY.foc_modulation = FOCModulationType::SinePWM;
-  motorY.controller = MotionControlType::angle_openloop;
-  motorY.voltage_limit = 0.8;
-  // motorY.PID_velocity.P = 0.001f;
-  // motorY.PID_velocity.I = 0;
-  // motorY.PID_velocity.D = 0;
-  // motorY.LPF_velocity.Tf = 0.01f;
-  // motorY.P_angle.P = 0.1;
-  motorY.velocity_limit = 10;
-
-
-
-  Serial.begin(115200);
-  _delay(100);
-  Serial.println("INIT");
-
-  // initialize motor
-  motorX.init();
-  motorX.initFOC();
-  motorY.init();
-  motorY.initFOC();
-
-  motorX.disable();
-
-  Serial.println(F("Motor ready."));
-  _delay(100);
-  Serial.println("calibrating...");
-  sensor.init();
-  Serial.println("Done\r\n");
-  motorY.disable();   
-}
+float target = 0.0;
 
 void serialLoop()
 {
@@ -104,29 +46,53 @@ float mapfloat(float x, float in_min, float in_max, float out_min, float out_max
   return (float)(x - in_min) * (out_max - out_min) / (float)(in_max - in_min) + out_min;
 }
 
+void setup()
+{
+  Serial.begin(115200);
+  _delay(100);
+  Serial.println("INIT");
+  pinMode(3, OUTPUT);
+  pinMode(5, OUTPUT);
+  pinMode(6, OUTPUT);
+  digitalWrite(3, LOW);
+  digitalWrite(5, LOW);
+  digitalWrite(6, LOW);
+
+  driver.voltage_power_supply = 6.0;
+  driver.init();
+  motor.linkDriver(&driver);
+  motor.useMonitoring(Serial);
+  motor.controller = MotionControlType::angle;
+  motor.foc_modulation = FOCModulationType::SinePWM;
+  motor.voltage_limit = 1.5;
+  motor.voltage_sensor_align = 0.8;
+  motor.PID_velocity.P = 0.075f;
+  motor.PID_velocity.I = 0.01;
+  motor.PID_velocity.D = 0.0;
+  motor.LPF_velocity.Tf = 0.01f;
+  motor.P_angle.P = 200.0;
+  motor.P_angle.I = 50.0;
+  motor.velocity_limit = 50;
+
+  motor.useMonitoring(Serial);
+
+
+  // Init sensor
+  motor.init();
+  Serial.println("calibrating sensor in open loop...");
+  sensor.init();
+  Serial.println("Done");
+  delay(1000);
+
+  motor.linkSensor(&sensor);
+  motor.init();
+  motor.initFOC();
+}
+
 void loop()
 {
-  sensor.update();
-  // serialLoop();
-
-  // int senseX = analogRead(A2);
-  // int senseY = analogRead(A3);
-
-  // targetX = mapfloat(senseX, 0.0, 1024.0, -0.6, 1.2);
-  // targetY = mapfloat(senseY, 0.0, 1024.0, -0.5, 2.2);
-
-  // Serial.print("senseX=");
-  // Serial.print(senseX);
-  // Serial.print("\t mappedX=");
-  // Serial.print(targetX);
-  // Serial.print("\t senseY=");
-  // Serial.print(senseY);
-  // Serial.print("\t mappedY=");
-  // Serial.println(targetY);
-
-  // motorX.move(targetX);
-  // motorX.loopFOC();
-
-  // motorY.move(targetY);
-  // motorY.loopFOC();
+  serialLoop();
+  motor.move(target);
+  motor.loopFOC();
+  motor.monitor();
 }
\ No newline at end of file