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

added pre init calibration

Browse Source
This commit is contained in:
Timo Schneider
2026-01-10 18:04:37 +01:00
parent c2d489fff4
commit 579fca0b40
10 changed files with 47 additions and 146 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
boards/nucleo_h563zi.overlay
View File

@@ -6,6 +6,12 @@
stepper0 = &stepper0;
};
chosen {
pid,counter = &counter5;
pid,status = &green_led_1;
pid,calibration = &yellow_led_1;
};
stepper0: tmc2209_motor {
compatible = "adi,tmc2209";

1
src/CMakeLists.txt
View File

@@ -2,7 +2,6 @@ cmake_minimum_required(VERSION 3.20.0)
target_include_directories(app PRIVATE ./)
add_subdirectory(./led_controller)
add_subdirectory(./stepper_controller)
add_subdirectory(./microros)
#add_subdirectory(./usb)

10
src/devices.cpp
View File

@@ -9,17 +9,15 @@ bool verify_are_devices_available(void) {
devices_ready = false;
}
/*
if (!device_is_ready(led1.port)) {
printk("LED1 not ready\n");
if (!device_is_ready(status_led.port)) {
printk("Status LED not ready\n");
devices_ready = false;
}
if (!device_is_ready(led2.port)) {
printk("LED2 not ready\n");
if (!device_is_ready(calibration_led.port)) {
printk("Calibration LED not ready\n");
devices_ready = false;
}
*/
if (!device_is_ready(pid_counter)) {
printk("Counter not ready\n");

4
src/devices.hpp
View File

@@ -13,8 +13,8 @@
#endif
static const struct gpio_dt_spec led0 = GPIO_DT_SPEC_GET(DT_ALIAS(led0), gpios);
//static const struct gpio_dt_spec led1 = GPIO_DT_SPEC_GET(DT_ALIAS(led1), gpios);
//static const struct gpio_dt_spec led2 = GPIO_DT_SPEC_GET(DT_ALIAS(led2), gpios);
static const struct gpio_dt_spec status_led = GPIO_DT_SPEC_GET(DT_CHOSEN(pid_status), gpios);
static const struct gpio_dt_spec calibration_led = GPIO_DT_SPEC_GET(DT_CHOSEN(pid_calibration), gpios);
static const struct device *rotenc0 = DEVICE_DT_GET(DT_ALIAS(rotenc0));
static const struct device *stepper0 = DEVICE_DT_GET(DT_ALIAS(stepper0));

9
src/led_controller/CMakeLists.txt
View File

@@ -1,9 +0,0 @@
cmake_minimum_required(VERSION 3.20.0)
target_include_directories(app PRIVATE ./)
#add_subdirectory()
target_sources(app PRIVATE
led_controller.cpp
)

98
src/led_controller/led_controller.cpp
View File

@@ -1,98 +0,0 @@
#include "led_controller.hpp"
#include "devices.hpp"
#include "rcl_util.hpp"
#include <zephyr/drivers/gpio.h>
#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <std_msgs/msg/bool.h>
std_msgs__msg__Bool led_msg;
rcl_node_t LedController::node;
rcl_subscription_t LedController::led0_subscription;
/*
rcl_subscription_t LedController::led1_subscription;
rcl_subscription_t LedController::led2_subscription;
*/
void led0_callback(const void *msg){
gpio_pin_configure_dt(&led0, ((std_msgs__msg__Bool *)msg)->data ? GPIO_OUTPUT_ACTIVE : GPIO_OUTPUT_INACTIVE);
}
/*
void led1_callback(const void *msg){
gpio_pin_configure_dt(&led1, ((std_msgs__msg__Bool *)msg)->data ? GPIO_OUTPUT_ACTIVE : GPIO_OUTPUT_INACTIVE);
}
void led2_callback(const void *msg){
gpio_pin_configure_dt(&led2, ((std_msgs__msg__Bool *)msg)->data ? GPIO_OUTPUT_ACTIVE : GPIO_OUTPUT_INACTIVE);
}
*/
rcl_ret_t LedController::setup(rclc_support_t* support, rclc_executor_t* executor) {
RCL_RETURN_ON_ERROR(rclc_node_init_default(
&LedController::node,
"led_controller",
"",
support));
RCL_RETURN_ON_ERROR(rclc_subscription_init_default(
&LedController::led0_subscription,
&LedController::node,
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool),
"/nucleo/led0"));
/*
RCL_RETURN_ON_ERROR(rclc_subscription_init_default(
&LedController::led1_subscription,
&LedController::node,
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool),
"/nucleo/led1"));
RCL_RETURN_ON_ERROR(rclc_subscription_init_default(
&LedController::led2_subscription,
&LedController::node,
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool),
"/nucleo/led2"));
*/
RCL_RETURN_ON_ERROR(rclc_executor_add_subscription(
executor,
&LedController::led0_subscription,
&led_msg,
led0_callback,
ON_NEW_DATA
));
/*
RCL_RETURN_ON_ERROR(rclc_executor_add_subscription(
executor,
&LedController::led1_subscription,
&led_msg,
led1_callback,
ON_NEW_DATA
));
RCL_RETURN_ON_ERROR(rclc_executor_add_subscription(
executor,
&LedController::led2_subscription,
&led_msg,
led2_callback,
ON_NEW_DATA
));
*/
return RCL_RET_OK;
}
void LedController::kill() {
rcl_subscription_fini(&LedController::led0_subscription, &LedController::node);
//rcl_subscription_fini(&LedController::led1_subscription, &LedController::node);
//rcl_subscription_fini(&LedController::led2_subscription, &LedController::node);
rcl_node_fini(&LedController::node);
}

19
src/led_controller/led_controller.hpp
View File

@@ -1,19 +0,0 @@
#pragma once
#include <rclc/executor.h>
#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <rosidl_runtime_c/message_type_support_struct.h>
class LedController {
public:
static rcl_ret_t setup(rclc_support_t* support, rclc_executor_t* executor);
static void kill();
private:
static rcl_node_t node;
static rcl_subscription_t led0_subscription;
/*
static rcl_subscription_t led1_subscription;
static rcl_subscription_t led2_subscription;
*/
};

7
src/microros/microros_state_machine.cpp
View File

@@ -1,6 +1,5 @@
#include "microros_state_machine.hpp"
#include "led_controller.hpp"
#include "stepper_controller.hpp"
#include <zephyr/kernel.h>
@@ -32,7 +31,6 @@ void micro_ros_state_machine() {
micro_ros_state = ROS_AGENT_CONNECTION_DISCONNECTED;
case ROS_AGENT_CONNECTION_DISCONNECTED:
LedController::kill();
StepperController::kill();
rclc_executor_fini(&executor);
rclc_support_fini(&support);
@@ -49,11 +47,6 @@ void micro_ros_state_machine() {
micro_ros_state = ROS_AGENT_CONNECTION_SETUP;
case ROS_AGENT_CONNECTION_SETUP:
if (LedController::setup(&support, &executor) != RCL_RET_OK) {
printk("LED Controller cant be initialized!\r\n");
micro_ros_state = ROS_AGENT_CONNECTION_ERROR;
break;
}
if (StepperController::setup(&support, &executor) != RCL_RET_OK) {
printk("Servo Controller cant be initialized!\r\n");
micro_ros_state = ROS_AGENT_CONNECTION_ERROR;

35
src/stepper_controller/stepper_controller.cpp
View File

@@ -29,6 +29,8 @@ float StepperController::target_angle = 0.0f;
float StepperController::current_velocity;
float StepperController::current_acceleration;
float StepperController::direction_factor = 1.0;
// max accel and velocity
float StepperController::max_velocity = 2.0f; // rps
float StepperController::max_accel = 15.0f; // rpss
@@ -92,12 +94,37 @@ void StepperController::init() {
.ticks = counter_us_to_ticks(pid_counter, PID_LOOP_US), // 2 ms = 500 Hz
.flags = 0,
};
calibrate();
counter_set_channel_alarm(pid_counter, 0, &alarm_cfg);
counter_start(pid_counter);
}
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.1);
k_sleep(K_MSEC(250));
sample_angle();
last_angle = current_angle;
float left_angle = current_angle;
set_motor_velocity(-0.1);
k_sleep(K_MSEC(500));
sample_angle();
float right_angle = current_angle;
set_motor_velocity(0.1);
k_sleep(K_MSEC(250));
if (right_angle >= left_angle) {
direction_factor = -1.0;
}
gpio_pin_configure_dt(&status_led, GPIO_OUTPUT_ACTIVE);
gpio_pin_configure_dt(&calibration_led, GPIO_OUTPUT_INACTIVE);
}
rcl_ret_t StepperController::setup(rclc_support_t* support, rclc_executor_t* executor)
@@ -160,12 +187,12 @@ void StepperController::sample_angle() {
return;
}
val.val1 = (val.val1 + ROT_MIDDLE_OFFSET) % 360;
float angle = (float)val.val1 - 180.0f + (float)val.val2 / 1000000.0f;
float angle = direction_factor * ((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)
if (full_angle - last_angle > 270)
angle_rotation_count--;
if (full_angle - last_angle < -350)
if (full_angle - last_angle < -270)
angle_rotation_count++;
last_angle = current_angle;

4
src/stepper_controller/stepper_controller.hpp
View File

@@ -9,11 +9,15 @@
class StepperController {
public:
static void init();
static void calibrate();
static rcl_ret_t setup(rclc_support_t* support, rclc_executor_t* executor);
static void pid();
static rcl_ret_t publish();
static void kill();
// calibration
static float direction_factor;
// angles
static float current_angle;
static float target_angle;