added pre init calibration

boards/nucleo_h563zi.overlay

@@ -6,6 +6,12 @@
         stepper0 = &stepper0;
     };
 
+    chosen {
+		pid,counter = &counter5;
+        pid,status = &green_led_1;
+        pid,calibration = &yellow_led_1;
+	};
+
     stepper0: tmc2209_motor {
         compatible = "adi,tmc2209";
 

src/CMakeLists.txt

@@ -2,7 +2,6 @@ cmake_minimum_required(VERSION 3.20.0)
 
 target_include_directories(app PRIVATE ./)
 
-add_subdirectory(./led_controller)
 add_subdirectory(./stepper_controller)
 add_subdirectory(./microros)
 #add_subdirectory(./usb)

src/devices.cpp

@@ -9,17 +9,15 @@ bool verify_are_devices_available(void) {
         devices_ready = false;
     }
 
-    /*
+    if (!device_is_ready(status_led.port)) {
-    if (!device_is_ready(led1.port)) {
+        printk("Status LED not ready\n");
-        printk("LED1 not ready\n");
         devices_ready = false;
     }
 
-    if (!device_is_ready(led2.port)) {
+    if (!device_is_ready(calibration_led.port)) {
-        printk("LED2 not ready\n");
+        printk("Calibration LED not ready\n");
         devices_ready = false;
     }
-    */
 
     if (!device_is_ready(pid_counter)) {
         printk("Counter not ready\n");

src/devices.hpp

@@ -13,8 +13,8 @@
 #endif
 
 static const struct gpio_dt_spec led0 = GPIO_DT_SPEC_GET(DT_ALIAS(led0), gpios);
-//static const struct gpio_dt_spec led1 = GPIO_DT_SPEC_GET(DT_ALIAS(led1), gpios);
+static const struct gpio_dt_spec status_led = GPIO_DT_SPEC_GET(DT_CHOSEN(pid_status), gpios);
-//static const struct gpio_dt_spec led2 = GPIO_DT_SPEC_GET(DT_ALIAS(led2), gpios);
+static const struct gpio_dt_spec calibration_led = GPIO_DT_SPEC_GET(DT_CHOSEN(pid_calibration), gpios);
 
 static const struct device *rotenc0 = DEVICE_DT_GET(DT_ALIAS(rotenc0));
 static const struct device *stepper0 = DEVICE_DT_GET(DT_ALIAS(stepper0));

src/led_controller/CMakeLists.txt

@@ -1,9 +0,0 @@
-cmake_minimum_required(VERSION 3.20.0)
-
-target_include_directories(app PRIVATE ./)
-
-#add_subdirectory()
-
-target_sources(app PRIVATE
-    led_controller.cpp
-)

src/led_controller/led_controller.cpp

@@ -1,98 +0,0 @@
-#include "led_controller.hpp"
-#include "devices.hpp"
-#include "rcl_util.hpp"
-
-#include <zephyr/drivers/gpio.h>
-#include <rcl/rcl.h>
-#include <rclc/rclc.h>
-#include <rclc/executor.h>
-#include <std_msgs/msg/bool.h>
-
-std_msgs__msg__Bool led_msg;
-rcl_node_t LedController::node;
-rcl_subscription_t LedController::led0_subscription;
-/*
-rcl_subscription_t LedController::led1_subscription;
-rcl_subscription_t LedController::led2_subscription;
-*/
-
-void led0_callback(const void *msg){
-    gpio_pin_configure_dt(&led0, ((std_msgs__msg__Bool *)msg)->data ? GPIO_OUTPUT_ACTIVE : GPIO_OUTPUT_INACTIVE);
-}
-
-
-/*
-void led1_callback(const void *msg){
-    gpio_pin_configure_dt(&led1, ((std_msgs__msg__Bool *)msg)->data ? GPIO_OUTPUT_ACTIVE : GPIO_OUTPUT_INACTIVE);
-}
-
-
-void led2_callback(const void *msg){
-    gpio_pin_configure_dt(&led2, ((std_msgs__msg__Bool *)msg)->data ? GPIO_OUTPUT_ACTIVE : GPIO_OUTPUT_INACTIVE);
-}
-*/
-
-
-rcl_ret_t LedController::setup(rclc_support_t* support, rclc_executor_t* executor) {
-    RCL_RETURN_ON_ERROR(rclc_node_init_default(
-        &LedController::node, 
-        "led_controller",
-        "",
-        support));
-
-    RCL_RETURN_ON_ERROR(rclc_subscription_init_default(
-        &LedController::led0_subscription,
-        &LedController::node,
-        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool),
-        "/nucleo/led0"));
-       
-    /*
-    RCL_RETURN_ON_ERROR(rclc_subscription_init_default(
-        &LedController::led1_subscription,
-        &LedController::node,
-        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool),
-        "/nucleo/led1"));
-
-    RCL_RETURN_ON_ERROR(rclc_subscription_init_default(
-        &LedController::led2_subscription,
-        &LedController::node,
-        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool),
-        "/nucleo/led2"));
-    */
-
-    RCL_RETURN_ON_ERROR(rclc_executor_add_subscription(
-        executor,
-        &LedController::led0_subscription,
-        &led_msg,
-        led0_callback,
-        ON_NEW_DATA
-    ));
-
-    /*
-    RCL_RETURN_ON_ERROR(rclc_executor_add_subscription(
-        executor,
-        &LedController::led1_subscription,
-        &led_msg,
-        led1_callback,
-        ON_NEW_DATA
-    ));
-
-    RCL_RETURN_ON_ERROR(rclc_executor_add_subscription(
-        executor,
-        &LedController::led2_subscription,
-        &led_msg,
-        led2_callback,
-        ON_NEW_DATA
-    ));
-    */
-
-    return RCL_RET_OK;
-}
-
-
-void LedController::kill() {
-    rcl_subscription_fini(&LedController::led0_subscription, &LedController::node);
-    //rcl_subscription_fini(&LedController::led1_subscription, &LedController::node);
-    //rcl_subscription_fini(&LedController::led2_subscription, &LedController::node);
-    rcl_node_fini(&LedController::node);
-}

src/led_controller/led_controller.hpp

@@ -1,19 +0,0 @@
-#pragma once
-#include <rclc/executor.h>
-#include <rcl/rcl.h>
-#include <rclc/rclc.h>
-#include <rosidl_runtime_c/message_type_support_struct.h>
-
-class LedController {
-public:
-    static rcl_ret_t setup(rclc_support_t* support, rclc_executor_t* executor);
-    static void kill();
-
-private:
-    static rcl_node_t node;
-    static rcl_subscription_t led0_subscription;
-    /*
-    static rcl_subscription_t led1_subscription;
-    static rcl_subscription_t led2_subscription;
-    */
-};

src/microros/microros_state_machine.cpp

@@ -1,6 +1,5 @@
 #include "microros_state_machine.hpp"
 
-#include "led_controller.hpp"
 #include "stepper_controller.hpp"
 
 #include <zephyr/kernel.h>
@@ -32,7 +31,6 @@ void micro_ros_state_machine() {
             micro_ros_state = ROS_AGENT_CONNECTION_DISCONNECTED;
 
         case ROS_AGENT_CONNECTION_DISCONNECTED:
-            LedController::kill();
             StepperController::kill();
             rclc_executor_fini(&executor);
             rclc_support_fini(&support);
@@ -49,11 +47,6 @@ void micro_ros_state_machine() {
             micro_ros_state = ROS_AGENT_CONNECTION_SETUP;
 
         case ROS_AGENT_CONNECTION_SETUP:
-            if (LedController::setup(&support, &executor) != RCL_RET_OK) {
-                printk("LED Controller cant be initialized!\r\n");
-                micro_ros_state = ROS_AGENT_CONNECTION_ERROR;
-                break;
-            }
             if (StepperController::setup(&support, &executor) != RCL_RET_OK) {
                 printk("Servo Controller cant be initialized!\r\n");
                 micro_ros_state = ROS_AGENT_CONNECTION_ERROR;

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;