diff --git a/src/microros/microros_clock.hpp b/src/microros/microros_clock.hpp deleted file mode 100644 index b0be45e..0000000 --- a/src/microros/microros_clock.hpp +++ /dev/null @@ -1,4 +0,0 @@ -#pragma once -#include - -static rcl_clock_t clock; \ No newline at end of file diff --git a/src/microros/microros_state_machine.cpp b/src/microros/microros_state_machine.cpp index 5f4423c..f130630 100644 --- a/src/microros/microros_state_machine.cpp +++ b/src/microros/microros_state_machine.cpp @@ -1,6 +1,5 @@ #include "microros_state_machine.hpp" -#include "microros_clock.hpp" #include "led_controller.hpp" #include "servo_controller.hpp" @@ -42,10 +41,6 @@ void micro_ros_state_machine() { k_msleep(1000); break; } - if (rcl_clock_init(RCL_ROS_TIME, &clock, &allocator) != RCL_RET_OK) { - printf("Failed to initialize clock\n"); - return; - } if (rclc_executor_init(&executor, &support.context, 8, &allocator) != RCL_RET_OK){ printk("Can't init executor!\r\n"); break; @@ -74,6 +69,7 @@ void micro_ros_state_machine() { } micro_ros_state = ROS_AGENT_CONNECTION_CONNECTED; printk("micro ros agent reachable!\r\n"); + rmw_uros_sync_session(100); case ROS_AGENT_CONNECTION_CONNECTED: if (rclc_executor_spin_some(&executor, 100) != RCL_RET_OK) { diff --git a/src/servo_controller/servo_controller.cpp b/src/servo_controller/servo_controller.cpp index 74bb0dd..e29fc10 100644 --- a/src/servo_controller/servo_controller.cpp +++ b/src/servo_controller/servo_controller.cpp @@ -1,18 +1,20 @@ #include "servo_controller.hpp" #include "devices.hpp" #include "rcl_util.hpp" -#include "microros_clock.hpp" #include -#include #include #include +#include + const uint32_t min_pulse_ns = 500000; const uint32_t max_pulse_ns = 2500000; rcl_node_t ServoController::node; -rcl_subscription_t ServoController::servo0_subscription; +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; @@ -20,6 +22,8 @@ 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; @@ -29,6 +33,14 @@ 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(); @@ -54,24 +66,51 @@ rcl_ret_t ServoController::setup(rclc_support_t* support, rclc_executor_t* execu RCL_RETURN_ON_ERROR(rclc_publisher_init_default( &ServoController::servo0_publisher, &ServoController::node, - ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, TransformStamped), - "/tf" + ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32), + "/nucleo/servo0/position" )); RCL_RETURN_ON_ERROR(rclc_subscription_init_default( - &ServoController::servo0_subscription, + &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_subscription, + &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, @@ -123,40 +162,18 @@ void ServoController::servo_pid() { } rcl_ret_t ServoController::publish() { - geometry_msgs__msg__TransformStamped tf; + std_msgs__msg__Float32 msg; + std_msgs__msg__Float32__init(&msg); - int64_t now_ns = 0; - rcl_clock_get_now(&clock, &now_ns); - tf.header.stamp.sec = now_ns / 1000000000ULL; - tf.header.stamp.nanosec = now_ns % 1000000000ULL; + msg.data = ServoController::current_angle; - // Fill header - char frame_id_buf[32] = "livox_base"; - tf.header.frame_id.data = frame_id_buf; - tf.header.frame_id.size = strlen(frame_id_buf); - tf.header.frame_id.capacity = sizeof(frame_id_buf); - - char child_id_buf[32] = "frame_default"; - tf.child_frame_id.data = child_id_buf; - tf.child_frame_id.size = strlen(child_id_buf); - tf.child_frame_id.capacity = sizeof(child_id_buf); - - // Rotation around Z - float yaw = (current_angle - 90.0f) * 3.141592653589793f / 180.0f; - tf.transform.translation.x = 0.0; - tf.transform.translation.y = 0.0; - tf.transform.translation.z = 0.0; - - tf.transform.rotation.x = 0.0f; - tf.transform.rotation.y = 0.0f; - tf.transform.rotation.z = sinf(yaw / 2.0f); - tf.transform.rotation.w = cosf(yaw / 2.0f); - - return rcl_publish(&ServoController::servo0_publisher, &tf, NULL); + return rcl_publish(&ServoController::servo0_publisher, &msg, NULL); } void ServoController::kill() { rcl_publisher_fini(&ServoController::servo0_publisher, &ServoController::node); - rcl_subscription_fini(&ServoController::servo0_subscription, &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); } \ No newline at end of file diff --git a/src/servo_controller/servo_controller.hpp b/src/servo_controller/servo_controller.hpp index 7efe93c..228b6d9 100644 --- a/src/servo_controller/servo_controller.hpp +++ b/src/servo_controller/servo_controller.hpp @@ -16,10 +16,16 @@ public: 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_subscription; + 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 @@ -33,8 +39,4 @@ private: static constexpr float kp = 2.0f; static constexpr float ki = 0.0f; static constexpr float kd = 0.05f; - - // Limits - static constexpr float max_velocity = 50.0f; // deg/s - static constexpr float max_accel = 100.0f; // deg/s^2 };