added pre init calibration

src/stepper_controller/stepper_controller.cpp

@@ -29,6 +29,8 @@ float StepperController::target_angle = 0.0f;
 float StepperController::current_velocity;
 float StepperController::current_acceleration;
 
+float StepperController::direction_factor = 1.0;
+
 // max accel and velocity
 float StepperController::max_velocity = 2.0f; // rps
 float StepperController::max_accel = 15.0f; // rpss
@@ -92,12 +94,37 @@ void StepperController::init() {
         .ticks = counter_us_to_ticks(pid_counter, PID_LOOP_US), // 2 ms = 500 Hz
         .flags = 0,
     };
+
+    calibrate();
     
     counter_set_channel_alarm(pid_counter, 0, &alarm_cfg);
     counter_start(pid_counter);
+}
+
+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));
 
     sample_angle();
-    last_angle = current_angle;
+    float left_angle = current_angle;
+
+    set_motor_velocity(-0.1);
+    k_sleep(K_MSEC(500));
+
+    sample_angle();
+    float right_angle = current_angle; 
+
+    set_motor_velocity(0.1);
+    k_sleep(K_MSEC(250));
+    
+    if (right_angle >= left_angle) {
+        direction_factor = -1.0;
+    }
+    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)
@@ -160,12 +187,12 @@ void StepperController::sample_angle() {
         return;
     }   
     val.val1 = (val.val1 + ROT_MIDDLE_OFFSET) % 360;
-    float angle = (float)val.val1 - 180.0f + (float)val.val2 / 1000000.0f;
+    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 > 350)
+    if (full_angle - last_angle > 270)
         angle_rotation_count--;
-    if (full_angle - last_angle < -350)
+    if (full_angle - last_angle < -270)
         angle_rotation_count++;
 
     last_angle = current_angle;

src/stepper_controller/stepper_controller.hpp

@@ -9,11 +9,15 @@
 class StepperController {
 public:
     static void init();
+    static void calibrate();
     static rcl_ret_t setup(rclc_support_t* support, rclc_executor_t* executor);
     static void pid();
     static rcl_ret_t publish();
     static void kill();
 
+    // calibration
+    static float direction_factor;
+
     // angles
     static float current_angle;
     static float target_angle;