fixed everything

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,active_led = &green_led_1;
-        pid,calibration = &yellow_led_1;
+        pid,offset_led = &yellow_led_1;
+        pid,calibration_led = &red_led_1;
+        pid,rotenc = &as5600;
+        pid,stepper = &stepper0;
 	};
 
     stepper0: tmc2209_motor {

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

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;

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_active_led), gpios);
-static const struct gpio_dt_spec status_led = GPIO_DT_SPEC_GET(DT_CHOSEN(pid_status), 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), 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 *rotenc0 = DEVICE_DT_GET(DT_CHOSEN(pid_rotenc));
-static const struct device *stepper0 = DEVICE_DT_GET(DT_ALIAS(stepper0));
+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);

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,

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);
+    set_motor_velocity(0.02);
-    k_sleep(K_MSEC(250));
+    k_sleep(K_MSEC(1000));
 
     sample_angle();
     float left_angle = current_angle;
 
-    set_motor_velocity(-0.1);
+    set_motor_velocity(-0.02);
-    k_sleep(K_MSEC(500));
+    k_sleep(K_MSEC(2000));
 
     sample_angle();
     float right_angle = current_angle; 
 
-    set_motor_velocity(0.1);
+    set_motor_velocity(0.02);
-    k_sleep(K_MSEC(250));
+    k_sleep(K_MSEC(1000));
 
-    if (right_angle >= left_angle) {
+    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) {