diff --git a/boards/nucleo_h563zi.overlay b/boards/nucleo_h563zi.overlay
index 77af479..f5950c3 100644
--- a/boards/nucleo_h563zi.overlay
+++ b/boards/nucleo_h563zi.overlay
@@ -1,15 +1,15 @@
 / {
     aliases {
-        led0 = &red_led_1;
-
-        rotenc0 = &as5600;
-        stepper0 = &stepper0;
+        
     };
 
     chosen {
 		pid,counter = &counter5;
-        pid,status = &green_led_1;
-        pid,calibration = &yellow_led_1;
+        pid,active_led = &green_led_1;
+        pid,offset_led = &yellow_led_1;
+        pid,calibration_led = &red_led_1;
+        pid,rotenc = &as5600;
+        pid,stepper = &stepper0;
 	};
 
     stepper0: tmc2209_motor {
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index e838116..9201263 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -4,7 +4,7 @@ target_include_directories(app PRIVATE ./)
 
 add_subdirectory(./stepper_controller)
 add_subdirectory(./microros)
-#add_subdirectory(./usb)
+add_subdirectory(./usb)
 
 target_sources(app PRIVATE
     main.cpp
diff --git a/src/devices.cpp b/src/devices.cpp
index bb95100..b89388e 100644
--- a/src/devices.cpp
+++ b/src/devices.cpp
@@ -4,16 +4,17 @@
 bool verify_are_devices_available(void) {
     bool devices_ready = true;
 
-    if (!device_is_ready(led0.port)) {
-        printk("LED0 not ready\n");
-        devices_ready = false;
-    }
-
     if (!device_is_ready(status_led.port)) {
         printk("Status LED not ready\n");
         devices_ready = false;
     }
 
+    if (!device_is_ready(work_led.port)) {
+        printk("LED0 not ready\n");
+        devices_ready = false;
+    }
+
+
     if (!device_is_ready(calibration_led.port)) {
         printk("Calibration LED not ready\n");
         devices_ready = false;
diff --git a/src/devices.hpp b/src/devices.hpp
index 870cbb6..18f2ba8 100644
--- a/src/devices.hpp
+++ b/src/devices.hpp
@@ -12,12 +12,12 @@
 #include <devicetree.h>
 #endif
 
-static const struct gpio_dt_spec led0 = GPIO_DT_SPEC_GET(DT_ALIAS(led0), gpios);
-static const struct gpio_dt_spec status_led = GPIO_DT_SPEC_GET(DT_CHOSEN(pid_status), gpios);
-static const struct gpio_dt_spec calibration_led = GPIO_DT_SPEC_GET(DT_CHOSEN(pid_calibration), gpios);
+static const struct gpio_dt_spec status_led = GPIO_DT_SPEC_GET(DT_CHOSEN(pid_active_led), gpios);
+static const struct gpio_dt_spec work_led = GPIO_DT_SPEC_GET(DT_CHOSEN(pid_offset_led), gpios);
+static const struct gpio_dt_spec calibration_led = GPIO_DT_SPEC_GET(DT_CHOSEN(pid_calibration_led), gpios);
 
-static const struct device *rotenc0 = DEVICE_DT_GET(DT_ALIAS(rotenc0));
-static const struct device *stepper0 = DEVICE_DT_GET(DT_ALIAS(stepper0));
+static const struct device *rotenc0 = DEVICE_DT_GET(DT_CHOSEN(pid_rotenc));
+static const struct device *stepper0 = DEVICE_DT_GET(DT_CHOSEN(pid_stepper));
 static const struct device *pid_counter = DEVICE_DT_GET(DT_CHOSEN(pid_counter));
 
 bool verify_are_devices_available(void);
diff --git a/src/main.cpp b/src/main.cpp
index c1cd000..55bed2d 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -10,7 +10,7 @@
 #include <rmw_microros/rmw_microros.h>
 
 #include "devices.hpp"
-//#include "usb_cdc_acm.hpp"
+#include "usb_cdc_acm.hpp"
 #include "stepper_controller.hpp"
 #include "microros_state_machine.hpp"
 
@@ -23,7 +23,7 @@ int main(void)
 
     StepperController::init();
     
-    //usb_init();
+    usb_init();
 
     rmw_uros_set_custom_transport(
         MICRO_ROS_FRAMING_REQUIRED,
diff --git a/src/stepper_controller/stepper_controller.cpp b/src/stepper_controller/stepper_controller.cpp
index fbd3f36..1227c33 100644
--- a/src/stepper_controller/stepper_controller.cpp
+++ b/src/stepper_controller/stepper_controller.cpp
@@ -104,27 +104,36 @@ void StepperController::init() {
 void StepperController::calibrate() {
     gpio_pin_configure_dt(&status_led, GPIO_OUTPUT_INACTIVE);
     gpio_pin_configure_dt(&calibration_led, GPIO_OUTPUT_ACTIVE);
-    set_motor_velocity(0.1);
-    k_sleep(K_MSEC(250));
+    set_motor_velocity(0.02);
+    k_sleep(K_MSEC(1000));
 
     sample_angle();
     float left_angle = current_angle;
 
-    set_motor_velocity(-0.1);
-    k_sleep(K_MSEC(500));
+    set_motor_velocity(-0.02);
+    k_sleep(K_MSEC(2000));
 
     sample_angle();
     float right_angle = current_angle; 
 
-    set_motor_velocity(0.1);
-    k_sleep(K_MSEC(250));
-    
-    if (right_angle >= left_angle) {
+    set_motor_velocity(0.02);
+    k_sleep(K_MSEC(1000));
+
+    if (right_angle < left_angle) {
         direction_factor = -1.0;
     }
+    
+    printk("Servo direction calibration:\n");
+    printk("Left  = %d.%02d\n", (int)left_angle, abs((int)(left_angle * 100) % 100));
+    printk("Right = %d.%02d\n", (int)right_angle, abs((int)(right_angle * 100) % 100));
+    printk("-> %c\n" , direction_factor > 0 ? '+' : '-');
+    printk("----------------------------\n");
+
+    stepper_stop(stepper0);
+    k_sleep(K_MSEC(100));
+
     gpio_pin_configure_dt(&status_led, GPIO_OUTPUT_ACTIVE);
     gpio_pin_configure_dt(&calibration_led, GPIO_OUTPUT_INACTIVE);
-
 }
 
 rcl_ret_t StepperController::setup(rclc_support_t* support, rclc_executor_t* executor)
@@ -190,9 +199,9 @@ void StepperController::sample_angle() {
     float angle = direction_factor * ((float)val.val1 - 180.0f + (float)val.val2 / 1000000.0f);
 
     float full_angle = angle + 360.0f * (float)angle_rotation_count;
-    if (full_angle - last_angle > 270)
+    if (full_angle - last_angle > 330)
         angle_rotation_count--;
-    if (full_angle - last_angle < -270)
+    if (full_angle - last_angle < -330)
         angle_rotation_count++;
 
     last_angle = current_angle;
@@ -264,6 +273,8 @@ void StepperController::pid() {
 }
 
 void StepperController::set_motor_velocity(float rps) {
+    gpio_pin_configure_dt(&work_led, fabs(rps) > 0.05 ? GPIO_OUTPUT_ACTIVE : GPIO_OUTPUT_INACTIVE);
+
     uint32_t motor_steps_hz = abs((int32_t)(rps * (float)SENSOR_STEPS_PER_REVOLUTION));
 
     if (motor_steps_hz == 0) {