moved from servo to stepper

boards/nucleo_h563zi.overlay

@@ -86,6 +86,7 @@
     status = "okay";
     pinctrl-0 = <&i2c2_scl_pf1 &i2c2_sda_pf0>;
 	pinctrl-names = "default";
+    clock-frequency = <I2C_BITRATE_FAST>;
 
     as5600: as5600@36 {
         compatible = "ams,as5600";

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

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)
 

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(

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 <zephyr/kernel.h>
 #include <rcl/rcl.h>
@@ -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 (k_uptime_get() % 9 == 0) {
-                if (ServoController::publish() != RCL_RET_OK) {
+                if (StepperController::publish() != RCL_RET_OK) {
                     micro_ros_state = ROS_AGENT_CONNECTION_UNKNOWN;
                     printk("micro ros servo publish error!\r\n");
                     break;

src/servo_controller/servo_controller.cpp

@@ -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 <std_msgs/msg/float32.h>
-#include <zephyr/drivers/counter.h>
-#include <zephyr/drivers/sensor.h>
-#include <zephyr/drivers/stepper.h>
-#include <zephyr/kernel.h>
-#include <rmw_microros/rmw_microros.h>
-
-
-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);
-}

src/servo_controller/servo_controller.hpp

@@ -1,42 +0,0 @@
-#pragma once
-#include <zephyr/types.h>
-#include <rclc/executor.h>
-#include <rcl/rcl.h>
-#include <rclc/rclc.h>
-#include <rosidl_runtime_c/message_type_support_struct.h>
-
-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;
-};

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
 )

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 <cstdint>
+#include <std_msgs/msg/float32.h>
+#include <zephyr/drivers/counter.h>
+#include <zephyr/drivers/sensor.h>
+#include <zephyr/drivers/stepper.h>
+#include <zephyr/kernel.h>
+#include <rmw_microros/rmw_microros.h>
+
+#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);
+}

src/stepper_controller/stepper_controller.hpp

@@ -0,0 +1,57 @@
+#pragma once
+#include <sys/stat.h>
+#include <zephyr/types.h>
+#include <rclc/executor.h>
+#include <rcl/rcl.h>
+#include <rclc/rclc.h>
+#include <rosidl_runtime_c/message_type_support_struct.h>
+
+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);
+};