From 230d35c58b1617c0a6ed999fab584acb0e2cb71c Mon Sep 17 00:00:00 2001 From: Joshua Whitley Date: Wed, 14 Apr 2021 15:39:00 -0500 Subject: [PATCH 01/17] Using std::thread for rxThread --- vesc_driver/src/vesc_interface.cpp | 19 ++++++++----------- 1 file changed, 8 insertions(+), 11 deletions(-) diff --git a/vesc_driver/src/vesc_interface.cpp b/vesc_driver/src/vesc_interface.cpp index 3febe1a..618185f 100644 --- a/vesc_driver/src/vesc_interface.cpp +++ b/vesc_driver/src/vesc_interface.cpp @@ -35,6 +35,7 @@ #include #include #include +#include #include @@ -51,21 +52,21 @@ class VescInterface::Impl serial::eightbits, serial::parity_none, serial::stopbits_one, serial::flowcontrol_none) {} - void* rxThread(void); + void rxThread(); - static void* rxThreadHelper(void *context) + std::thread rxThreadHelper() { - return ((VescInterface::Impl*)context)->rxThread(); + return std::thread(&Impl::rxThread, this); } - pthread_t rx_thread_; + std::thread rx_thread_; bool rx_thread_run_; PacketHandlerFunction packet_handler_; ErrorHandlerFunction error_handler_; serial::Serial serial_; }; -void* VescInterface::Impl::rxThread(void) +void VescInterface::Impl::rxThread() { Buffer buffer; buffer.reserve(4096); @@ -200,9 +201,7 @@ void VescInterface::connect(const std::string& port) // start up a monitoring thread impl_->rx_thread_run_ = true; - int result = - pthread_create(&impl_->rx_thread_, NULL, &VescInterface::Impl::rxThreadHelper, impl_.get()); - assert(0 == result); + impl_->rx_thread_ = impl_->rxThreadHelper(); } void VescInterface::disconnect() @@ -213,9 +212,7 @@ void VescInterface::disconnect() { // bring down read thread impl_->rx_thread_run_ = false; - int result = pthread_join(impl_->rx_thread_, NULL); - assert(0 == result); - + impl_->rx_thread_.join(); impl_->serial_.close(); } } From 83289aad71112608b2db492a7819b0145d8874cc Mon Sep 17 00:00:00 2001 From: Thomas Balch Date: Wed, 8 Jun 2022 12:41:44 -0400 Subject: [PATCH 02/17] Add current control to vesc driver --- .../vesc_ackermann/ackermann_to_vesc.h | 4 ++ vesc_ackermann/src/ackermann_to_vesc.cpp | 37 ++++++++++++++++++- 2 files changed, 40 insertions(+), 1 deletion(-) diff --git a/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.h b/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.h index 33c4d10..2674ab3 100644 --- a/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.h +++ b/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.h @@ -42,7 +42,9 @@ class AckermannToVesc private: // ROS parameters // conversion gain and offset + bool previous_mode_speed_ = true; double speed_to_erpm_gain_, speed_to_erpm_offset_; + double accel_to_current_gain_, accel_to_brake_gain_; double steering_to_servo_gain_, steering_to_servo_offset_; /** @todo consider also providing an interpolated look-up table conversion */ @@ -50,6 +52,8 @@ class AckermannToVesc // ROS services ros::Publisher erpm_pub_; ros::Publisher servo_pub_; + ros::Publisher current_pub_; + ros::Publisher brake_pub_; ros::Subscriber ackermann_sub_; // ROS callbacks diff --git a/vesc_ackermann/src/ackermann_to_vesc.cpp b/vesc_ackermann/src/ackermann_to_vesc.cpp index da29626..af22e79 100644 --- a/vesc_ackermann/src/ackermann_to_vesc.cpp +++ b/vesc_ackermann/src/ackermann_to_vesc.cpp @@ -46,6 +46,10 @@ AckermannToVesc::AckermannToVesc(ros::NodeHandle nh, ros::NodeHandle private_nh) return; if (!getRequiredParam(nh, "speed_to_erpm_offset", &speed_to_erpm_offset_)) return; + if (!getRequiredParam(nh, "accel_to_current_gain", &accel_to_current_gain_)) + return; + if (!getRequiredParam(nh, "accel_to_brake_gain", &accel_to_brake_gain_)) + return; if (!getRequiredParam(nh, "steering_angle_to_servo_gain", &steering_to_servo_gain_)) return; if (!getRequiredParam(nh, "steering_angle_to_servo_offset", &steering_to_servo_offset_)) @@ -53,6 +57,8 @@ AckermannToVesc::AckermannToVesc(ros::NodeHandle nh, ros::NodeHandle private_nh) // create publishers to vesc electric-RPM (speed) and servo commands erpm_pub_ = nh.advertise("commands/motor/speed", 10); + current_pub_ = nh.advertise("commands/motor/current", 10); + brake_pub_ = nh.advertise("commands/motor/brake", 10); servo_pub_ = nh.advertise("commands/servo/position", 10); // subscribe to ackermann topic @@ -66,6 +72,19 @@ void AckermannToVesc::ackermannCmdCallback(const AckermannMsgPtr& cmd) std_msgs::Float64::Ptr erpm_msg(new std_msgs::Float64); erpm_msg->data = speed_to_erpm_gain_ * cmd->drive.speed + speed_to_erpm_offset_; + // calc vesc current/brake (acceleration) + bool is_positive_accel = true; + std_msgs::Float64::Ptr current_msg(new std_msgs::Float64); + std_msgs::Float64::Ptr brake_msg(new std_msgs::Float64); + current_msg->data = 0; + brake_msg->data = 0; + if (cmd->drive.acceleration < 0) { + brake_msg->data = accel_to_brake_gain_ * cmd->drive.acceleration; + is_positive_accel = false; + } else { + current_msg->data = accel_to_current_gain_ * cmd->drive.acceleration; + } + // calc steering angle (servo) std_msgs::Float64::Ptr servo_msg(new std_msgs::Float64); servo_msg->data = steering_to_servo_gain_ * cmd->drive.steering_angle + steering_to_servo_offset_; @@ -73,9 +92,25 @@ void AckermannToVesc::ackermannCmdCallback(const AckermannMsgPtr& cmd) // publish if (ros::ok()) { - erpm_pub_.publish(erpm_msg); + // The below code attempts to stick to the previous mode until a new message forces a mode switch. + if (erpm_msg->data != 0 || previous_mode_speed_) { + erpm_pub_.publish(erpm_msg); + } else { + if (is_positive_accel) { + current_pub_.publish(current_msg); + } else { + brake_pub_.publish(brake_msg); + } + } servo_pub_.publish(servo_msg); } + + // The lines below are to determine which mode we are in so we can hold until new messages force a switch. + if (erpm_msg->data != 0) { + previous_mode_speed_ = true; + } else if (current_msg->data != 0 || brake_msg->data != 0) { + previous_mode_speed_ = false; + } } template From 4b43fc3bef45d02da5c5654ffc105cd0fdcd34bc Mon Sep 17 00:00:00 2001 From: Thomas Balch Date: Wed, 8 Jun 2022 12:41:44 -0400 Subject: [PATCH 03/17] Add current control to vesc driver --- .../vesc_ackermann/ackermann_to_vesc.h | 4 ++ vesc_ackermann/src/ackermann_to_vesc.cpp | 49 ++++++++++++++++++- 2 files changed, 52 insertions(+), 1 deletion(-) diff --git a/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.h b/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.h index 33c4d10..2674ab3 100644 --- a/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.h +++ b/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.h @@ -42,7 +42,9 @@ class AckermannToVesc private: // ROS parameters // conversion gain and offset + bool previous_mode_speed_ = true; double speed_to_erpm_gain_, speed_to_erpm_offset_; + double accel_to_current_gain_, accel_to_brake_gain_; double steering_to_servo_gain_, steering_to_servo_offset_; /** @todo consider also providing an interpolated look-up table conversion */ @@ -50,6 +52,8 @@ class AckermannToVesc // ROS services ros::Publisher erpm_pub_; ros::Publisher servo_pub_; + ros::Publisher current_pub_; + ros::Publisher brake_pub_; ros::Subscriber ackermann_sub_; // ROS callbacks diff --git a/vesc_ackermann/src/ackermann_to_vesc.cpp b/vesc_ackermann/src/ackermann_to_vesc.cpp index da29626..c8c057f 100644 --- a/vesc_ackermann/src/ackermann_to_vesc.cpp +++ b/vesc_ackermann/src/ackermann_to_vesc.cpp @@ -46,6 +46,10 @@ AckermannToVesc::AckermannToVesc(ros::NodeHandle nh, ros::NodeHandle private_nh) return; if (!getRequiredParam(nh, "speed_to_erpm_offset", &speed_to_erpm_offset_)) return; + if (!getRequiredParam(nh, "accel_to_current_gain", &accel_to_current_gain_)) + return; + if (!getRequiredParam(nh, "accel_to_brake_gain", &accel_to_brake_gain_)) + return; if (!getRequiredParam(nh, "steering_angle_to_servo_gain", &steering_to_servo_gain_)) return; if (!getRequiredParam(nh, "steering_angle_to_servo_offset", &steering_to_servo_offset_)) @@ -53,6 +57,8 @@ AckermannToVesc::AckermannToVesc(ros::NodeHandle nh, ros::NodeHandle private_nh) // create publishers to vesc electric-RPM (speed) and servo commands erpm_pub_ = nh.advertise("commands/motor/speed", 10); + current_pub_ = nh.advertise("commands/motor/current", 10); + brake_pub_ = nh.advertise("commands/motor/brake", 10); servo_pub_ = nh.advertise("commands/servo/position", 10); // subscribe to ackermann topic @@ -66,6 +72,22 @@ void AckermannToVesc::ackermannCmdCallback(const AckermannMsgPtr& cmd) std_msgs::Float64::Ptr erpm_msg(new std_msgs::Float64); erpm_msg->data = speed_to_erpm_gain_ * cmd->drive.speed + speed_to_erpm_offset_; + // calc vesc current/brake (acceleration) + bool is_positive_accel = true; + std_msgs::Float64::Ptr current_msg(new std_msgs::Float64); + std_msgs::Float64::Ptr brake_msg(new std_msgs::Float64); + current_msg->data = 0; + brake_msg->data = 0; + if (cmd->drive.acceleration < 0) + { + brake_msg->data = accel_to_brake_gain_ * cmd->drive.acceleration; + is_positive_accel = false; + } + else + { + current_msg->data = accel_to_current_gain_ * cmd->drive.acceleration; + } + // calc steering angle (servo) std_msgs::Float64::Ptr servo_msg(new std_msgs::Float64); servo_msg->data = steering_to_servo_gain_ * cmd->drive.steering_angle + steering_to_servo_offset_; @@ -73,9 +95,34 @@ void AckermannToVesc::ackermannCmdCallback(const AckermannMsgPtr& cmd) // publish if (ros::ok()) { - erpm_pub_.publish(erpm_msg); + // The below code attempts to stick to the previous mode until a new message forces a mode switch. + if (erpm_msg->data != 0 || previous_mode_speed_) + { + erpm_pub_.publish(erpm_msg); + } + else + { + if (is_positive_accel) + { + current_pub_.publish(current_msg); + } + else + { + brake_pub_.publish(brake_msg); + } + } servo_pub_.publish(servo_msg); } + + // The lines below are to determine which mode we are in so we can hold until new messages force a switch. + if (erpm_msg->data != 0) + { + previous_mode_speed_ = true; + } + else if (current_msg->data != 0 || brake_msg->data != 0) + { + previous_mode_speed_ = false; + } } template From e79898fc61b765bee6622f9d82387f5ac61c6543 Mon Sep 17 00:00:00 2001 From: Valerio Magnago Date: Mon, 20 Feb 2023 00:25:49 +0100 Subject: [PATCH 04/17] Fix if comparison. --- vesc_driver/src/vesc_driver.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/vesc_driver/src/vesc_driver.cpp b/vesc_driver/src/vesc_driver.cpp index 95371eb..ed90113 100644 --- a/vesc_driver/src/vesc_driver.cpp +++ b/vesc_driver/src/vesc_driver.cpp @@ -190,7 +190,7 @@ void VescDriver::vescErrorCallback(const std::string& error) */ void VescDriver::dutyCycleCallback(const std_msgs::Float64::ConstPtr& duty_cycle) { - if (driver_mode_ = MODE_OPERATING) + if (driver_mode_ == MODE_OPERATING) { vesc_.setDutyCycle(duty_cycle_limit_.clip(duty_cycle->data)); } @@ -203,7 +203,7 @@ void VescDriver::dutyCycleCallback(const std_msgs::Float64::ConstPtr& duty_cycle */ void VescDriver::currentCallback(const std_msgs::Float64::ConstPtr& current) { - if (driver_mode_ = MODE_OPERATING) + if (driver_mode_ == MODE_OPERATING) { vesc_.setCurrent(current_limit_.clip(current->data)); } @@ -216,7 +216,7 @@ void VescDriver::currentCallback(const std_msgs::Float64::ConstPtr& current) */ void VescDriver::brakeCallback(const std_msgs::Float64::ConstPtr& brake) { - if (driver_mode_ = MODE_OPERATING) + if (driver_mode_ == MODE_OPERATING) { vesc_.setBrake(brake_limit_.clip(brake->data)); } @@ -230,7 +230,7 @@ void VescDriver::brakeCallback(const std_msgs::Float64::ConstPtr& brake) */ void VescDriver::speedCallback(const std_msgs::Float64::ConstPtr& speed) { - if (driver_mode_ = MODE_OPERATING) + if (driver_mode_ == MODE_OPERATING) { vesc_.setSpeed(speed_limit_.clip(speed->data)); } @@ -242,7 +242,7 @@ void VescDriver::speedCallback(const std_msgs::Float64::ConstPtr& speed) */ void VescDriver::positionCallback(const std_msgs::Float64::ConstPtr& position) { - if (driver_mode_ = MODE_OPERATING) + if (driver_mode_ == MODE_OPERATING) { // ROS uses radians but VESC seems to use degrees. Convert to degrees. double position_deg = position_limit_.clip(position->data) * 180.0 / M_PI; @@ -255,7 +255,7 @@ void VescDriver::positionCallback(const std_msgs::Float64::ConstPtr& position) */ void VescDriver::servoCallback(const std_msgs::Float64::ConstPtr& servo) { - if (driver_mode_ = MODE_OPERATING) + if (driver_mode_ == MODE_OPERATING) { double servo_clipped(servo_limit_.clip(servo->data)); vesc_.setServo(servo_clipped); From 5d4565906b861d2b8b78362c07d1f112bb08766d Mon Sep 17 00:00:00 2001 From: jetsonCommit Date: Thu, 15 May 2025 20:50:57 -0700 Subject: [PATCH 05/17] fixed odometry --- vesc_ackermann/src/vesc_to_odom.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/vesc_ackermann/src/vesc_to_odom.cpp b/vesc_ackermann/src/vesc_to_odom.cpp index 17213f3..bf18c60 100644 --- a/vesc_ackermann/src/vesc_to_odom.cpp +++ b/vesc_ackermann/src/vesc_to_odom.cpp @@ -64,7 +64,7 @@ VescToOdom::VescToOdom(const rclcpp::NodeOptions & options) declare_parameter("speed_to_erpm_offset", 0.0); speed_to_erpm_gain_ = get_parameter("speed_to_erpm_gain").get_value(); - speed_to_erpm_offset_ = get_parameter("speed_to_erpm_gain").get_value(); + speed_to_erpm_offset_ = get_parameter("speed_to_erpm_offset").get_value(); if (use_servo_cmd_) { declare_parameter("steering_angle_to_servo_gain", 0.0); @@ -104,7 +104,7 @@ void VescToOdom::vescStateCallback(const VescStateStamped::SharedPtr state) } // convert to engineering units - double current_speed = (-state->state.speed - speed_to_erpm_offset_) / speed_to_erpm_gain_; + double current_speed = (state->state.speed - speed_to_erpm_offset_) / speed_to_erpm_gain_; if (std::fabs(current_speed) < 0.05) { current_speed = 0.0; } From 8728f4da95af8b583eb1d1df00dcf222951b469d Mon Sep 17 00:00:00 2001 From: jetsonCommit Date: Thu, 15 May 2025 21:36:10 -0700 Subject: [PATCH 06/17] first braking test --- .../vesc_ackermann/ackermann_to_vesc.hpp | 6 +++ vesc_ackermann/src/ackermann_to_vesc.cpp | 47 +++++++++++++++++-- 2 files changed, 49 insertions(+), 4 deletions(-) diff --git a/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.hpp b/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.hpp index b646c36..b5bd4a0 100644 --- a/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.hpp +++ b/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.hpp @@ -32,6 +32,7 @@ #define VESC_ACKERMANN__ACKERMANN_TO_VESC_HPP_ #include +#include #include #include @@ -39,6 +40,7 @@ namespace vesc_ackermann { using ackermann_msgs::msg::AckermannDriveStamped; +using nav_msgs::msg::Odometry; using std_msgs::msg::Float64; class AckermannToVesc : public rclcpp::Node @@ -51,16 +53,20 @@ class AckermannToVesc : public rclcpp::Node // conversion gain and offset double speed_to_erpm_gain_, speed_to_erpm_offset_; double steering_to_servo_gain_, steering_to_servo_offset_; + double current_vel_, vel_diff_thresh_; /** @todo consider also providing an interpolated look-up table conversion */ // ROS services rclcpp::Publisher::SharedPtr erpm_pub_; rclcpp::Publisher::SharedPtr servo_pub_; + rclcpp::Publisher::SharedPtr brake_pub_; + rclcpp::Subscription::SharedPtr odom_sub_; rclcpp::Subscription::SharedPtr ackermann_sub_; // ROS callbacks void ackermannCmdCallback(const AckermannDriveStamped::SharedPtr cmd); + void odomCallback(const nav_msgs::msg::Odometry::SharedPtr odom_msg); }; } // namespace vesc_ackermann diff --git a/vesc_ackermann/src/ackermann_to_vesc.cpp b/vesc_ackermann/src/ackermann_to_vesc.cpp index 12a69a0..bf5b887 100644 --- a/vesc_ackermann/src/ackermann_to_vesc.cpp +++ b/vesc_ackermann/src/ackermann_to_vesc.cpp @@ -41,6 +41,7 @@ namespace vesc_ackermann { using ackermann_msgs::msg::AckermannDriveStamped; +using nav_msgs::msg::Odometry; using std::placeholders::_1; using std_msgs::msg::Float64; @@ -59,30 +60,68 @@ AckermannToVesc::AckermannToVesc(const rclcpp::NodeOptions & options) steering_to_servo_gain_ = get_parameter("steering_angle_to_servo_gain").get_value(); steering_to_servo_offset_ = get_parameter("steering_angle_to_servo_offset").get_value(); + // create publishers to vesc electric-RPM (speed) and servo commands erpm_pub_ = create_publisher("commands/motor/speed", 10); servo_pub_ = create_publisher("commands/servo/position", 10); + // create brake publisher + brake_pub_ = create_publisher("commands/motor/brake", 10); + current_vel_ = 0.0; + vel_diff_thresh_ = 0.3; + // subscribe to ackermann topic ackermann_sub_ = create_subscription( "ackermann_cmd", 10, std::bind(&AckermannToVesc::ackermannCmdCallback, this, _1)); + + // subscribe to odom topic + odom_sub_ = create_subscription( + "odom", 10, std::bind(&AckermannToVesc::odomCallback, this, _1)); + } void AckermannToVesc::ackermannCmdCallback(const AckermannDriveStamped::SharedPtr cmd) { + double commanded_vel = cmd->drive.speed; + // calc vesc electric RPM (speed) Float64 erpm_msg; - erpm_msg.data = speed_to_erpm_gain_ * cmd->drive.speed + speed_to_erpm_offset_; + erpm_msg.data = speed_to_erpm_gain_ * commanded_vel + speed_to_erpm_offset_; // calc steering angle (servo) Float64 servo_msg; servo_msg.data = steering_to_servo_gain_ * cmd->drive.steering_angle + steering_to_servo_offset_; - // publish + // brake msg + Float64 brake_msg; + brake_msg.data = 20000; + + // publish (original code) + // if (rclcpp::ok()) { + // erpm_pub_->publish(erpm_msg); + // servo_pub_->publish(servo_msg); + // } + + // publish (new code) if (rclcpp::ok()) { - erpm_pub_->publish(erpm_msg); - servo_pub_->publish(servo_msg); + if (commanded_vel > 0 && current_vel_ > commanded_vel + vel_diff_thresh_) { + brake_pub_->publish(brake_msg); + servo_pub_->publish(servo_msg); + } else if (commanded_vel < 0 && current_vel_ < commanded_vel - vel_diff_thresh_) { + brake_pub_->publish(brake_msg); + servo_pub_->publish(servo_msg); + } else { + erpm_pub_->publish(erpm_msg); + servo_pub_->publish(servo_msg); + } } + +} + +void AckermannToVesc::odomCallback(const nav_msgs::msg::Odometry::SharedPtr odom_msg) +{ + current_vel_ = odom_msg->twist.twist.linear.x; + // RCLCPP_INFO(get_logger(), "Current velocity: %f", current_vel_); } } // namespace vesc_ackermann From 0ba271f65f69fd3037424ff9ecc2bda9aedf8c19 Mon Sep 17 00:00:00 2001 From: jetsonCommit Date: Sat, 17 May 2025 15:19:56 -0700 Subject: [PATCH 07/17] linear scaling braking --- vesc_ackermann/src/ackermann_to_vesc.cpp | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/vesc_ackermann/src/ackermann_to_vesc.cpp b/vesc_ackermann/src/ackermann_to_vesc.cpp index bf5b887..87efe63 100644 --- a/vesc_ackermann/src/ackermann_to_vesc.cpp +++ b/vesc_ackermann/src/ackermann_to_vesc.cpp @@ -68,7 +68,7 @@ AckermannToVesc::AckermannToVesc(const rclcpp::NodeOptions & options) // create brake publisher brake_pub_ = create_publisher("commands/motor/brake", 10); current_vel_ = 0.0; - vel_diff_thresh_ = 0.3; + vel_diff_thresh_ = 0.1; // subscribe to ackermann topic ackermann_sub_ = create_subscription( @@ -94,7 +94,20 @@ void AckermannToVesc::ackermannCmdCallback(const AckermannDriveStamped::SharedPt // brake msg Float64 brake_msg; - brake_msg.data = 20000; + // brake_msg.data = 20000; + //brake_msg.data = 10000; + + // We wanto linearly increase the brake value from 0 to 20000 + // if the commanded velocity is less than the current velocity + // The brake at at 0.5 difference between the commanded and current velocity should be 10000 + // The brake at at 1.0 difference between the commanded and current velocity should be 20000 + + brake_msg.data = (current_vel_ - commanded_vel) * 20000; + if (brake_msg.data < 0) { + brake_msg.data = 0; + } else if (brake_msg.data > 20000) { + brake_msg.data = 20000; + } // publish (original code) // if (rclcpp::ok()) { From 82920af3f176ae815dc2ae4e8975fc0262a144af Mon Sep 17 00:00:00 2001 From: Sanjot Bains <63271005+sanjotbains@users.noreply.github.com> Date: Sun, 18 May 2025 10:11:37 -0700 Subject: [PATCH 08/17] Add Traxxas motor config file --- velineon3500_HFI.xml | 217 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 217 insertions(+) create mode 100644 velineon3500_HFI.xml diff --git a/velineon3500_HFI.xml b/velineon3500_HFI.xml new file mode 100644 index 0000000..326f7ad --- /dev/null +++ b/velineon3500_HFI.xml @@ -0,0 +1,217 @@ + + + 3 + 1 + 0 + 2 + 0 + 99 + -71 + 99 + -60 + 1 + 0.005 + 150 + -100000 + 100000 + 0.8 + 300 + 1500 + 8 + 57 + 10.2 + 9 + 1000 + 1100 + 0 + 85 + 100 + 85 + 100 + 0.15 + 0.005 + 0.95 + 1.5e+06 + -1.5e+06 + 1 + 1 + 1 + 150 + 1100 + 10 + 62 + 0.8 + 80000 + 600 + -1 + 1 + 3 + 2 + 5 + 6 + 4 + -1 + 2000 + 0.00264233 + 6.61179 + 30000 + 0.12 + 0 + 180 + 7 + 3 + 2000 + 30000 + 2.64233e-06 + 2.69788e-07 + 0.00661179 + 0.000707035 + 2.00041e+09 + 0.05 + -1 + 50 + 1000 + 1 + 2500 + 1400 + 0 + 0.9 + 0.2 + 0.1 + 0 + 0.1 + 0.05 + 0 + -1 + 255 + 255 + 255 + 255 + 255 + 255 + 255 + 255 + 500 + 2500 + 4000 + 0 + 0 + 0 + 0 + 0 + 25 + 0.1 + 0 + 3 + 10 + 6 + 8 + 0.3 + 0.15 + 0 + 3000 + 5 + 0.001 + 1 + 1 + 2048.78 + 2049.13 + 2048.43 + -0.0004 + 0.0001 + 0.0002 + 0 + 0 + 0 + 1 + 1 + 4000 + 0 + 0 + 0.9 + 0.02 + 0.2 + 0 + 0 + 5 + 0.002 + 0.0015 + 0.0002 + 0.2 + 900 + 1 + 25000 + 0 + 0.025 + 0 + 0 + 0 + 0.00035 + 0.2 + 1 + 0 + 0.01 + 0.05 + 0.0046 + 0.04 + 500 + 0.02 + 0.5 + 8192 + 1 + 1 + 1.65 + 1.65 + 0.5 + 0 + 0 + 0 + 0 + 16 + 3000 + 35000 + 25000 + 3380 + 0 + 0 + 0.61 + 10000 + 25 + 3 + 45 + 2 + 1.459 + 0.11 + 0 + 3 + 5 + 1 + Traxxas + Velineon 3500 + 248 + 0 + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'Roboto'; font-size:12pt; font-weight:400; font-style:normal;"> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">A motor description can be edited here.</p></body></html> + 0 + 0 + 0 + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'Roboto'; font-size:12pt; font-weight:400; font-style:normal;"> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Some comments about the motor quality. Images can be added as well.</p></body></html> + 1 + 3 + 45 + 65 + 0.05 + 0 + 2.9 + 2.5 + 4.2 + 4.3 + 0 + From e8f4d017989e1c97bfb911f4a736083bba827a29 Mon Sep 17 00:00:00 2001 From: Sanjot Bains <63271005+sanjotbains@users.noreply.github.com> Date: Sun, 18 May 2025 10:15:47 -0700 Subject: [PATCH 09/17] Update ackermann_to_vesc.cpp Removed old code --- vesc_ackermann/src/ackermann_to_vesc.cpp | 16 ---------------- 1 file changed, 16 deletions(-) diff --git a/vesc_ackermann/src/ackermann_to_vesc.cpp b/vesc_ackermann/src/ackermann_to_vesc.cpp index a9471c6..87efe63 100644 --- a/vesc_ackermann/src/ackermann_to_vesc.cpp +++ b/vesc_ackermann/src/ackermann_to_vesc.cpp @@ -88,22 +88,6 @@ void AckermannToVesc::ackermannCmdCallback(const AckermannDriveStamped::SharedPt Float64 erpm_msg; erpm_msg.data = speed_to_erpm_gain_ * commanded_vel + speed_to_erpm_offset_; - // calc vesc current/brake (acceleration) - bool is_positive_accel = true; - std_msgs::Float64::Ptr current_msg(new std_msgs::Float64); - std_msgs::Float64::Ptr brake_msg(new std_msgs::Float64); - current_msg->data = 0; - brake_msg->data = 0; - if (cmd->drive.acceleration < 0) - { - brake_msg->data = accel_to_brake_gain_ * cmd->drive.acceleration; - is_positive_accel = false; - } - else - { - current_msg->data = accel_to_current_gain_ * cmd->drive.acceleration; - } - // calc steering angle (servo) Float64 servo_msg; servo_msg.data = steering_to_servo_gain_ * cmd->drive.steering_angle + steering_to_servo_offset_; From eb4f783983aaba9c809c1141db73284c87b7a1ca Mon Sep 17 00:00:00 2001 From: Sanjot Bains <63271005+sanjotbains@users.noreply.github.com> Date: Sun, 18 May 2025 10:16:57 -0700 Subject: [PATCH 10/17] Update README.md --- README.md | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 09f8afc..24d00bb 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -# Veddar VESC Interface +# Ray-Quasar: Veddar VESC Interface ![ROS2 CI Workflow](https://github.com/f1tenth/vesc/workflows/ROS2%20CI%20Workflow/badge.svg) @@ -6,6 +6,11 @@ Packages to interface with Veddar VESC motor controllers. See https://vesc-proje This is a ROS2 implementation of the ROS1 driver using the new serial driver located in [transport drivers](https://github.com/ros-drivers/transport_drivers). +## Ackermann to VESC Node has been reconfigured to publish braking messages. +This functionality existed in the ROS1 version of this package but was not included in the port to ROS2. I have added it back in, and improved it somewhat. + +If you are an F1Tenth/Roboracer team using Traxxas' Velineon 3351R 3500 BLDC motor [this motor configuration XML](https://github.com/ray-quasar/vesc/blob/main/velineon3500_HFI.xml) will get you HFI for smoother starts as well. + ## How to test 1. Clone this repository and [transport drivers](https://github.com/ros-drivers/transport_drivers) into `src`. From 0c6a36223f0411c9c897b11755a207ecd217ef43 Mon Sep 17 00:00:00 2001 From: jetsonCommit Date: Thu, 22 May 2025 20:03:11 -0700 Subject: [PATCH 11/17] parametrizing vel_diff_thresh --- vesc_ackermann/src/ackermann_to_vesc.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/vesc_ackermann/src/ackermann_to_vesc.cpp b/vesc_ackermann/src/ackermann_to_vesc.cpp index 87efe63..8a8cc12 100644 --- a/vesc_ackermann/src/ackermann_to_vesc.cpp +++ b/vesc_ackermann/src/ackermann_to_vesc.cpp @@ -53,7 +53,8 @@ AckermannToVesc::AckermannToVesc(const rclcpp::NodeOptions & options) declare_parameter("speed_to_erpm_offset", 0.0); declare_parameter("steering_angle_to_servo_gain", 0.0); declare_parameter("steering_angle_to_servo_offset", 0.0); - + declare_parameter("vel_diff_thresh", 0.1); + // get conversion parameters speed_to_erpm_gain_ = get_parameter("speed_to_erpm_gain").get_value(); speed_to_erpm_offset_ = get_parameter("speed_to_erpm_offset").get_value(); @@ -65,10 +66,12 @@ AckermannToVesc::AckermannToVesc(const rclcpp::NodeOptions & options) erpm_pub_ = create_publisher("commands/motor/speed", 10); servo_pub_ = create_publisher("commands/servo/position", 10); + // get vel diff parameter + vel_diff_thresh_ = get_parameter("vel_diff_thresh").get_value(); + // create brake publisher brake_pub_ = create_publisher("commands/motor/brake", 10); current_vel_ = 0.0; - vel_diff_thresh_ = 0.1; // subscribe to ackermann topic ackermann_sub_ = create_subscription( From 9844544a0ca504ef7bf9f5d043c428ca6a583f1f Mon Sep 17 00:00:00 2001 From: jetsonCommit Date: Thu, 22 May 2025 23:13:50 -0700 Subject: [PATCH 12/17] making the ackermann to vesc conversion more robust --- .../vesc_ackermann/ackermann_to_vesc.hpp | 6 +- vesc_ackermann/src/ackermann_to_vesc.cpp | 388 +++++++++++++----- 2 files changed, 301 insertions(+), 93 deletions(-) diff --git a/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.hpp b/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.hpp index b5bd4a0..fb36b94 100644 --- a/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.hpp +++ b/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.hpp @@ -50,10 +50,12 @@ class AckermannToVesc : public rclcpp::Node private: // ROS parameters + bool previous_mode_speed_ = true; // conversion gain and offset double speed_to_erpm_gain_, speed_to_erpm_offset_; double steering_to_servo_gain_, steering_to_servo_offset_; - double current_vel_, vel_diff_thresh_; + double current_vel_, brake_deadzone_; + double accel_to_current_gain_, accel_to_brake_gain_; /** @todo consider also providing an interpolated look-up table conversion */ @@ -61,6 +63,8 @@ class AckermannToVesc : public rclcpp::Node rclcpp::Publisher::SharedPtr erpm_pub_; rclcpp::Publisher::SharedPtr servo_pub_; rclcpp::Publisher::SharedPtr brake_pub_; + rclcpp::Publisher::SharedPtr current_pub_; + rclcpp::Subscription::SharedPtr odom_sub_; rclcpp::Subscription::SharedPtr ackermann_sub_; diff --git a/vesc_ackermann/src/ackermann_to_vesc.cpp b/vesc_ackermann/src/ackermann_to_vesc.cpp index 8a8cc12..5ac8544 100644 --- a/vesc_ackermann/src/ackermann_to_vesc.cpp +++ b/vesc_ackermann/src/ackermann_to_vesc.cpp @@ -31,7 +31,7 @@ #include "vesc_ackermann/ackermann_to_vesc.hpp" #include -#include +#include #include #include @@ -40,108 +40,312 @@ namespace vesc_ackermann { -using ackermann_msgs::msg::AckermannDriveStamped; -using nav_msgs::msg::Odometry; -using std::placeholders::_1; -using std_msgs::msg::Float64; + using ackermann_msgs::msg::AckermannDriveStamped; + using nav_msgs::msg::Odometry; + using std::placeholders::_1; + using std_msgs::msg::Float64; -AckermannToVesc::AckermannToVesc(const rclcpp::NodeOptions & options) -: Node("ackermann_to_vesc_node", options) -{ - // declare parameters - declare_parameter("speed_to_erpm_gain", 0.0); - declare_parameter("speed_to_erpm_offset", 0.0); - declare_parameter("steering_angle_to_servo_gain", 0.0); - declare_parameter("steering_angle_to_servo_offset", 0.0); - declare_parameter("vel_diff_thresh", 0.1); + AckermannToVesc::AckermannToVesc(const rclcpp::NodeOptions &options) + : Node("ackermann_to_vesc_node", options) + { + // Declare parameters, initialized with default 0 + // These four are what we started with: + declare_parameter("speed_to_erpm_gain", 0.0); + declare_parameter("speed_to_erpm_offset", 0.0); + declare_parameter("steering_angle_to_servo_gain", 0.0); + declare_parameter("steering_angle_to_servo_offset", 0.0); - // get conversion parameters - speed_to_erpm_gain_ = get_parameter("speed_to_erpm_gain").get_value(); - speed_to_erpm_offset_ = get_parameter("speed_to_erpm_offset").get_value(); - steering_to_servo_gain_ = get_parameter("steering_angle_to_servo_gain").get_value(); - steering_to_servo_offset_ = get_parameter("steering_angle_to_servo_offset").get_value(); + // I added this one (originally just a regular variable) as part of the first braking test: + // Renaming to brake_deadzone + declare_parameter("brake_deadzone", 0.1); + + // I am adding these two in as part of the conversion to acceleration based control: + declare_parameter("accel_to_current_gain", 0.0); + declare_parameter("accel_to_brake_gain", 0.0) + // Get conversion parameters from config file, in our case 'vesc.yaml' + // Originals: + speed_to_erpm_gain_ = get_parameter("speed_to_erpm_gain").get_value(); + speed_to_erpm_offset_ = get_parameter("speed_to_erpm_offset").get_value(); + steering_to_servo_gain_ = get_parameter("steering_angle_to_servo_gain").get_value(); + steering_to_servo_offset_ = get_parameter("steering_angle_to_servo_offset").get_value(); - // create publishers to vesc electric-RPM (speed) and servo commands - erpm_pub_ = create_publisher("commands/motor/speed", 10); - servo_pub_ = create_publisher("commands/servo/position", 10); + // Braking Test: + brake_deadzone_ = get_parameter("brake_deadzone").get_value(); - // get vel diff parameter - vel_diff_thresh_ = get_parameter("vel_diff_thresh").get_value(); + // Acceleration Test: + accel_to_current_gain_ = get_parameter("accel_to_current_gain").get_value(); + accel_to_brake_gain_ = get_parameter("accel_to_brake_gain").get_value(); - // create brake publisher - brake_pub_ = create_publisher("commands/motor/brake", 10); - current_vel_ = 0.0; + // Create publishers to VESC electric-RPM (speed) and servo commands + // The ERPM publisher should only be used if the acceleration parameter is 0 + erpm_pub_ = create_publisher("commands/motor/speed", 10); + servo_pub_ = create_publisher("commands/servo/position", 10); - // subscribe to ackermann topic - ackermann_sub_ = create_subscription( - "ackermann_cmd", 10, std::bind(&AckermannToVesc::ackermannCmdCallback, this, _1)); + // Create brake publisher + brake_pub_ = create_publisher("commands/motor/brake", 10); + // Initialize current velocity storage variable + current_vel_ = 0.0; - // subscribe to odom topic - odom_sub_ = create_subscription( - "odom", 10, std::bind(&AckermannToVesc::odomCallback, this, _1)); + // Create current publisher + current_pub_ = create_publisher("commands/motor/current", 10); -} + // Subscribe to Ackermann topic + // This is what supplies the input information for the rest of the code. + // I don't care to change the disparityExtender code, so we'll just work with the speed commands therein. + ackermann_sub_ = create_subscription( + "ackermann_cmd", 10, std::bind(&AckermannToVesc::ackermannCmdCallback, this, _1)); -void AckermannToVesc::ackermannCmdCallback(const AckermannDriveStamped::SharedPtr cmd) -{ - double commanded_vel = cmd->drive.speed; - - // calc vesc electric RPM (speed) - Float64 erpm_msg; - erpm_msg.data = speed_to_erpm_gain_ * commanded_vel + speed_to_erpm_offset_; - - // calc steering angle (servo) - Float64 servo_msg; - servo_msg.data = steering_to_servo_gain_ * cmd->drive.steering_angle + steering_to_servo_offset_; - - // brake msg - Float64 brake_msg; - // brake_msg.data = 20000; - //brake_msg.data = 10000; - - // We wanto linearly increase the brake value from 0 to 20000 - // if the commanded velocity is less than the current velocity - // The brake at at 0.5 difference between the commanded and current velocity should be 10000 - // The brake at at 1.0 difference between the commanded and current velocity should be 20000 - - brake_msg.data = (current_vel_ - commanded_vel) * 20000; - if (brake_msg.data < 0) { - brake_msg.data = 0; - } else if (brake_msg.data > 20000) { - brake_msg.data = 20000; - } - - // publish (original code) - // if (rclcpp::ok()) { - // erpm_pub_->publish(erpm_msg); - // servo_pub_->publish(servo_msg); - // } - - // publish (new code) - if (rclcpp::ok()) { - if (commanded_vel > 0 && current_vel_ > commanded_vel + vel_diff_thresh_) { - brake_pub_->publish(brake_msg); - servo_pub_->publish(servo_msg); - } else if (commanded_vel < 0 && current_vel_ < commanded_vel - vel_diff_thresh_) { - brake_pub_->publish(brake_msg); - servo_pub_->publish(servo_msg); - } else { - erpm_pub_->publish(erpm_msg); - servo_pub_->publish(servo_msg); - } - } - -} - -void AckermannToVesc::odomCallback(const nav_msgs::msg::Odometry::SharedPtr odom_msg) -{ - current_vel_ = odom_msg->twist.twist.linear.x; - // RCLCPP_INFO(get_logger(), "Current velocity: %f", current_vel_); -} + // Subscribe to odom topic + // Because we are just listening to the speed commands, we need a tracker of the existing velocity + odom_sub_ = create_subscription( + "odom", 10, std::bind(&AckermannToVesc::odomCallback, this, _1)); + } + + void AckermannToVesc::ackermannCmdCallback(const AckermannDriveStamped::SharedPtr cmd) + { + // Calculate steering angle (servo) + Float64 servo_msg; + servo_msg.data = steering_to_servo_gain_ * cmd->drive.steering_angle + steering_to_servo_offset_; + + // Calculate BLDC command + // Case 1: We are supplied the acceleration-to-current gain and acceleration-to-brake gain + if (accel_to_current_gain_ != 0 && accel_to_brake_gain_ != 0) + { + bool is_positive_accel = true; + // 1.1 We have an incoming acceleration command. + if (cmd->drive.acceleration != 0) + { + if (cmd->drive.acceleration < 0) + { + // Apply controlled braking + Float64 brake_msg; + brake_msg.data = accel_to_brake_gain_ * cmd->drive.acceleration; + is_positive_accel = false; + } else + { + // Apply motor current (throttle) + Float64 current_msg; + current_msg.data = accel_to_current_gain_ * cmd->drive.acceleration; + } + previous_mode_speed_ = false; + } + // 1.2 We do not have an incoming acceleration command. We have an incoming velocity command. + else if (erpm_msg.data != 0 || previous_mode_speed_) { + // Calculate the required acceleration: + // TODO: Calculate dt + double acceleration = (commanded_vel - current_vel_) / dt; // Desired acceleration + if (acceleration > 0) + { + // Apply motor current (throttle) + Float64 current_msg; + current_msg.data = acceleration * acceleration_to_current_gain; + } else + { + // Apply controlled braking + Float64 brake_msg; + brake_msg.data = -acceleration * deceleration_to_brake_gain; + is_positive_accel = false; + } + previous_mode_speed_ = true; + } + } + // Case 2: If the acceleration-to-current gain and acceleration-to-brake gain is 0, then we operate entirely with the velocity message + else if (accel_to_current_gain_ == 0 && accel_to_brake_gain_ == 0) + { + double vel_diff = current_vel_ - commanded_vel; + // 2.1 The commanded velocity has increased: + if (vel_diff < 0) + { + // Calculate the ERPM using the speed-to-ERPM gain and offset: + Float64 erpm_msg; + erpm_msg.data = speed_to_erpm_gain_ * commanded_vel + speed_to_erpm_offset_; + } + // 2.2 The commanded velocity has decreased: + if (vel_diff > 0) + { + // Calculate the brake value: + brake_msg.data = (vel_diff) * 20000; // Going to update this to a sigmoid probably + brake_msg.data = std::clamp(brake_msg.data, 0.0, 20000.0); + is_positive_accel_ = false; + } + } + + // Publish motor commands: + if (rclcpp::ok()) { + if !(is_positive_accel) { + // Publish brake + brake_pub_->publish(brake_msg); + } + else { + // Need to check whether we are operating based on ERPM or accel + if (/*accel*/) { + current_pub_->publish(current_msg); + } + else if (/*ERPM*/) { + erpm_pub_->publish(erpm_msg); + } + } + // Publish servo position + servo_pub_->publish(servo_msg); + } + + // @todo Brake sigmoid + // @todo Brake hysterisis + + + // // calc vesc current/brake (acceleration) + + + // if (cmd->drive.acceleration < 0) + // { + // brake_msg.data = accel_to_brake_gain_ * cmd->drive.acceleration; + // is_positive_accel = false; + // } + // else + // { + // current_msg.data = accel_to_current_gain_ * cmd->drive.acceleration; + // } + + + // // publish + // if (rclcpp::ok()) + // { + // // The below code attempts to stick to the previous mode until a new message forces a mode switch. + // if (erpm_msg.data != 0 || previous_mode_speed_) + // { + // erpm_pub_->publish(erpm_msg); + // } + // else + // { + // if (is_positive_accel) + // { + // current_pub_->publish(current_msg); + // } + // else + // { + // brake_pub_->publish(brake_msg); + // } + // } + // servo_pub_->publish(servo_msg); + } + + // The lines below are to determine which mode we are in so we can hold until new messages force a switch. + // if (erpm_msg.data != 0) + // { + // previous_mode_speed_ = true; + // } + // else if (current_msg.data != 0 || brake_msg.data != 0) + // { + // previous_mode_speed_ = false; + // } + + // brake msg + // Float64 brake_msg; + // brake_msg.data = 20000; + // brake_msg.data = 10000; + + // We wanto linearly increase the brake value from 0 to 20000 + // if the commanded velocity is less than the current velocity + // The brake at at 0.5 difference between the commanded and current velocity should be 10000 + // The brake at at 1.0 difference between the commanded and current velocity should be 20000 + + // brake_msg.data = (current_vel_ - commanded_vel) * 20000; + // if (brake_msg.data < 0) { + // brake_msg.data = 0; + // } else if (brake_msg.data > 20000) { + // brake_msg.data = 20000; + // } + + // publish (original code) + // if (rclcpp::ok()) { + // erpm_pub_->publish(erpm_msg); + // servo_pub_->publish(servo_msg); + // } + + // publish (new code) + // if (rclcpp::ok()) { + // if (commanded_vel > 0 && current_vel_ > commanded_vel + vel_diff_thresh_) { + // brake_pub_->publish(brake_msg); + // servo_pub_->publish(servo_msg); + // } else if (commanded_vel < 0 && current_vel_ < commanded_vel - vel_diff_thresh_) { + // brake_pub_->publish(brake_msg); + // servo_pub_->publish(servo_msg); + // } else { + // erpm_pub_->publish(erpm_msg); + // servo_pub_->publish(servo_msg); + // } + // } + + /* + // calc vesc electric RPM (speed) + std_msgs::Float64::Ptr erpm_msg(new std_msgs::Float64); + erpm_msg->data = speed_to_erpm_gain_ * cmd->drive.speed + speed_to_erpm_offset_; + + // calc vesc current/brake (acceleration) + bool is_positive_accel = true; + std_msgs::Float64::Ptr current_msg(new std_msgs::Float64); + std_msgs::Float64::Ptr brake_msg(new std_msgs::Float64); + current_msg->data = 0; + brake_msg->data = 0; + if (cmd->drive.acceleration < 0) + { + brake_msg->data = accel_to_brake_gain_ * cmd->drive.acceleration; + is_positive_accel = false; + } + else + { + current_msg->data = accel_to_current_gain_ * cmd->drive.acceleration; + } + + // calc steering angle (servo) + std_msgs::Float64::Ptr servo_msg(new std_msgs::Float64); + servo_msg->data = steering_to_servo_gain_ * cmd->drive.steering_angle + steering_to_servo_offset_; + + + // publish + if (ros::ok()) + { + // The below code attempts to stick to the previous mode until a new message forces a mode switch. + if (erpm_msg->data != 0 || previous_mode_speed_) + { + erpm_pub_.publish(erpm_msg); + } + else + { + if (is_positive_accel) + { + current_pub_.publish(current_msg); + } + else + { + brake_pub_.publish(brake_msg); + } + } + servo_pub_.publish(servo_msg); + } + + // The lines below are to determine which mode we are in so we can hold until new messages force a switch. + if (erpm_msg->data != 0) + { + previous_mode_speed_ = true; + } + else if (current_msg->data != 0 || brake_msg->data != 0) + { + previous_mode_speed_ = false; + } + */ + } + + void AckermannToVesc::odomCallback(const nav_msgs::msg::Odometry::SharedPtr odom_msg) + { + // Everytime there is a new publish to /odom, we update the current velocity + current_vel_ = odom_msg->twist.twist.linear.x; + // RCLCPP_INFO(get_logger(), "Current velocity: %f", current_vel_); + } -} // namespace vesc_ackermann +} // namespace vesc_ackermann -#include "rclcpp_components/register_node_macro.hpp" // NOLINT +#include "rclcpp_components/register_node_macro.hpp" // NOLINT RCLCPP_COMPONENTS_REGISTER_NODE(vesc_ackermann::AckermannToVesc) From 5f4527c09aaf5affcb97700b72c75890a31f2931 Mon Sep 17 00:00:00 2001 From: jetsonCommit Date: Fri, 23 May 2025 17:50:06 -0700 Subject: [PATCH 13/17] REWROTE THE WHOLE THING< RAWDOGGED IT NO LLM JSUT STRAIGHT ZEN MODE --- .../vesc_ackermann/ackermann_to_vesc.hpp | 9 ++- vesc_ackermann/src/ackermann_to_vesc.cpp | 71 +++++++++++-------- 2 files changed, 49 insertions(+), 31 deletions(-) diff --git a/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.hpp b/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.hpp index fb36b94..f64fbdc 100644 --- a/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.hpp +++ b/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.hpp @@ -50,13 +50,20 @@ class AckermannToVesc : public rclcpp::Node private: // ROS parameters - bool previous_mode_speed_ = true; + u_int8_t operation_mode_; + // bool previous_mode_speed_ = true; // conversion gain and offset double speed_to_erpm_gain_, speed_to_erpm_offset_; double steering_to_servo_gain_, steering_to_servo_offset_; double current_vel_, brake_deadzone_; double accel_to_current_gain_, accel_to_brake_gain_; + enum operation_modes { + ACCEL_TO_CURRENT, + VEL_TO_CURRENT, + VEL_TO_ERPM + }; + /** @todo consider also providing an interpolated look-up table conversion */ // ROS services diff --git a/vesc_ackermann/src/ackermann_to_vesc.cpp b/vesc_ackermann/src/ackermann_to_vesc.cpp index 5ac8544..9880ef7 100644 --- a/vesc_ackermann/src/ackermann_to_vesc.cpp +++ b/vesc_ackermann/src/ackermann_to_vesc.cpp @@ -61,7 +61,7 @@ namespace vesc_ackermann // I am adding these two in as part of the conversion to acceleration based control: declare_parameter("accel_to_current_gain", 0.0); - declare_parameter("accel_to_brake_gain", 0.0) + declare_parameter("accel_to_brake_gain", 0.0); // Get conversion parameters from config file, in our case 'vesc.yaml' // Originals: @@ -104,8 +104,19 @@ namespace vesc_ackermann void AckermannToVesc::ackermannCmdCallback(const AckermannDriveStamped::SharedPtr cmd) { - // Calculate steering angle (servo) + // Initialize all messages at the start Float64 servo_msg; + Float64 brake_msg; + Float64 current_msg; + Float64 erpm_msg; + bool is_positive_accel = true; + + // Zero-initialize message data + brake_msg.data = 0.0; + current_msg.data = 0.0; + erpm_msg.data = 0.0; + + // Calculate steering angle (servo) servo_msg.data = steering_to_servo_gain_ * cmd->drive.steering_angle + steering_to_servo_offset_; // Calculate BLDC command @@ -116,49 +127,48 @@ namespace vesc_ackermann // 1.1 We have an incoming acceleration command. if (cmd->drive.acceleration != 0) { + operation_mode_ = ACCEL_TO_CURRENT; // For deciding which message to publish if (cmd->drive.acceleration < 0) { // Apply controlled braking - Float64 brake_msg; brake_msg.data = accel_to_brake_gain_ * cmd->drive.acceleration; is_positive_accel = false; } else { // Apply motor current (throttle) - Float64 current_msg; current_msg.data = accel_to_current_gain_ * cmd->drive.acceleration; } - previous_mode_speed_ = false; + // previous_mode_speed_ = false; } // 1.2 We do not have an incoming acceleration command. We have an incoming velocity command. - else if (erpm_msg.data != 0 || previous_mode_speed_) { + else if (cmd->drive.speed != 0 || operation_mode_ == VEL_TO_CURRENT) { // Calculate the required acceleration: // TODO: Calculate dt - double acceleration = (commanded_vel - current_vel_) / dt; // Desired acceleration + operation_mode_ = VEL_TO_CURRENT; // For deciding which message to publish + double commanded_vel = cmd->drive.speed; + double acceleration = 10 * (commanded_vel - current_vel_); // . / 0.1; // Desired acceleration if (acceleration > 0) { // Apply motor current (throttle) - Float64 current_msg; - current_msg.data = acceleration * acceleration_to_current_gain; + current_msg.data = acceleration * accel_to_current_gain_; } else { // Apply controlled braking - Float64 brake_msg; - brake_msg.data = -acceleration * deceleration_to_brake_gain; + brake_msg.data = -acceleration * accel_to_brake_gain_; is_positive_accel = false; } - previous_mode_speed_ = true; } } // Case 2: If the acceleration-to-current gain and acceleration-to-brake gain is 0, then we operate entirely with the velocity message else if (accel_to_current_gain_ == 0 && accel_to_brake_gain_ == 0) { + operation_mode_ = VEL_TO_ERPM; // For deciding which message to publish + double commanded_vel = cmd->drive.speed; double vel_diff = current_vel_ - commanded_vel; // 2.1 The commanded velocity has increased: if (vel_diff < 0) { // Calculate the ERPM using the speed-to-ERPM gain and offset: - Float64 erpm_msg; erpm_msg.data = speed_to_erpm_gain_ * commanded_vel + speed_to_erpm_offset_; } // 2.2 The commanded velocity has decreased: @@ -167,29 +177,30 @@ namespace vesc_ackermann // Calculate the brake value: brake_msg.data = (vel_diff) * 20000; // Going to update this to a sigmoid probably brake_msg.data = std::clamp(brake_msg.data, 0.0, 20000.0); - is_positive_accel_ = false; + is_positive_accel = false; } } // Publish motor commands: - if (rclcpp::ok()) { - if !(is_positive_accel) { - // Publish brake - brake_pub_->publish(brake_msg); - } - else { - // Need to check whether we are operating based on ERPM or accel - if (/*accel*/) { - current_pub_->publish(current_msg); - } - else if (/*ERPM*/) { - erpm_pub_->publish(erpm_msg); + if (rclcpp::ok()) { + if (!is_positive_accel) { + if (brake_msg.data != 0.0) { // Only publish if we actually set a brake value + brake_pub_->publish(brake_msg); + } + } else { + if (operation_mode_ == ACCEL_TO_CURRENT || operation_mode_ == VEL_TO_CURRENT) { + if (current_msg.data != 0.0) { + current_pub_->publish(current_msg); + } + } else if (operation_mode_ == VEL_TO_ERPM) { + if (erpm_msg.data != 0.0) { + erpm_pub_->publish(erpm_msg); + } } } - // Publish servo position servo_pub_->publish(servo_msg); } - + } // @todo Brake sigmoid // @todo Brake hysterisis @@ -228,7 +239,7 @@ namespace vesc_ackermann // } // } // servo_pub_->publish(servo_msg); - } + // } // The lines below are to determine which mode we are in so we can hold until new messages force a switch. // if (erpm_msg.data != 0) @@ -335,7 +346,7 @@ namespace vesc_ackermann previous_mode_speed_ = false; } */ - } + // } void AckermannToVesc::odomCallback(const nav_msgs::msg::Odometry::SharedPtr odom_msg) { From e55dfabfa111f192949fe2a1474c9efc414abcd5 Mon Sep 17 00:00:00 2001 From: jetsonCommit Date: Sun, 25 May 2025 23:55:02 -0700 Subject: [PATCH 14/17] braking sigmoid curve, parameterized functions, configurable from launch YAML --- .../vesc_ackermann/ackermann_to_vesc.hpp | 1 + vesc_ackermann/src/ackermann_to_vesc.cpp | 22 ++++++++++++++++--- 2 files changed, 20 insertions(+), 3 deletions(-) diff --git a/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.hpp b/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.hpp index f64fbdc..676e5ae 100644 --- a/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.hpp +++ b/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.hpp @@ -54,6 +54,7 @@ class AckermannToVesc : public rclcpp::Node // bool previous_mode_speed_ = true; // conversion gain and offset double speed_to_erpm_gain_, speed_to_erpm_offset_; + double speed_to_braking_gain_, speed_to_braking_center_, speed_to_braking_max_, speed_to_braking_min_; double steering_to_servo_gain_, steering_to_servo_offset_; double current_vel_, brake_deadzone_; double accel_to_current_gain_, accel_to_brake_gain_; diff --git a/vesc_ackermann/src/ackermann_to_vesc.cpp b/vesc_ackermann/src/ackermann_to_vesc.cpp index 9880ef7..91f924b 100644 --- a/vesc_ackermann/src/ackermann_to_vesc.cpp +++ b/vesc_ackermann/src/ackermann_to_vesc.cpp @@ -58,7 +58,13 @@ namespace vesc_ackermann // I added this one (originally just a regular variable) as part of the first braking test: // Renaming to brake_deadzone declare_parameter("brake_deadzone", 0.1); - + + // Adding braking parameters for VEL_TO_ERPM mode + declare_parameter("speed_to_braking_gain", 0.0); + declare_parameter("speed_to_braking_center", 0.0); + declare_parameter("speed_to_braking_max", 20000.0); + declare_parameter("speed_to_braking_min", 0.0); + // I am adding these two in as part of the conversion to acceleration based control: declare_parameter("accel_to_current_gain", 0.0); declare_parameter("accel_to_brake_gain", 0.0); @@ -73,6 +79,12 @@ namespace vesc_ackermann // Braking Test: brake_deadzone_ = get_parameter("brake_deadzone").get_value(); + // Braking parameters for VEL_TO_ERPM mode + speed_to_braking_gain_ = get_parameter("speed_to_braking_gain").get_value(); + speed_to_braking_center_ = get_parameter("speed_to_braking_center").get_value(); + speed_to_braking_max_ = get_parameter("speed_to_braking_max").get_value(); + speed_to_braking_min_ = get_parameter("speed_to_braking_min").get_value(); + // Acceleration Test: accel_to_current_gain_ = get_parameter("accel_to_current_gain").get_value(); accel_to_brake_gain_ = get_parameter("accel_to_brake_gain").get_value(); @@ -175,8 +187,12 @@ namespace vesc_ackermann if (vel_diff > 0) { // Calculate the brake value: - brake_msg.data = (vel_diff) * 20000; // Going to update this to a sigmoid probably - brake_msg.data = std::clamp(brake_msg.data, 0.0, 20000.0); + // brake_msg.data = (vel_diff) * speed_to_braking_gain_ + speed_to_braking_center_; + brake_msg.data = ( + (speed_to_braking_max_) + / (1 + exp(- speed_to_braking_gain_ * (vel_diff - speed_to_braking_center_))) + ); + // brake_msg.data = std::clamp(brake_msg.data, 0.0, 20000.0); is_positive_accel = false; } } From 8bcca1984e5527495b770dab245fb8c2839a2ea6 Mon Sep 17 00:00:00 2001 From: jetsonCommit Date: Sun, 1 Jun 2025 14:16:35 -0700 Subject: [PATCH 15/17] psuedocode for initial acceleration handling, fixing reverse --- vesc_ackermann/src/ackermann_to_vesc.cpp | 164 +++-------------------- 1 file changed, 19 insertions(+), 145 deletions(-) diff --git a/vesc_ackermann/src/ackermann_to_vesc.cpp b/vesc_ackermann/src/ackermann_to_vesc.cpp index 91f924b..e6300a5 100644 --- a/vesc_ackermann/src/ackermann_to_vesc.cpp +++ b/vesc_ackermann/src/ackermann_to_vesc.cpp @@ -176,6 +176,25 @@ namespace vesc_ackermann { operation_mode_ = VEL_TO_ERPM; // For deciding which message to publish double commanded_vel = cmd->drive.speed; + + // Case A: We have a postive commanded velocity: + // I. The commanded velocity is greater than the current velocity + // If the current velocity is zero we need to go slow to get a rotor position reading before we can accelerate + // Send a (0.5 m/s) ERPM message + + // II. The commanded velocity is less than the current velocity + // Apply the BRAKE + + // Case B: We have a negative commanded velocity: + // I. The commanded velocity is less (faster) than the current velocity + // If the current velocity is zero we need to go slow to get a rotor position reading + // Send a (-0.5 m/s) ERPM message + + // II. The commanded velocity is greater (slower) than the current velocity + // Apply the BRAKE + + // ----- OLD CODE ------ + double vel_diff = current_vel_ - commanded_vel; // 2.1 The commanded velocity has increased: if (vel_diff < 0) @@ -217,152 +236,7 @@ namespace vesc_ackermann servo_pub_->publish(servo_msg); } } - // @todo Brake sigmoid - // @todo Brake hysterisis - - - // // calc vesc current/brake (acceleration) - - - // if (cmd->drive.acceleration < 0) - // { - // brake_msg.data = accel_to_brake_gain_ * cmd->drive.acceleration; - // is_positive_accel = false; - // } - // else - // { - // current_msg.data = accel_to_current_gain_ * cmd->drive.acceleration; - // } - - - // // publish - // if (rclcpp::ok()) - // { - // // The below code attempts to stick to the previous mode until a new message forces a mode switch. - // if (erpm_msg.data != 0 || previous_mode_speed_) - // { - // erpm_pub_->publish(erpm_msg); - // } - // else - // { - // if (is_positive_accel) - // { - // current_pub_->publish(current_msg); - // } - // else - // { - // brake_pub_->publish(brake_msg); - // } - // } - // servo_pub_->publish(servo_msg); - // } - - // The lines below are to determine which mode we are in so we can hold until new messages force a switch. - // if (erpm_msg.data != 0) - // { - // previous_mode_speed_ = true; - // } - // else if (current_msg.data != 0 || brake_msg.data != 0) - // { - // previous_mode_speed_ = false; - // } - - // brake msg - // Float64 brake_msg; - // brake_msg.data = 20000; - // brake_msg.data = 10000; - - // We wanto linearly increase the brake value from 0 to 20000 - // if the commanded velocity is less than the current velocity - // The brake at at 0.5 difference between the commanded and current velocity should be 10000 - // The brake at at 1.0 difference between the commanded and current velocity should be 20000 - - // brake_msg.data = (current_vel_ - commanded_vel) * 20000; - // if (brake_msg.data < 0) { - // brake_msg.data = 0; - // } else if (brake_msg.data > 20000) { - // brake_msg.data = 20000; - // } - - // publish (original code) - // if (rclcpp::ok()) { - // erpm_pub_->publish(erpm_msg); - // servo_pub_->publish(servo_msg); - // } - - // publish (new code) - // if (rclcpp::ok()) { - // if (commanded_vel > 0 && current_vel_ > commanded_vel + vel_diff_thresh_) { - // brake_pub_->publish(brake_msg); - // servo_pub_->publish(servo_msg); - // } else if (commanded_vel < 0 && current_vel_ < commanded_vel - vel_diff_thresh_) { - // brake_pub_->publish(brake_msg); - // servo_pub_->publish(servo_msg); - // } else { - // erpm_pub_->publish(erpm_msg); - // servo_pub_->publish(servo_msg); - // } - // } - - /* - // calc vesc electric RPM (speed) - std_msgs::Float64::Ptr erpm_msg(new std_msgs::Float64); - erpm_msg->data = speed_to_erpm_gain_ * cmd->drive.speed + speed_to_erpm_offset_; - - // calc vesc current/brake (acceleration) - bool is_positive_accel = true; - std_msgs::Float64::Ptr current_msg(new std_msgs::Float64); - std_msgs::Float64::Ptr brake_msg(new std_msgs::Float64); - current_msg->data = 0; - brake_msg->data = 0; - if (cmd->drive.acceleration < 0) - { - brake_msg->data = accel_to_brake_gain_ * cmd->drive.acceleration; - is_positive_accel = false; - } - else - { - current_msg->data = accel_to_current_gain_ * cmd->drive.acceleration; - } - - // calc steering angle (servo) - std_msgs::Float64::Ptr servo_msg(new std_msgs::Float64); - servo_msg->data = steering_to_servo_gain_ * cmd->drive.steering_angle + steering_to_servo_offset_; - - - // publish - if (ros::ok()) - { - // The below code attempts to stick to the previous mode until a new message forces a mode switch. - if (erpm_msg->data != 0 || previous_mode_speed_) - { - erpm_pub_.publish(erpm_msg); - } - else - { - if (is_positive_accel) - { - current_pub_.publish(current_msg); - } - else - { - brake_pub_.publish(brake_msg); - } - } - servo_pub_.publish(servo_msg); - } - // The lines below are to determine which mode we are in so we can hold until new messages force a switch. - if (erpm_msg->data != 0) - { - previous_mode_speed_ = true; - } - else if (current_msg->data != 0 || brake_msg->data != 0) - { - previous_mode_speed_ = false; - } - */ - // } void AckermannToVesc::odomCallback(const nav_msgs::msg::Odometry::SharedPtr odom_msg) { From a7fa0edbddee3398a13d89ccea7912814369736d Mon Sep 17 00:00:00 2001 From: jetsonCommit Date: Sun, 1 Jun 2025 15:11:30 -0700 Subject: [PATCH 16/17] first successful compile --- vesc_ackermann/src/ackermann_to_vesc.cpp | 133 ++++++++++++++++++----- 1 file changed, 105 insertions(+), 28 deletions(-) diff --git a/vesc_ackermann/src/ackermann_to_vesc.cpp b/vesc_ackermann/src/ackermann_to_vesc.cpp index e6300a5..0108d1a 100644 --- a/vesc_ackermann/src/ackermann_to_vesc.cpp +++ b/vesc_ackermann/src/ackermann_to_vesc.cpp @@ -121,7 +121,8 @@ namespace vesc_ackermann Float64 brake_msg; Float64 current_msg; Float64 erpm_msg; - bool is_positive_accel = true; + bool publish_brake = false; + bool publish_ERPM = false; // Zero-initialize message data brake_msg.data = 0.0; @@ -144,7 +145,7 @@ namespace vesc_ackermann { // Apply controlled braking brake_msg.data = accel_to_brake_gain_ * cmd->drive.acceleration; - is_positive_accel = false; + // is_positive_accel = false; } else { // Apply motor current (throttle) @@ -167,7 +168,7 @@ namespace vesc_ackermann { // Apply controlled braking brake_msg.data = -acceleration * accel_to_brake_gain_; - is_positive_accel = false; + // is_positive_accel = false; } } } @@ -178,51 +179,127 @@ namespace vesc_ackermann double commanded_vel = cmd->drive.speed; // Case A: We have a postive commanded velocity: + if (commanded_vel >= 0) + { // I. The commanded velocity is greater than the current velocity + if (commanded_vel >= current_vel_) + { + // If the current velocity is negative apply the brake + if (current_vel_ < 0) + { + // Calculate the brake value + brake_msg.data = ( + (speed_to_braking_max_) + / (1 + exp(- speed_to_braking_gain_ * (abs(current_vel_ - commanded_vel) - speed_to_braking_center_))) + ); + publish_brake = true; + } // If the current velocity is zero we need to go slow to get a rotor position reading before we can accelerate + else if (current_vel_ == 0 && commanded_vel > 0.5) + { // Send a (0.5 m/s) ERPM message - + // Calculate the ERPM using the speed-to-ERPM gain and offset: + erpm_msg.data = speed_to_erpm_gain_ * 0.5 + speed_to_erpm_offset_; + // Hopefully the compiler will pick this up and pre-evaluate + publish_ERPM = true; + } + // The current velocity is greater than zero + else + { + // Calculate the ERPM using the speed-to-ERPM gain and offset: + erpm_msg.data = speed_to_erpm_gain_ * commanded_vel + speed_to_erpm_offset_; + publish_ERPM = true; + } + } // II. The commanded velocity is less than the current velocity + else if (commanded_vel < current_vel_) + { // Apply the BRAKE + brake_msg.data = ( + (speed_to_braking_max_) + / (1 + exp(- speed_to_braking_gain_ * (abs(current_vel_ - commanded_vel) - speed_to_braking_center_))) + ); + // is_positive_accel = false; + publish_brake = true; + } + } // Case B: We have a negative commanded velocity: + else if (commanded_vel < 0) + { // I. The commanded velocity is less (faster) than the current velocity + if (commanded_vel <= current_vel_) + { + // If the current velocity is positive + if (current_vel_ > 0) + { + // Apply the brake + brake_msg.data = ( + (speed_to_braking_max_) + / (1 + exp(- speed_to_braking_gain_ * (abs(current_vel_ - commanded_vel) - speed_to_braking_center_))) + ); + publish_brake = true; + } // If the current velocity is zero we need to go slow to get a rotor position reading + else if (current_vel_ == 0 && commanded_vel < -0.5) + { // Send a (-0.5 m/s) ERPM message - + erpm_msg.data = speed_to_erpm_gain_ * -0.5 + speed_to_erpm_offset_; + // Hopefully the compiler will pick this up and pre-evaluate + publish_ERPM = true; + } + // The current velocity is negative + else + { + // Calculate the ERPM using the speed-to-ERPM gain and offset: + erpm_msg.data = speed_to_erpm_gain_ * commanded_vel + speed_to_erpm_offset_; + publish_ERPM = true; + } + } // II. The commanded velocity is greater (slower) than the current velocity + else if (commanded_vel > current_vel_) + { // Apply the BRAKE - - // ----- OLD CODE ------ - - double vel_diff = current_vel_ - commanded_vel; - // 2.1 The commanded velocity has increased: - if (vel_diff < 0) - { - // Calculate the ERPM using the speed-to-ERPM gain and offset: - erpm_msg.data = speed_to_erpm_gain_ * commanded_vel + speed_to_erpm_offset_; - } - // 2.2 The commanded velocity has decreased: - if (vel_diff > 0) - { - // Calculate the brake value: - // brake_msg.data = (vel_diff) * speed_to_braking_gain_ + speed_to_braking_center_; - brake_msg.data = ( - (speed_to_braking_max_) - / (1 + exp(- speed_to_braking_gain_ * (vel_diff - speed_to_braking_center_))) - ); - // brake_msg.data = std::clamp(brake_msg.data, 0.0, 20000.0); - is_positive_accel = false; + // Apply the BRAKE + brake_msg.data = ( + (speed_to_braking_max_) + / (1 + exp(- speed_to_braking_gain_ * (abs(current_vel_ - commanded_vel) - speed_to_braking_center_))) + ); + // is_positive_accel = false; + publish_brake = true; + } } } + // ----- OLD CODE ------ + + // double vel_diff = current_vel_ - commanded_vel; + // // 2.1 The commanded velocity has increased: + // if (vel_diff < 0) + // { + // // Calculate the ERPM using the speed-to-ERPM gain and offset: + // erpm_msg.data = speed_to_erpm_gain_ * commanded_vel + speed_to_erpm_offset_; + // } + // // 2.2 The commanded velocity has decreased: + // if (vel_diff > 0) + // { + // // Calculate the brake value: + // // brake_msg.data = (vel_diff) * speed_to_braking_gain_ + speed_to_braking_center_; + // brake_msg.data = ( + // (speed_to_braking_max_) + // / (1 + exp(- speed_to_braking_gain_ * (vel_diff - speed_to_braking_center_))) + // ); + // // brake_msg.data = std::clamp(brake_msg.data, 0.0, 20000.0); + // is_positive_accel = false; + // } + // } // Publish motor commands: if (rclcpp::ok()) { - if (!is_positive_accel) { + if (publish_brake) { if (brake_msg.data != 0.0) { // Only publish if we actually set a brake value brake_pub_->publish(brake_msg); } - } else { + } else if (publish_ERPM) { if (operation_mode_ == ACCEL_TO_CURRENT || operation_mode_ == VEL_TO_CURRENT) { if (current_msg.data != 0.0) { current_pub_->publish(current_msg); From 6107301d54bd3eec56031a1db3db859b3e041201 Mon Sep 17 00:00:00 2001 From: jetsonCommit Date: Mon, 2 Jun 2025 21:36:52 -0700 Subject: [PATCH 17/17] fixed reverse, added smooth start --- vesc_ackermann/src/ackermann_to_vesc.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/vesc_ackermann/src/ackermann_to_vesc.cpp b/vesc_ackermann/src/ackermann_to_vesc.cpp index 0108d1a..2bc42b1 100644 --- a/vesc_ackermann/src/ackermann_to_vesc.cpp +++ b/vesc_ackermann/src/ackermann_to_vesc.cpp @@ -195,11 +195,11 @@ namespace vesc_ackermann publish_brake = true; } // If the current velocity is zero we need to go slow to get a rotor position reading before we can accelerate - else if (current_vel_ == 0 && commanded_vel > 0.5) + else if (current_vel_ < 1 && commanded_vel > 1) { // Send a (0.5 m/s) ERPM message // Calculate the ERPM using the speed-to-ERPM gain and offset: - erpm_msg.data = speed_to_erpm_gain_ * 0.5 + speed_to_erpm_offset_; + erpm_msg.data = speed_to_erpm_gain_ * (current_vel_ + 0.4) + speed_to_erpm_offset_; // Hopefully the compiler will pick this up and pre-evaluate publish_ERPM = true; }