diff --git a/platformio.ini b/platformio.ini
index d55db0d..9f28ed2 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -15,4 +15,4 @@ framework = arduino
 monitor_port = COM3
 monitor_speed = 115200
 lib_archive = no
-; lib_deps = askuric/Simple FOC@^2.2.2
+lib_deps = askuric/Simple FOC@^2.2.2
diff --git a/src/linearHallSensor.cpp b/src/linearHallSensor.cpp
new file mode 100644
index 0000000..560f0b3
--- /dev/null
+++ b/src/linearHallSensor.cpp
@@ -0,0 +1,223 @@
+#include "linearHallSensor.h"
+
+float norm(float x, float in_min, float in_max)
+{
+    return (float)(x + 1.0) * (2.0) / (float)(in_max - in_min) -1.0;
+}
+
+LinearHallSensor::LinearHallSensor(uint8_t ch1, uint8_t ch2)
+{
+    _analogPin1 = ch1;
+    _analogPin2 = ch2;
+}
+
+void LinearHallSensor::init(BLDCMotor motor)
+{
+    int senseA = analogRead(A0);
+    int senseB = analogRead(A1);
+
+    int minA = senseA, maxA = senseA, minB = senseB, maxB = senseB;
+
+    // Swipe motor 2 times
+    for (float i = -0.55; i <= 2.25; 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);
+    }
+
+    delay(100);
+
+    for (float i = 2.25; i >= -0.55; 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);
+    }
+
+    _maxCh1 = maxA;
+    _maxCh2 = maxB;
+    _minCh1 = minA;
+    _minCh2 = minB;
+
+    // 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;
+
+    //measure offset angle
+    float normCh1 = norm(_prevCh1, _minCh1, _maxCh1);
+    float normCh2 = norm(_prevCh2, _minCh2, _maxCh2);
+    float pos = atan2(normCh1, normCh2);
+
+    _offset =  M_PI_2 - pos; 
+
+
+    Serial.println("maxA: " + String(maxA) + "\tminA: " + String(minA) + "\tmaxB: " + String(maxB) + "\tminB: " + String(minB));
+    Serial.println("Current Ch1:" + String(_prevCh1) + "\tCh2:" + String(_prevCh2));
+}
+
+
+float LinearHallSensor::readSensorCallback()
+{
+    // Update variables
+    float currentCh1 = analogRead(_analogPin1);
+    float currentCh2 = analogRead(_analogPin2);
+
+    //  Normalise variable between -1 and 1
+    float normCh1 = norm(currentCh1, _minCh1, _maxCh1);
+    float normCh2 = norm(currentCh2, _minCh2, _maxCh2);
+
+    _currentPosition = atan2f(normCh2, normCh1);
+    Serial.print("Current angle: " + String(_currentPosition));
+
+    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;
+}
diff --git a/src/linearHallSensor.h b/src/linearHallSensor.h
new file mode 100644
index 0000000..531fbb2
--- /dev/null
+++ b/src/linearHallSensor.h
@@ -0,0 +1,68 @@
+#ifndef LinearHallSensor_h
+#define LinearHallSensor_h
+
+#include "Arduino.h"
+#include "BLDCMotor.h"
+
+enum Slope : int8_t{
+    INCREASING    = 1,
+    DECREASING    = -1,
+    EQUAL     = 0
+};
+
+/**
+ * @brief Linear Hall sensor class
+ * 
+ */
+class LinearHallSensor
+{
+    public:
+        /**
+         Linear Hall Sensor class constructor
+        @param ch1 Analog pin for channel 1
+        @param ch2 Analog pin for channel 2
+        */ 
+        LinearHallSensor(uint8_t ch1, uint8_t ch2);
+
+
+        /**
+         * @brief Initialize variable by measuring max and min value
+         * 
+         */
+        void init(BLDCMotor motor);
+
+
+        /**
+         * @brief Read the sensors and return the current angle in rad
+         * 
+         * @return rad and angle 
+         */
+        float readSensorCallbackStateMachine();
+
+        float readSensorCallback();
+
+    private:
+        uint8_t _analogPin1;
+        uint8_t _analogPin2;
+        
+        uint16_t _maxCh1;
+        uint16_t _maxCh2;
+        uint16_t _minCh1;
+        uint16_t _minCh2;
+
+        float _currentPosition;
+        float _offset;
+
+        float _minPositionEndValue;
+        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 ba60637..0911a69 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -1,7 +1,8 @@
 #include <Arduino.h>
 #include <SimpleFOC.h>
+#include <linearHallSensor.h>
+
 
-MagneticSensorAnalog sensor = MagneticSensorAnalog(A0, 183, 512);
 
 BLDCMotor motorX = BLDCMotor(4);
 BLDCDriver3PWM driverX = BLDCDriver3PWM(3, 5, 6);
@@ -9,6 +10,21 @@ BLDCDriver3PWM driverX = BLDCDriver3PWM(3, 5, 6);
 BLDCMotor motorY = BLDCMotor(4);
 BLDCDriver3PWM driverY = BLDCDriver3PWM(7, 8, 9);
 
+LinearHallSensor linearSensor = LinearHallSensor(A0, A1);
+
+void initSensor()
+{
+  linearSensor.init(motorY);
+}
+
+float callback()
+{
+  return linearSensor.readSensorCallback();
+}
+
+GenericSensor sensor = GenericSensor(callback, initSensor);
+
+
 float targetX = 0.0;
 float targetY = 0.0;
 
@@ -16,37 +32,35 @@ float target = 6.3;
 
 void setup()
 {
-
-  sensor.init();
-  // motorY.linkSensor(&sensor);
-
   // 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.PID_velocity.P = 0.02f;
-  motorX.PID_velocity.I = 20;
-  motorX.PID_velocity.D = 0;
-  motorX.voltage_limit = 0.8;
-  motorX.LPF_velocity.Tf = 0.01f;
-  motorX.P_angle.P = 20;
-  motorX.velocity_limit = 20;
+  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::SpaceVectorPWM;
+  motorY.foc_modulation = FOCModulationType::SinePWM;
   motorY.controller = MotionControlType::angle_openloop;
-  motorY.PID_velocity.P = 0.001f;
-  motorY.PID_velocity.I = 0;
-  motorY.PID_velocity.D = 0;
   motorY.voltage_limit = 0.8;
-  motorY.LPF_velocity.Tf = 0.01f;
-  motorY.P_angle.P = 0.1;
-  motorY.velocity_limit = 3;
+  // 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);
@@ -58,10 +72,14 @@ void setup()
   motorY.init();
   motorY.initFOC();
 
-  Serial.println(F("Motor ready."));
-  _delay(1000);
-  // Serial.println(sensor.getPreciseAngle());
+  motorX.disable();
 
+  Serial.println(F("Motor ready."));
+  _delay(100);
+  Serial.println("calibrating...");
+  sensor.init();
+  Serial.println("Done\r\n");
+  motorY.disable();   
 }
 
 void serialLoop()
@@ -89,30 +107,26 @@ float mapfloat(float x, float in_min, float in_max, float out_min, float out_max
 void loop()
 {
   sensor.update();
+  // serialLoop();
 
-  // Serial.println(sensor.getPreciseAngle());
-  // delay(5);
+  // int senseX = analogRead(A2);
+  // int senseY = analogRead(A3);
 
-  serialLoop();
+  // targetX = mapfloat(senseX, 0.0, 1024.0, -0.6, 1.2);
+  // targetY = mapfloat(senseY, 0.0, 1024.0, -0.5, 2.2);
 
-  int senseX = analogRead(A2);
-  int senseY = analogRead(A3);
+  // 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);
 
-  targetX = mapfloat(senseX, 0.0, 1024.0, -0.6, 1.2);
-  targetY = mapfloat(senseY, 0.0, 1024.0, -0.5, 2.2);
+  // motorX.move(targetX);
+  // motorX.loopFOC();
 
-  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();
+  // motorY.move(targetY);
+  // motorY.loopFOC();
 }
\ No newline at end of file