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

added retained calibration

Browse Source
This commit is contained in:
Timo Schneider
2026-01-11 12:03:47 +01:00
parent 55d35c41af
commit e9fe3496ac
8 changed files with 69 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
.vscode/launch.json vendored
View File

@@ -9,7 +9,7 @@
"showDevDebugOutput": "none",
"servertype": "external",
"gdbTarget": "localhost:3333",
"targetId": "nrf54l",
"targetId": "stm32h5",
}
]
}

19
boards/nucleo_h563zi.overlay
View File

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

8
prj.conf
View File

@@ -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
CONFIG_SPEED_OPTIMIZATIONS=n
CONFIG_DEBUG_OPTIMIZATIONS=y
CONFIG_DEBUG_THREAD_INFO=y

12
src/devices.cpp
View File

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

6
src/devices.hpp
View File

@@ -12,9 +12,9 @@
#include <devicetree.h>
#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));

2
src/main.cpp
View File

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

17
src/microros/microros_state_machine.cpp
View File

@@ -1,8 +1,10 @@
#include "microros_state_machine.hpp"
#include "devices.hpp"
#include "stepper_controller.hpp"
#include <zephyr/kernel.h>
#include <zephyr/sys/reboot.h>
#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
@@ -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);
}
}

37
src/stepper_controller/stepper_controller.cpp
View File

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