diff --git a/src/linearHallSensor.cpp b/src/linearHallSensor.cpp
index c574239..0202af4 100644
--- a/src/linearHallSensor.cpp
+++ b/src/linearHallSensor.cpp
@@ -67,19 +67,19 @@ void LinearHallSensor::init(BLDCMotor motor)
     MotionControlType prevController = motor.controller;
     motor.controller = MotionControlType::angle_openloop;
     float prevVoltageLimit = motor.voltage_limit;
-    motor.voltage_limit = 0.8;
+    motor.voltage_limit = 1.5;
 
     // Swipe motor to search hard end and find max analog values of sensors
     bool endFound = false;
     const float step = 0.0025;
     uint8_t currentCheck = 0;  // current check number
-    const uint8_t nCheck = 15; // number of times to check if its the same value
     const uint8_t epsilon = 2;
     float currentPosition = 0.0;
 
-    uint32_t senseA[30];
-    uint32_t senseB[30];
-    int N = 30;
+    const uint8_t N = 40;
+    uint32_t senseA[N];
+    uint32_t senseB[N];
+    
     int ptr = 0;
 
     init_arr(senseA, N, 0);
@@ -117,9 +117,8 @@ void LinearHallSensor::init(BLDCMotor motor)
         // Move to new position
         currentPosition += step;
         motor.move(currentPosition);
-        delay(2);
+        delay(1);
     }
-    // _maxPositionEndValue = currentPosition - (step * nCheck);
     _maxPositionEndValue = currentPosition - (step * (ptr > N ? N : ptr));
     currentPosition = _maxPositionEndValue - M_PI / 8;
     delay(100);
@@ -158,7 +157,7 @@ void LinearHallSensor::init(BLDCMotor motor)
         // Move to new position
         currentPosition -= step;
         motor.move(currentPosition);
-        delay(2);
+        delay(1);
     }
     _minPositionEndValue = currentPosition + (step * (ptr > N ? N : ptr));
     Serial.println("\t- Found second end stop : Min position = " + String(_minPositionEndValue));
@@ -175,7 +174,7 @@ void LinearHallSensor::init(BLDCMotor motor)
     Serial.println("\t- Offset: " + String(_offset));
 
     // Read max angle
-    for (float i = _minPositionEndValue; i <= _maxPositionEndValue; i = i + 0.005)
+    for (float i = _minPositionEndValue; i <= _maxPositionEndValue; i = i + 0.0025)
     {
         motor.move(i);
         delay(1);
@@ -189,10 +188,10 @@ void LinearHallSensor::init(BLDCMotor motor)
         sum += readSensorCallback();
     _maxSensor = sum / 50.0;
 
-    Serial.println("\t- Max sensor: " + String(_maxSensor));
+    Serial.println("\t- Max angle: " + String(_maxSensor));
 
     // Recovering previous controller type and go in the middle
-    for (float i = _maxPositionEndValue; i >= (_maxPositionEndValue + _minPositionEndValue) / 2.0; i = i - 0.01)
+    for (float i = _maxPositionEndValue; i >= (_maxPositionEndValue + _minPositionEndValue); i = i - 0.005)
     {
         motor.move(i);
         delay(1);
diff --git a/src/main.cpp b/src/main.cpp
index 6ab9c20..f82fb70 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -7,6 +7,10 @@ BLDCMotor motor1 = BLDCMotor(4);
 BLDCDriver3PWM driver1 = BLDCDriver3PWM(M3_PWM1, M3_PWM2, M3_PWM3);
 LinearHallSensor linearSensor1 = LinearHallSensor(M3_Hall1, M3_Hall2);
 
+BLDCMotor motor2 = BLDCMotor(4);
+BLDCDriver3PWM driver2 = BLDCDriver3PWM(M1_PWM2, M1_PWM1, M1_PWM3);
+LinearHallSensor linearSensor2 = LinearHallSensor(M1_Hall1, M1_Hall2);
+
 void initSensor1()
 {
     linearSensor1.init(motor1);
@@ -16,7 +20,17 @@ float callback1()
     return linearSensor1.readSensorCallback();
 }
 
+void initSensor2()
+{
+    linearSensor2.init(motor2);
+}
+float callback2()
+{
+    return linearSensor2.readSensorCallback();
+}
+
 GenericSensor sensor1 = GenericSensor(callback1, initSensor1);
+GenericSensor sensor2 = GenericSensor(callback2, initSensor2);
 
 float targetX = 0.0;
 float targetY = 0.0;
@@ -49,14 +63,17 @@ void setup()
 {
 
     Serial.begin(115200);
-    delay(5000);
+    delay(3000);
     Serial.println("INIT");
 
     pinMode(LED_BUILTIN, OUTPUT); // Lightup LED
     digitalWrite(LED_BUILTIN, LOW);
 
-    driver1.voltage_power_supply = 7.1;
-    driver1.init();
+    // ### MOTOR 1 ###
+    Serial.println("\n\t\t### MOTOR 1 ###\n");
+    driver1.voltage_power_supply = 7.0;
+    Serial.println("Driver1 init: " + String(driver1.init()));
+    Serial.println("Driver2 init: " + String(driver2.init()));
     motor1.linkDriver(&driver1);
     motor1.useMonitoring(Serial);
     motor1.controller = MotionControlType::angle;
@@ -71,17 +88,53 @@ void setup()
     motor1.P_angle.I = 10.0;
     motor1.velocity_limit = 25;
 
-    // Init sensor
+    // Init sensor 1
     motor1.init();
     Serial.println("calibrating sensor in open loop...");
     sensor1.init();
-    Serial.println("Done");
+    Serial.println("Sensor 1 done");
     delay(1000);
 
     motor1.linkSensor(&sensor1);
     motor1.init();
+    motor1.initFOC(2.98, CW);
+    // motor1.initFOC();
+    Serial.println("Motor 1 Done");
+    motor1.disable();
+
+    delay(1000);
+
+    // ### MOTOR 2 ###
+    Serial.println("\n\t\t### MOTOR 2 ###\n");
+    motor2.useMonitoring(Serial);
+    driver2.voltage_power_supply = 7.0;
+    // Serial.println("Driver init: " + String(driver2.init()));
+    motor2.linkDriver(&driver2);
+
+    motor2.controller = MotionControlType::angle;
+    motor2.foc_modulation = FOCModulationType::SinePWM;
+    motor2.voltage_limit = 1.0;
+    motor2.voltage_sensor_align = 1.0;
+    motor2.PID_velocity.P = 0.05f;
+    motor2.PID_velocity.I = 0.01;
+    motor2.PID_velocity.D = 0.0;
+    motor2.LPF_velocity.Tf = 0.01f;
+    motor2.P_angle.P = 150.0;
+    motor2.P_angle.I = 10.0;
+    motor2.velocity_limit = 25;
+
+    // Init sensor 2
+    motor2.init();
+    Serial.println("calibrating sensor 2 in open loop...");
+    sensor2.init();
+    Serial.println("Sensor 2 done");
+    delay(1000);
+
+    motor2.linkSensor(&sensor2);
+    motor2.init();
     // motor.initFOC(5.48, CCW);
-    motor1.initFOC();
+    motor2.initFOC();
+    Serial.println("Motor 2 Done");
 }
 
 void loop()
@@ -90,4 +143,8 @@ void loop()
     motor1.move(target);
     motor1.loopFOC();
     motor1.monitor();
+
+    motor2.move(target);
+    motor2.loopFOC();
+    motor2.monitor();
 }
\ No newline at end of file