moved from servo to stepper

src/stepper_controller/CMakeLists.txt

@@ -0,0 +1,9 @@
+cmake_minimum_required(VERSION 3.20.0)
+
+target_include_directories(app PRIVATE ./)
+
+#add_subdirectory()
+
+target_sources(app PRIVATE
+    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);
+};