diff --git a/boards/nucleo_h563zi.overlay b/boards/nucleo_h563zi.overlay index eb44073..77af479 100644 --- a/boards/nucleo_h563zi.overlay +++ b/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"; diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 0ba4a61..e838116 100644 --- a/src/CMakeLists.txt +++ b/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) diff --git a/src/devices.cpp b/src/devices.cpp index 2b48a37..bb95100 100644 --- a/src/devices.cpp +++ b/src/devices.cpp @@ -9,17 +9,15 @@ bool verify_are_devices_available(void) { devices_ready = false; } - /* - if (!device_is_ready(led1.port)) { - printk("LED1 not ready\n"); + if (!device_is_ready(status_led.port)) { + printk("Status LED not ready\n"); devices_ready = false; } - if (!device_is_ready(led2.port)) { - printk("LED2 not ready\n"); + if (!device_is_ready(calibration_led.port)) { + printk("Calibration LED not ready\n"); devices_ready = false; } - */ if (!device_is_ready(pid_counter)) { printk("Counter not ready\n"); diff --git a/src/devices.hpp b/src/devices.hpp index 45fcddb..870cbb6 100644 --- a/src/devices.hpp +++ b/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 led2 = GPIO_DT_SPEC_GET(DT_ALIAS(led2), 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 device *rotenc0 = DEVICE_DT_GET(DT_ALIAS(rotenc0)); static const struct device *stepper0 = DEVICE_DT_GET(DT_ALIAS(stepper0)); diff --git a/src/led_controller/CMakeLists.txt b/src/led_controller/CMakeLists.txt deleted file mode 100644 index 7802d36..0000000 --- a/src/led_controller/CMakeLists.txt +++ /dev/null @@ -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 -) \ No newline at end of file diff --git a/src/led_controller/led_controller.cpp b/src/led_controller/led_controller.cpp deleted file mode 100644 index 514aa97..0000000 --- a/src/led_controller/led_controller.cpp +++ /dev/null @@ -1,98 +0,0 @@ -#include "led_controller.hpp" -#include "devices.hpp" -#include "rcl_util.hpp" - -#include -#include -#include -#include -#include - -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); -} \ No newline at end of file diff --git a/src/led_controller/led_controller.hpp b/src/led_controller/led_controller.hpp deleted file mode 100644 index 8529304..0000000 --- a/src/led_controller/led_controller.hpp +++ /dev/null @@ -1,19 +0,0 @@ -#pragma once -#include -#include -#include -#include - -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; - */ -}; diff --git a/src/microros/microros_state_machine.cpp b/src/microros/microros_state_machine.cpp index 6ddb2f2..d15586f 100644 --- a/src/microros/microros_state_machine.cpp +++ b/src/microros/microros_state_machine.cpp @@ -1,6 +1,5 @@ #include "microros_state_machine.hpp" -#include "led_controller.hpp" #include "stepper_controller.hpp" #include @@ -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; diff --git a/src/stepper_controller/stepper_controller.cpp b/src/stepper_controller/stepper_controller.cpp index 7688f1c..fbd3f36 100644 --- a/src/stepper_controller/stepper_controller.cpp +++ b/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; diff --git a/src/stepper_controller/stepper_controller.hpp b/src/stepper_controller/stepper_controller.hpp index 34453b3..e07436d 100644 --- a/src/stepper_controller/stepper_controller.hpp +++ b/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;