SHERPA/microros_turntable_controller
4
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

moved from servo to stepper

Browse Source
This commit is contained in:
Timo Schneider
2025-11-30 16:50:46 +01:00
parent 0ff1ab4b88
commit a4c2d82c4c
10 changed files with 350 additions and 254 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
boards/nucleo_h563zi.overlay
View File

@@ -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";

5
prj.conf
View File

@@ -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

2
src/CMakeLists.txt
View File

@@ -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)

5
src/main.cpp
View File

@@ -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(

10
src/microros/microros_state_machine.cpp
View File

@@ -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 (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;

202
src/servo_controller/servo_controller.cpp
View File

@@ -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);
}

42
src/servo_controller/servo_controller.hpp
View File

@@ -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;
};

2
src/servo_controller/CMakeLists.txt → src/stepper_controller/CMakeLists.txt
View File

@@ -5,5 +5,5 @@ target_include_directories(app PRIVATE ./)
#add_subdirectory()
target_sources(app PRIVATE
servo_controller.cpp
stepper_controller.cpp
)

278
src/stepper_controller/stepper_controller.cpp Normal file
View File

@@ -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);
}

57
src/stepper_controller/stepper_controller.hpp Normal file
View File

@@ -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);
};