From a4c2d82c4c8b1a283624995087bf9ade0c731c4f Mon Sep 17 00:00:00 2001 From: Timo Date: Sun, 30 Nov 2025 16:50:46 +0100 Subject: [PATCH] moved from servo to stepper --- boards/nucleo_h563zi.overlay | 1 + prj.conf | 5 +- src/CMakeLists.txt | 2 +- src/main.cpp | 5 +- src/microros/microros_state_machine.cpp | 10 +- src/servo_controller/servo_controller.cpp | 202 ------------- src/servo_controller/servo_controller.hpp | 42 --- .../CMakeLists.txt | 2 +- src/stepper_controller/stepper_controller.cpp | 278 ++++++++++++++++++ src/stepper_controller/stepper_controller.hpp | 57 ++++ 10 files changed, 350 insertions(+), 254 deletions(-) delete mode 100644 src/servo_controller/servo_controller.cpp delete mode 100644 src/servo_controller/servo_controller.hpp rename src/{servo_controller => stepper_controller}/CMakeLists.txt (83%) create mode 100644 src/stepper_controller/stepper_controller.cpp create mode 100644 src/stepper_controller/stepper_controller.hpp diff --git a/boards/nucleo_h563zi.overlay b/boards/nucleo_h563zi.overlay index 50dda36..9ba1091 100644 --- a/boards/nucleo_h563zi.overlay +++ b/boards/nucleo_h563zi.overlay @@ -86,6 +86,7 @@ status = "okay"; pinctrl-0 = <&i2c2_scl_pf1 &i2c2_sda_pf0>; pinctrl-names = "default"; + clock-frequency = ; as5600: as5600@36 { compatible = "ams,as5600"; diff --git a/prj.conf b/prj.conf index c236dce..acf4e78 100644 --- a/prj.conf +++ b/prj.conf @@ -57,5 +57,8 @@ CONFIG_STEPPER_ADI_TMC2209=y CONFIG_STEPPER_ADI_TMC=y CONFIG_STEPPER_ADI_TMC_UART=y -# debug +# builld +CONFIG_NO_OPTIMIZATIONS=n +CONFIG_SIZE_OPTIMIZATIONS=n +CONFIG_SPEED_OPTIMIZATIONS=y CONFIG_DEBUG_OPTIMIZATIONS=n \ No newline at end of file diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index fe2e221..5c88082 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.20.0) target_include_directories(app PRIVATE ./) add_subdirectory(./led_controller) -add_subdirectory(./servo_controller) +add_subdirectory(./stepper_controller) add_subdirectory(./microros) add_subdirectory(./usb) diff --git a/src/main.cpp b/src/main.cpp index 8e8c8c7..55bed2d 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -11,10 +11,9 @@ #include "devices.hpp" #include "usb_cdc_acm.hpp" -#include "servo_controller.hpp" +#include "stepper_controller.hpp" #include "microros_state_machine.hpp" - int main(void) { if (!verify_are_devices_available()) { @@ -22,6 +21,8 @@ int main(void) for (;;){} } + StepperController::init(); + usb_init(); rmw_uros_set_custom_transport( diff --git a/src/microros/microros_state_machine.cpp b/src/microros/microros_state_machine.cpp index f130630..6ddb2f2 100644 --- a/src/microros/microros_state_machine.cpp +++ b/src/microros/microros_state_machine.cpp @@ -1,7 +1,7 @@ #include "microros_state_machine.hpp" #include "led_controller.hpp" -#include "servo_controller.hpp" +#include "stepper_controller.hpp" #include #include @@ -33,7 +33,7 @@ void micro_ros_state_machine() { case ROS_AGENT_CONNECTION_DISCONNECTED: LedController::kill(); - ServoController::kill(); + StepperController::kill(); rclc_executor_fini(&executor); rclc_support_fini(&support); if (rclc_support_init(&support, 0, NULL, &allocator) != RCL_RET_OK) { @@ -54,7 +54,7 @@ void micro_ros_state_machine() { micro_ros_state = ROS_AGENT_CONNECTION_ERROR; break; } - if (ServoController::setup(&support, &executor) != RCL_RET_OK) { + if (StepperController::setup(&support, &executor) != RCL_RET_OK) { printk("Servo Controller cant be initialized!\r\n"); micro_ros_state = ROS_AGENT_CONNECTION_ERROR; break; @@ -77,8 +77,8 @@ void micro_ros_state_machine() { printk("micro ros spin error!\r\n"); break; } - if (k_uptime_get() % 20 == 0) { - if (ServoController::publish() != RCL_RET_OK) { + if (k_uptime_get() % 9 == 0) { + if (StepperController::publish() != RCL_RET_OK) { micro_ros_state = ROS_AGENT_CONNECTION_UNKNOWN; printk("micro ros servo publish error!\r\n"); break; diff --git a/src/servo_controller/servo_controller.cpp b/src/servo_controller/servo_controller.cpp deleted file mode 100644 index e8b656d..0000000 --- a/src/servo_controller/servo_controller.cpp +++ /dev/null @@ -1,202 +0,0 @@ -#include "servo_controller.hpp" -#include "devices.hpp" -#include "rcl/types.h" -#include "rcl_util.hpp" -#include "zephyr/sys/printk.h" - -#include -#include -#include -#include -#include -#include - - -const uint32_t min_pulse_ns = 500000; -const uint32_t max_pulse_ns = 2500000; - -rcl_node_t ServoController::node; -rcl_subscription_t ServoController::servo0_sub_pos; -rcl_subscription_t ServoController::servo0_sub_maxv; -rcl_subscription_t ServoController::servo0_sub_maxa; -rcl_publisher_t ServoController::servo0_publisher; -float ServoController::current_angle = 90.0f; -float ServoController::target_angle = 90.0f; -uint32_t ServoController::last_step = 0; -float ServoController::current_velocity = 0.0f; -float ServoController::last_error = 0.0f; -float ServoController::integral = 0.0f; -float ServoController::max_velocity = 20.0f; -float ServoController::max_accel = 35.0f; - -struct counter_alarm_cfg alarm_cfg; - -std_msgs__msg__Float32 servo_target_msg; - -void servo0_set_target_callback(const void *msg){ - ServoController::target_angle = (float)((std_msgs__msg__Float32 *)msg)->data; -} - -void servo0_set_maxv_callback(const void *msg){ - ServoController::max_velocity = (float)((std_msgs__msg__Float32 *)msg)->data; -} - -void servo0_set_maxa_callback(const void *msg){ - ServoController::max_accel = (float)((std_msgs__msg__Float32 *)msg)->data; -} - -void servo_pid_work_cb(struct k_work *work) -{ - ServoController::servo_pid(); -} -K_WORK_DEFINE(servo_pid_work, servo_pid_work_cb); - - -void counter_cb(const struct device *dev, uint8_t chan_id, uint32_t ticks, void *user_data) -{ - k_work_submit(&servo_pid_work); - counter_set_channel_alarm(dev, chan_id, &alarm_cfg); -} - - -rcl_ret_t ServoController::setup(rclc_support_t* support, rclc_executor_t* executor) -{ - uint32_t speed_hz = 6400; // desired step frequency in Hz - uint32_t interval_ns = 1000000000U / speed_hz; // ns per step - stepper_set_microstep_interval(stepper0, interval_ns); - stepper_enable(stepper0); - stepper_run(stepper0, STEPPER_DIRECTION_POSITIVE); - - RCL_RETURN_ON_ERROR(rclc_node_init_default( - &ServoController::node, - "servo_controller", - "", - support)); - - RCL_RETURN_ON_ERROR(rclc_publisher_init_default( - &ServoController::servo0_publisher, - &ServoController::node, - ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32), - "/nucleo/servo0/position" - )); - - RCL_RETURN_ON_ERROR(rclc_subscription_init_default( - &ServoController::servo0_sub_pos, - &ServoController::node, - ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32), - "/nucleo/servo0/target")); - - RCL_RETURN_ON_ERROR(rclc_subscription_init_default( - &ServoController::servo0_sub_maxv, - &ServoController::node, - ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32), - "/nucleo/servo0/max_v")); - - RCL_RETURN_ON_ERROR(rclc_subscription_init_default( - &ServoController::servo0_sub_maxa, - &ServoController::node, - ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32), - "/nucleo/servo0/max_a")); - - RCL_RETURN_ON_ERROR(rclc_executor_add_subscription( - executor, - &ServoController::servo0_sub_pos, - &servo_target_msg, - servo0_set_target_callback, - ON_NEW_DATA - )); - - RCL_RETURN_ON_ERROR(rclc_executor_add_subscription( - executor, - &ServoController::servo0_sub_maxv, - &servo_target_msg, - servo0_set_maxv_callback, - ON_NEW_DATA - )); - - RCL_RETURN_ON_ERROR(rclc_executor_add_subscription( - executor, - &ServoController::servo0_sub_maxa, - &servo_target_msg, - servo0_set_maxa_callback, - ON_NEW_DATA - )); - - alarm_cfg = { - .callback = counter_cb, - .ticks = counter_us_to_ticks(counter5, 2000), // 2 ms = 500 Hz - .flags = 0, - }; - - counter_set_channel_alarm(counter5, 0, &alarm_cfg); - counter_start(counter5); - - return RCL_RET_OK; -} - - -void ServoController::servo_pid() { - uint32_t now = k_cycle_get_32(); - - // wait for sync - if (last_step == 0) { - last_step = now; - return; - } - - // calc err & dt - float error = target_angle - current_angle; - float dt = (float)(now - last_step) / (float)CONFIG_SYS_CLOCK_HW_CYCLES_PER_SEC; - - // pid - float derivative = (error - last_error) / dt; - integral += error * dt; - float pid_vel = kp * error + ki * integral + kd * derivative; - - // clamp velocity - float target_velocity = fmaxf(-max_velocity, fminf(max_velocity, pid_vel)); - - // clamp acceleration - float max_dv = max_accel * dt; - current_velocity += fmaxf(-max_dv, fminf(max_dv, target_velocity - current_velocity)); - - // Update angle - current_angle += current_velocity * dt; - current_angle = fmaxf(0.0f, fminf(180.0f, current_angle)); - - last_error = error; - last_step = now; - - uint32_t pulse_ns = min_pulse_ns + (current_angle / 180.0f) * (max_pulse_ns - min_pulse_ns); - int ret = pwm_set_pulse_dt(&servo0, pulse_ns); -} - -rcl_ret_t ServoController::publish() { - std_msgs__msg__Float32 msg; - std_msgs__msg__Float32__init(&msg); - struct sensor_value val; - - if (sensor_sample_fetch(rotenc0)) { - printk("failed to fetch sensor value\n"); - return RCL_RET_OK; - } - - if (sensor_channel_get(rotenc0, SENSOR_CHAN_ROTATION, &val)) { - printk("failed to get sensor value\n"); - return RCL_RET_OK; - } - printk("angle: %d.%d\n", val.val1, val.val2); - - //msg.data = ServoController::current_angle; - msg.data = (float)val.val1 + (float)val.val1 / 1000000; - - return rcl_publish(&ServoController::servo0_publisher, &msg, NULL); -} - -void ServoController::kill() { - rcl_publisher_fini(&ServoController::servo0_publisher, &ServoController::node); - rcl_subscription_fini(&ServoController::servo0_sub_pos, &ServoController::node); - rcl_subscription_fini(&ServoController::servo0_sub_maxv, &ServoController::node); - rcl_subscription_fini(&ServoController::servo0_sub_maxa, &ServoController::node); - rcl_node_fini(&ServoController::node); -} \ No newline at end of file diff --git a/src/servo_controller/servo_controller.hpp b/src/servo_controller/servo_controller.hpp deleted file mode 100644 index 228b6d9..0000000 --- a/src/servo_controller/servo_controller.hpp +++ /dev/null @@ -1,42 +0,0 @@ -#pragma once -#include -#include -#include -#include -#include - -class ServoController { -public: - static rcl_ret_t setup(rclc_support_t* support, rclc_executor_t* executor); - static void servo_pid(); - static rcl_ret_t publish(); - static void kill(); - - // angles - static float current_angle; - static float target_angle; - - // Limits - static float max_velocity; - static float max_accel; - -private: - // ros - static rcl_node_t node; - static rcl_subscription_t servo0_sub_pos; - static rcl_subscription_t servo0_sub_maxv; - static rcl_subscription_t servo0_sub_maxa; - static rcl_publisher_t servo0_publisher; - - // pid states - static uint32_t last_step; - static float current_velocity; - static float last_error; - static float integral; - static float dt; - - // pid coefs - static constexpr float kp = 2.0f; - static constexpr float ki = 0.0f; - static constexpr float kd = 0.05f; -}; diff --git a/src/servo_controller/CMakeLists.txt b/src/stepper_controller/CMakeLists.txt similarity index 83% rename from src/servo_controller/CMakeLists.txt rename to src/stepper_controller/CMakeLists.txt index 0d7278e..db509fa 100644 --- a/src/servo_controller/CMakeLists.txt +++ b/src/stepper_controller/CMakeLists.txt @@ -5,5 +5,5 @@ target_include_directories(app PRIVATE ./) #add_subdirectory() target_sources(app PRIVATE - servo_controller.cpp + stepper_controller.cpp ) \ No newline at end of file diff --git a/src/stepper_controller/stepper_controller.cpp b/src/stepper_controller/stepper_controller.cpp new file mode 100644 index 0000000..1277ef8 --- /dev/null +++ b/src/stepper_controller/stepper_controller.cpp @@ -0,0 +1,278 @@ +#include "stepper_controller.hpp" +#include "devices.hpp" +#include "rcl/types.h" +#include "rcl_util.hpp" +#include "zephyr/sys/printk.h" + +#include +#include +#include +#include +#include +#include +#include + +#define PID_LOOP_US 500 +#define PID_LOOP_S_F ((float)(PID_LOOP_US) / 1000000) +#define MOTOR_GEAR_RATIO 4 +#define MOTOR_MICROSTEP 64 +#define MOTOR_STEPS_REVOLUTION 200 +#define SENSOR_STEPS_PER_REVOLUTION (MOTOR_STEPS_REVOLUTION * MOTOR_MICROSTEP * MOTOR_GEAR_RATIO) +#define ROT_MIDDLE_OFFSET -5 + +rcl_node_t StepperController::node; + +// target pos +rcl_subscription_t StepperController::stepper0_sub_pos; +float StepperController::current_angle; +float StepperController::target_angle = 0.0f; +float StepperController::current_velocity; +float StepperController::current_acceleration; + +// max accel and velocity +float StepperController::max_velocity = 2.0f; // rps +float StepperController::max_accel = 15.0f; // rpss + +// publisher +rcl_publisher_t StepperController::stepper0_publisher_x; +rcl_publisher_t StepperController::stepper0_publisher_v; +rcl_publisher_t StepperController::stepper0_publisher_a; + +// pid x +float StepperController::last_angle = 0; +float StepperController::integral_x = 0.0f; + +// pid v +float StepperController::motor_velocity = 0; +float StepperController::integral_v = 0.0f; + + +struct counter_alarm_cfg alarm_cfg; + +std_msgs__msg__Float32 stepper_target_msg; + +void stepper0_set_target_callback(const void *msg){ + float angle = (float)((std_msgs__msg__Float32 *)msg)->data; + if (angle < -80.0f) angle = -80.0f; + if (angle > 80.0f) angle = 80.0f; + StepperController::target_angle = angle; +} + +void stepper0_set_maxv_callback(const void *msg){ + StepperController::max_velocity = (float)((std_msgs__msg__Float32 *)msg)->data; +} + +void stepper0_set_maxa_callback(const void *msg){ + StepperController::max_accel = (float)((std_msgs__msg__Float32 *)msg)->data; +} + +void stepper_pid_work_cb(struct k_work *work) +{ + StepperController::pid(); +} +K_WORK_DEFINE(stepper_pid_work, stepper_pid_work_cb); + + +void counter_cb(const struct device *dev, uint8_t chan_id, uint32_t ticks, void *user_data) +{ + k_work_submit(&stepper_pid_work); + counter_set_channel_alarm(dev, chan_id, &alarm_cfg); +} + + +void StepperController::init() { + uint32_t speed_hz = 1; // desired step frequency in Hz + uint32_t interval_ns = 1000000000U / speed_hz; // ns per step + stepper_set_microstep_interval(stepper0, interval_ns); + stepper_enable(stepper0); + stepper_stop(stepper0); + + alarm_cfg = { + .callback = counter_cb, + .ticks = counter_us_to_ticks(counter5, PID_LOOP_US), // 2 ms = 500 Hz + .flags = 0, + }; + + counter_set_channel_alarm(counter5, 0, &alarm_cfg); + counter_start(counter5); + + sample_angle(); + last_angle = current_angle; +} + +rcl_ret_t StepperController::setup(rclc_support_t* support, rclc_executor_t* executor) +{ + RCL_RETURN_ON_ERROR(rclc_node_init_default( + &StepperController::node, + "stepper_controller", + "", + support)); + + RCL_RETURN_ON_ERROR(rclc_publisher_init_default( + &StepperController::stepper0_publisher_x, + &StepperController::node, + ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32), + "/nucleo/stepper0/position" + )); + + RCL_RETURN_ON_ERROR(rclc_publisher_init_default( + &StepperController::stepper0_publisher_v, + &StepperController::node, + ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32), + "/nucleo/stepper0/velocity" + )); + + RCL_RETURN_ON_ERROR(rclc_publisher_init_default( + &StepperController::stepper0_publisher_a, + &StepperController::node, + ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32), + "/nucleo/stepper0/acceleration" + )); + + RCL_RETURN_ON_ERROR(rclc_subscription_init_default( + &StepperController::stepper0_sub_pos, + &StepperController::node, + ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32), + "/nucleo/stepper0/target")); + + + RCL_RETURN_ON_ERROR(rclc_executor_add_subscription( + executor, + &StepperController::stepper0_sub_pos, + &stepper_target_msg, + stepper0_set_target_callback, + ON_NEW_DATA + )); + + return RCL_RET_OK; +} + +void StepperController::sample_angle() { + static int32_t angle_rotation_count = 0; + struct sensor_value val; + if (sensor_sample_fetch(rotenc0)) { + printk("failed to fetch sensor value\n"); + return; + } + + if (sensor_channel_get(rotenc0, SENSOR_CHAN_ROTATION, &val)) { + printk("failed to get sensor value\n"); + return; + } + val.val1 = (val.val1 + ROT_MIDDLE_OFFSET) % 360; + float angle = (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) + angle_rotation_count--; + if (full_angle - last_angle < -350) + angle_rotation_count++; + + last_angle = current_angle; + StepperController::current_angle = angle + 360.0f * (float)angle_rotation_count; + + float measured_velocity = (current_angle - last_angle) / (360.0f * PID_LOOP_S_F); + + // --- Static filter values stay persistent across calls --- + static float x = 0.0f; // estimated value + static float P = 0.0f; // estimate uncertainty + + // --- Tunable noise constants --- + static const float Q = 0.0003f; // process noise + static const float R = 0.122f; // measurement noise + + // ----- Prediction ----- + P = P + Q; + + // ----- Update ----- + float K = P / (P + R); + x = x + K * (measured_velocity - x); + P = (1 - K) * P; + + StepperController::current_velocity = x; +} + +float StepperController::pid_x(float e) { + float d_integral_x = e * PID_LOOP_S_F; + float u = e * kp_x + (integral_x + d_integral_x) * ki_x; + + if (u < -max_velocity) { + u = -max_velocity; + } + else if (u > max_velocity) { + u = max_velocity; + } + else { + integral_x += d_integral_x; + } + return u; +} + +float StepperController::pid_v(float e) { + float d_integral_v = e * PID_LOOP_S_F; + float u = e * kp_v + integral_v * ki_v; + + if (u < -max_accel) { + u = -max_accel; + } + else if (u > max_accel) { + u = max_accel; + } + else { + integral_v += d_integral_v; + } + return u; +} + +void StepperController::pid() { + sample_angle(); + + float e_x = target_angle - current_angle; + float target_velocity = pid_x(e_x); + float e_v = target_velocity - current_velocity; + current_acceleration = pid_v(e_v); + motor_velocity += (current_acceleration * PID_LOOP_S_F); + + set_motor_velocity(motor_velocity); +} + +void StepperController::set_motor_velocity(float rps) { + uint32_t motor_steps_hz = abs((int32_t)(rps * (float)SENSOR_STEPS_PER_REVOLUTION)); + + if (motor_steps_hz == 0) { + stepper_stop(stepper0); + } + else { + uint64_t microstep_interval_ns = 1000000000 / motor_steps_hz; + stepper_set_microstep_interval(stepper0, microstep_interval_ns); + stepper_run(stepper0, rps > 0 ? STEPPER_DIRECTION_POSITIVE : STEPPER_DIRECTION_NEGATIVE); + } +} + +rcl_ret_t StepperController::publish() { + std_msgs__msg__Float32 msg_x; + std_msgs__msg__Float32 msg_v; + std_msgs__msg__Float32 msg_a; + std_msgs__msg__Float32__init(&msg_x); + std_msgs__msg__Float32__init(&msg_v); + std_msgs__msg__Float32__init(&msg_a); + + msg_x.data = current_angle; + msg_v.data = current_velocity; + msg_a.data = current_acceleration; + + rcl_ret_t err; + err = rcl_publish(&stepper0_publisher_x, &msg_x, NULL); + if (err) return err; + err = rcl_publish(&stepper0_publisher_v, &msg_v, NULL); + if (err) return err; + return rcl_publish(&stepper0_publisher_a, &msg_a, NULL); +} + +void StepperController::kill() { + rcl_publisher_fini(&stepper0_publisher_x, &node); + rcl_publisher_fini(&stepper0_publisher_v, &node); + rcl_publisher_fini(&stepper0_publisher_a, &node); + rcl_subscription_fini(&stepper0_sub_pos, &node); + rcl_node_fini(&node); +} \ No newline at end of file diff --git a/src/stepper_controller/stepper_controller.hpp b/src/stepper_controller/stepper_controller.hpp new file mode 100644 index 0000000..34453b3 --- /dev/null +++ b/src/stepper_controller/stepper_controller.hpp @@ -0,0 +1,57 @@ +#pragma once +#include +#include +#include +#include +#include +#include + +class StepperController { +public: + static void init(); + static rcl_ret_t setup(rclc_support_t* support, rclc_executor_t* executor); + static void pid(); + static rcl_ret_t publish(); + static void kill(); + + // angles + static float current_angle; + static float target_angle; + + // velocity / accel + static float current_velocity; + static float current_acceleration; + + // Limits + static float max_velocity; + static float max_accel; + +private: + // ros + static rcl_node_t node; + static rcl_subscription_t stepper0_sub_pos; + static rcl_publisher_t stepper0_publisher_x; + static rcl_publisher_t stepper0_publisher_v; + static rcl_publisher_t stepper0_publisher_a; + + // pid x states + static float last_angle; + static float integral_x; + + // pid v states + static float motor_velocity; + static float integral_v; + + // pid x coefs + static constexpr float kp_x = 0.012f; + static constexpr float ki_x = 0.0001f; + + // pid v coefs + static constexpr float kp_v = 15.0f; + static constexpr float ki_v = 0.004f; + + static void sample_angle(); + static void set_motor_velocity(float rps); + static float pid_x(float e); + static float pid_v(float e); +};