From e9fe3496ac8511593eeda99b53d127ab21113da7 Mon Sep 17 00:00:00 2001 From: Timo Date: Sun, 11 Jan 2026 12:03:47 +0100 Subject: [PATCH] added retained calibration --- .vscode/launch.json | 2 +- boards/nucleo_h563zi.overlay | 19 +++++++--- prj.conf | 8 +++- src/devices.cpp | 12 +++--- src/devices.hpp | 6 +-- src/main.cpp | 2 + src/microros/microros_state_machine.cpp | 17 +++++++-- src/stepper_controller/stepper_controller.cpp | 37 ++++++++++++------- 8 files changed, 69 insertions(+), 34 deletions(-) diff --git a/.vscode/launch.json b/.vscode/launch.json index 83f2146..d575086 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -9,7 +9,7 @@ "showDevDebugOutput": "none", "servertype": "external", "gdbTarget": "localhost:3333", - "targetId": "nrf54l", + "targetId": "stm32h5", } ] } \ No newline at end of file diff --git a/boards/nucleo_h563zi.overlay b/boards/nucleo_h563zi.overlay index f5950c3..bec038f 100644 --- a/boards/nucleo_h563zi.overlay +++ b/boards/nucleo_h563zi.overlay @@ -4,10 +4,10 @@ }; chosen { + uros,status_led= &green_led_1; + uros,error_led = &red_led_1; pid,counter = &counter5; - pid,active_led = &green_led_1; pid,offset_led = &yellow_led_1; - pid,calibration_led = &red_led_1; pid,rotenc = &as5600; pid,stepper = &stepper0; }; @@ -39,6 +39,15 @@ status = "okay"; }; +&timers5 { + status = "okay"; + st,prescaler = <7>; + + counter5: counter { + status = "okay"; + }; +}; + &timers6 { status = "okay"; st,prescaler = <25>; @@ -48,11 +57,11 @@ }; }; -&timers5 { +&timers7 { status = "okay"; - st,prescaler = <7>; + st,prescaler = <25>; - counter5: counter { + counter7: counter { status = "okay"; }; }; diff --git a/prj.conf b/prj.conf index 1cfa748..10f6fc9 100644 --- a/prj.conf +++ b/prj.conf @@ -35,8 +35,12 @@ CONFIG_STEPPER=y CONFIG_STEPPER_ADI_TMC2209=y CONFIG_STEPPER_ADI_TMC=y +# reboot +CONFIG_REBOOT=y + # builld CONFIG_NO_OPTIMIZATIONS=n CONFIG_SIZE_OPTIMIZATIONS=n -CONFIG_SPEED_OPTIMIZATIONS=y -CONFIG_DEBUG_OPTIMIZATIONS=n \ No newline at end of file +CONFIG_SPEED_OPTIMIZATIONS=n +CONFIG_DEBUG_OPTIMIZATIONS=y +CONFIG_DEBUG_THREAD_INFO=y \ No newline at end of file diff --git a/src/devices.cpp b/src/devices.cpp index b89388e..9efa470 100644 --- a/src/devices.cpp +++ b/src/devices.cpp @@ -4,19 +4,19 @@ bool verify_are_devices_available(void) { bool devices_ready = true; - if (!device_is_ready(status_led.port)) { - printk("Status LED not ready\n"); + if (!device_is_ready(uros_connection_status_led.port)) { + printk("Microros connection status LED not ready\n"); devices_ready = false; } - if (!device_is_ready(work_led.port)) { - printk("LED0 not ready\n"); + if (!device_is_ready(pid_offset_led.port)) { + printk("PID offset LED not ready\n"); devices_ready = false; } - if (!device_is_ready(calibration_led.port)) { - printk("Calibration LED not ready\n"); + if (!device_is_ready(uros_error_led.port)) { + printk("Microros error LED not ready\n"); devices_ready = false; } diff --git a/src/devices.hpp b/src/devices.hpp index 18f2ba8..6938f57 100644 --- a/src/devices.hpp +++ b/src/devices.hpp @@ -12,9 +12,9 @@ #include #endif -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 gpio_dt_spec uros_connection_status_led = GPIO_DT_SPEC_GET(DT_CHOSEN(uros_status_led), gpios); +static const struct gpio_dt_spec pid_offset_led = GPIO_DT_SPEC_GET(DT_CHOSEN(pid_offset_led), gpios); +static const struct gpio_dt_spec uros_error_led = GPIO_DT_SPEC_GET(DT_CHOSEN(uros_error_led), gpios); static const struct device *rotenc0 = DEVICE_DT_GET(DT_CHOSEN(pid_rotenc)); static const struct device *stepper0 = DEVICE_DT_GET(DT_CHOSEN(pid_stepper)); diff --git a/src/main.cpp b/src/main.cpp index 55bed2d..37e218a 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -34,6 +34,8 @@ int main(void) zephyr_transport_read ); + gpio_pin_configure_dt(&uros_connection_status_led, GPIO_OUTPUT_INACTIVE); + while (1) { micro_ros_state_machine(); } diff --git a/src/microros/microros_state_machine.cpp b/src/microros/microros_state_machine.cpp index d15586f..4fff71b 100644 --- a/src/microros/microros_state_machine.cpp +++ b/src/microros/microros_state_machine.cpp @@ -1,8 +1,10 @@ #include "microros_state_machine.hpp" +#include "devices.hpp" #include "stepper_controller.hpp" #include +#include #include #include #include @@ -26,6 +28,7 @@ void micro_ros_state_machine() { static rclc_executor_t executor; switch (micro_ros_state) { case ROS_AGENT_CONNECTION_UNINITIALIZED: + gpio_pin_configure_dt(&uros_connection_status_led, GPIO_OUTPUT_INACTIVE); allocator = rcl_get_default_allocator(); printk("allocator initialized!\r\n"); micro_ros_state = ROS_AGENT_CONNECTION_DISCONNECTED; @@ -34,9 +37,10 @@ void micro_ros_state_machine() { StepperController::kill(); rclc_executor_fini(&executor); rclc_support_fini(&support); + if (rclc_support_init(&support, 0, NULL, &allocator) != RCL_RET_OK) { printk("Can't init rcl support!\r\n"); - k_msleep(1000); + k_msleep(200); break; } if (rclc_executor_init(&executor, &support.context, 8, &allocator) != RCL_RET_OK){ @@ -55,12 +59,14 @@ void micro_ros_state_machine() { micro_ros_state = ROS_AGENT_CONNECTION_CONNECTED; case ROS_AGENT_CONNECTION_UNKNOWN: - if (rmw_uros_ping_agent(150, 3) != RCL_RET_OK) { - micro_ros_state = ROS_AGENT_CONNECTION_DISCONNECTED; + if (rmw_uros_ping_agent(100, 1) != RCL_RET_OK) { + micro_ros_state = ROS_AGENT_CONNECTION_ERROR; + gpio_pin_configure_dt(&uros_connection_status_led, GPIO_OUTPUT_INACTIVE); printk("micro ros agent not reachable!\r\n"); break; } micro_ros_state = ROS_AGENT_CONNECTION_CONNECTED; + gpio_pin_configure_dt(&uros_connection_status_led, GPIO_OUTPUT_ACTIVE); printk("micro ros agent reachable!\r\n"); rmw_uros_sync_session(100); @@ -83,5 +89,10 @@ void micro_ros_state_machine() { case ROS_AGENT_CONNECTION_ERROR: default: micro_ros_state = ROS_AGENT_CONNECTION_ERROR; + gpio_pin_configure_dt(&uros_error_led, GPIO_OUTPUT_ACTIVE); + + k_sleep(K_MSEC(3000)); + + sys_reboot(SYS_REBOOT_WARM); } } diff --git a/src/stepper_controller/stepper_controller.cpp b/src/stepper_controller/stepper_controller.cpp index 1227c33..fd7fa22 100644 --- a/src/stepper_controller/stepper_controller.cpp +++ b/src/stepper_controller/stepper_controller.cpp @@ -22,6 +22,8 @@ rcl_node_t StepperController::node; +__attribute__((section(".noinit"))) float retained_calibration = 0.0f; + // target pos rcl_subscription_t StepperController::stepper0_sub_pos; float StepperController::current_angle; @@ -102,38 +104,45 @@ 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.02); - k_sleep(K_MSEC(1000)); + printk("Calibrating stepper motor ...\n"); + + if (fabs(retained_calibration) == 1.0) { // NaN check + direction_factor = retained_calibration; + printk("Cached calibration -> %c\n" , direction_factor > 0 ? '+' : '-'); + return; + } + + + set_motor_velocity(0.05); + k_sleep(K_MSEC(500)); sample_angle(); float left_angle = current_angle; - set_motor_velocity(-0.02); - k_sleep(K_MSEC(2000)); + set_motor_velocity(-0.05); + k_sleep(K_MSEC(1000)); sample_angle(); float right_angle = current_angle; - set_motor_velocity(0.02); - k_sleep(K_MSEC(1000)); + set_motor_velocity(0.05); + k_sleep(K_MSEC(500)); if (right_angle < left_angle) { direction_factor = -1.0; } + + stepper_stop(stepper0); - printk("Servo direction calibration:\n"); + printk("Stepper 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)); + retained_calibration = direction_factor; - gpio_pin_configure_dt(&status_led, GPIO_OUTPUT_ACTIVE); - gpio_pin_configure_dt(&calibration_led, GPIO_OUTPUT_INACTIVE); + k_sleep(K_MSEC(100)); } rcl_ret_t StepperController::setup(rclc_support_t* support, rclc_executor_t* executor) @@ -273,7 +282,7 @@ 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); + gpio_pin_configure_dt(&pid_offset_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));