From cfb3e6406d2731d6a92fc9cc289f994d8bf1df02 Mon Sep 17 00:00:00 2001 From: Victor Belov Date: Thu, 18 May 2023 17:41:19 +0300 Subject: [PATCH] Added axis_min_velocity configuration parameter to make it easier to work with motors which are unstable at low velocities --- README.md | 2 ++ config/odrive.yaml | 3 +++ src/odrive_axis.cpp | 30 +++++++++++++++++++++++++----- src/odrive_axis.hpp | 10 ++++++---- src/odrive_node.cpp | 2 +- 5 files changed, 37 insertions(+), 10 deletions(-) diff --git a/README.md b/README.md index 73cb2ae..44436b1 100644 --- a/README.md +++ b/README.md @@ -38,6 +38,7 @@ Driver is configured with the following configuration paramters: | ```can_tx_ropic``` | String | Topic name through which driver sends CAN messages to socketcan_bridge | | ```update_rate``` | Double | Update rate in Hz of how frequently driver gets data from ODrive controllers | | ```engage_on_startup``` | Boolean | If driver should engage all motors on startup (not implemented yet) | +| ```axis_min_velocity``` | Double | Minimum axis velocity, if target velocity is less then this value then it is set to this value (if set to 1.0 target velocity of 0.5 will become 1.0, target velocity of -0.5 will become -1.0) | | ```axis_names``` | String[] | Array of names of axises which are then used to name topics corresponding to axises| | ```axis_can_ids``` | Int[] | Array of axis CAN ids as they are configured on ODrive controllers for every axis | | ```axis_directions``` | String[] | Axis direction either ```forward``` or ```backward```, ```backward``` reverses axis control, angle and current velocity | @@ -61,6 +62,7 @@ When driver is started it creates the following topics for every axis configured |```/angle``` | ```std_msgs/Float64``` | Outbound | Angle of the axis in rad | |```/current``` | ```std_msgs/Float64``` | Outbound | Axis current consumption | |```/current_velocity``` | ```std_msgs/Float64``` | Outbound | Current axis angular velocity in rad/s | +|```/output_velocity``` | ```std_msgs/Float64``` | Outbound | Velocity which is sent to ODrive (will differ from target_velocity if axis_min_velocity is set) | |```/target_velocity``` | ```std_msgs/Float64``` | Inbound | Target axis angular velocity in rad/s | |```/voltage``` | ```std_msgs/Float64``` | Outbound | Axis voltage of power supply in V | diff --git a/config/odrive.yaml b/config/odrive.yaml index 7a3b252..e506dd8 100644 --- a/config/odrive.yaml +++ b/config/odrive.yaml @@ -8,6 +8,9 @@ can_tx_ropic: "/can/sent_messages" update_rate: 10 # engage axises on startup engage_on_startup: true +# minimum axis velocity - e.g if set to 1.0 target velocity will be set to this value +# for both positive and negative velocities (0.5 will become 1.0, -0.5 will become -1.0) +axis_min_velocity: 1.0 # array of strings representing names of axises axis_names: ["front_left_wheel", "front_right_wheel", "middle_left_wheel", "middle_right_wheel", "rear_left_wheel", "rear_right_wheel"] # array of ints representing can ids of axises diff --git a/src/odrive_axis.cpp b/src/odrive_axis.cpp index cb899a8..3301cca 100644 --- a/src/odrive_axis.cpp +++ b/src/odrive_axis.cpp @@ -13,11 +13,13 @@ namespace odrive { node->param("can_tx_topic", can_tx_topic_, "/can/sent_messages"); node->param("update_rate", update_rate_, DEFAULT_UPDATE_RATE); node->param("engage_on_startup", engage_on_startup_, false); + node->param("axis_min_velocity", axis_min_velocity_, 0); received_messages_sub_ = node->subscribe(can_rx_topic_, 1, std::bind(&ODriveAxis::canReceivedMessagesCallback, this, std::placeholders::_1)); sent_messages_pub_ = node->advertise(can_tx_topic_, 1); target_velocity_sub_ = node->subscribe("/" + axis_name_ + "/target_velocity", 1, std::bind(&ODriveAxis::velocityReceivedMessagesCallback, this, std::placeholders::_1)); + output_velocity_pub_ = node->advertise("/" + axis_name + "/output_velocity", 1); axis_angle_pub_ = node->advertise("/" + axis_name + "/angle", 1); axis_velocity_pub_ = node->advertise("/" + axis_name + "/current_velocity", 1); axis_voltage_pub_ = node->advertise("/" + axis_name + "/voltage", 1); @@ -50,7 +52,7 @@ namespace odrive { uint8_t *ptrPositionEstimate = (uint8_t *)&positionEstimate; uint8_t *ptrAxisVoltage = (uint8_t *)&axisVoltage; uint8_t *ptrAxisCurrent = (uint8_t *)&axisCurrent; - if (NODE_ID == axis_can_id_) { + if (NODE_ID == (uint32_t)axis_can_id_) { uint32_t CMD_ID = (ID & 0x01F); switch (CMD_ID) { case ODriveCommandId::HEARTBEAT_MESSAGE: @@ -98,12 +100,29 @@ namespace odrive { } void ODriveAxis::velocityReceivedMessagesCallback(const std_msgs::Float64::ConstPtr& msg) { - ROS_DEBUG("Received velocity message"); - double targetVelocity = msg->data / (2.0 * M_PI) * direction_; - setInputVelocity(targetVelocity); + std_msgs::Float64 velocity_msg; + double targetVelocity = msg->data; + // Check that target velocity is not less then axis minimum velocity in both directions + if (targetVelocity != 0 && axis_min_velocity_ != 0) { + if (targetVelocity > 0) { + if (targetVelocity < axis_min_velocity_) { + targetVelocity = axis_min_velocity_; + } + } else { + if (targetVelocity > (axis_min_velocity_ * -1)) { + targetVelocity = axis_min_velocity_ * -1; + } + } + } + // Publish velocity to output_velocity + velocity_msg.data = (double)targetVelocity; + output_velocity_pub_.publish(velocity_msg); + double odriveTargetVelocity = targetVelocity / (2.0 * M_PI) * direction_; + setInputVelocity(odriveTargetVelocity); } void ODriveAxis::updateTimerCallback(const ros::TimerEvent& event) { + (void)event; requestEncoderEstimate(); requestBusVoltageAndCurrent(); } @@ -173,7 +192,8 @@ namespace odrive { uint32_t ODriveAxis::createCanId(int axis_can_id, int command) { - uint32_t can_id = (axis_can_id_ << 5) | command; + uint32_t can_id; + can_id = (axis_can_id << 5) | command; return can_id; } diff --git a/src/odrive_axis.hpp b/src/odrive_axis.hpp index 77913d9..c292b12 100644 --- a/src/odrive_axis.hpp +++ b/src/odrive_axis.hpp @@ -68,11 +68,12 @@ namespace odrive { ros::Subscriber received_messages_sub_; ros::Publisher sent_messages_pub_; ros::Subscriber target_velocity_sub_; + ros::Publisher output_velocity_pub_; ros::Publisher axis_angle_pub_; - ros::Publisher axis_velocity_pub_; - ros::Publisher axis_voltage_pub_; - ros::Publisher axis_current_pub_; - ros::Timer axis_update_timer_; + ros::Publisher axis_velocity_pub_; + ros::Publisher axis_voltage_pub_; + ros::Publisher axis_current_pub_; + ros::Timer axis_update_timer_; std::string axis_name_; std::string can_rx_topic_; std::string can_tx_topic_; @@ -80,6 +81,7 @@ namespace odrive { int direction_; bool engage_on_startup_; double update_rate_; + double axis_min_velocity_; double axis_angle_; double axis_velocity_; double axis_voltage_; diff --git a/src/odrive_node.cpp b/src/odrive_node.cpp index b3bf093..19e4fe5 100644 --- a/src/odrive_node.cpp +++ b/src/odrive_node.cpp @@ -68,7 +68,7 @@ int main(int argc, char** argv) { ROS_ERROR("axis CAN ids must be distinct"); return -1; } - for (int i = 0; i < axis_names_list.size(); i++) { + for (int i = 0; i < (int)axis_names_list.size(); i++) { ROS_INFO("Adding axis %s with CAN id %d and direction %s", axis_names_list[i].c_str(), axis_can_ids_list[i], axis_directions_list[i].c_str()); if (axis_names_list[i].length() == 0) {