diff --git a/src/main.cpp b/src/main.cpp
index e646aea..f060e1a 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -1,20 +1,21 @@
 #include <Arduino.h>
 #include <SimpleFOC.h>
 #include <linearHallSensor.h>
+#include <pinout.h>
 
 BLDCMotor motor = BLDCMotor(4);
-BLDCDriver3PWM driver = BLDCDriver3PWM(7, 8, 9);
+BLDCDriver3PWM driver = BLDCDriver3PWM(M2_PWM1, M2_PWM2, M2_PWM3);
 
-LinearHallSensor linearSensor = LinearHallSensor(A0, A1, -0.75, 2.35);
+LinearHallSensor linearSensor = LinearHallSensor(M2_Hall1, M2_Hall2, -0.75, 2.35);
 
 void initSensor()
 {
-  linearSensor.init(motor);
+    linearSensor.init(motor);
 }
 
 float callback()
 {
-  return linearSensor.readSensorCallback();
+    return linearSensor.readSensorCallback();
 }
 
 GenericSensor sensor = GenericSensor(callback, initSensor);
@@ -26,73 +27,92 @@ float target = 0.0;
 
 void serialLoop()
 {
-  static String received_chars;
-  while (Serial.available())
-  {
-    char inChar = (char)Serial.read();
-    received_chars += inChar;
-    if (inChar == '\n')
+    static String received_chars;
+    while (Serial.available())
     {
-      target = received_chars.toFloat();
-      Serial.print("Target = ");
-      Serial.print(target);
-      received_chars = "";
+        char inChar = (char)Serial.read();
+        received_chars += inChar;
+        if (inChar == '\n')
+        {
+            target = received_chars.toFloat();
+            Serial.print("Target = ");
+            Serial.print(target);
+            received_chars = "";
+        }
     }
-  }
 }
 
 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;
+    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 = 120.0;
-  motor.P_angle.I = 10.0;
-  motor.velocity_limit = 50;
+    Serial.begin(115200);
+    delay(2000);
+    Serial.println("INIT");
 
-  motor.useMonitoring(Serial);
+    // pinMode(M1_PWM1, OUTPUT);
+    // pinMode(M1_PWM2, OUTPUT);
+    // pinMode(M1_PWM3, OUTPUT);
+    // digitalWrite(M1_PWM1, LOW);
+    // digitalWrite(M1_PWM2, LOW);
+    // digitalWrite(M1_PWM3, LOW);
 
-  // Init sensor
-  motor.init();
-  Serial.println("calibrating sensor in open loop...");
-  sensor.init();
-  Serial.println("Done");
-  delay(1000);
+    // pinMode(M2_PWM1, OUTPUT);
+    // pinMode(M2_PWM2, OUTPUT);
+    // pinMode(M2_PWM3, OUTPUT);
+    // digitalWrite(M2_PWM1, LOW);
+    // digitalWrite(M2_PWM2, LOW);
+    // digitalWrite(M2_PWM3, LOW);
 
-  motor.linkSensor(&sensor);
-  motor.init();
-  // motor.initFOC(5.48, CCW);
-  motor.initFOC();
+    // pinMode(M3_PWM1, OUTPUT);
+    // pinMode(M3_PWM2, OUTPUT);
+    // pinMode(M3_PWM3, OUTPUT);
+    // digitalWrite(M3_PWM1, LOW);
+    // digitalWrite(M3_PWM2, LOW);
+    // digitalWrite(M3_PWM3, LOW);
+
+    pinMode(LED_BUILTIN, OUTPUT);       // Lightup LED
+    digitalWrite(LED_BUILTIN, HIGH);
+
+    driver.voltage_power_supply = 5.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 = 120.0;
+    motor.P_angle.I = 10.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(5.48, CCW);
+    motor.initFOC();
 }
 
 void loop()
 {
-  serialLoop();
-  motor.move(target);
-  motor.loopFOC();
-  motor.monitor();
+    serialLoop();
+    motor.move(target);
+    motor.loopFOC();
+    motor.monitor();
 }
\ No newline at end of file
diff --git a/src/pinout.h b/src/pinout.h
new file mode 100644
index 0000000..c28e5c7
--- /dev/null
+++ b/src/pinout.h
@@ -0,0 +1,29 @@
+#ifndef pinout_h
+#define pinout_h
+
+#include "Arduino.h"
+
+// PCB board : STM32 DJI Gimbal V1.0
+
+#define M1_PWM1 PB13
+#define M1_PWM2 PB14
+#define M1_PWM3 PB15
+#define M1_Fault PB12
+#define M1_Hall1 PB1
+#define M1_Hall2 PB0
+
+#define M2_PWM1 PB9
+#define M2_PWM2 PB8
+#define M2_PWM3 PB5
+#define M2_Fault PB10
+#define M2_Hall1 PA4
+#define M2_Hall2 PA3
+
+#define M3_PWM1 PA5
+#define M3_PWM2 PA6
+#define M3_PWM3 PA7
+#define M3_Fault PA0
+#define M3_Hall1 PA2
+#define M3_Hall2 PA1
+
+#endif
\ No newline at end of file