From edc1d2ba0d4dd03af7cb2a0171a927b26c9fa7bb Mon Sep 17 00:00:00 2001 From: Ruixiang Du Date: Mon, 15 Jun 2020 18:51:59 +0800 Subject: [PATCH] updated hunter max steering angle --- .../include/hunter_base/hunter_base.hpp | 2 +- .../include/hunter_base/hunter_types.hpp | 4 ++-- src/hunter_sdk/hunter_base/src/hunter_base.cpp | 14 +++++++------- 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/hunter_sdk/hunter_base/include/hunter_base/hunter_base.hpp b/src/hunter_sdk/hunter_base/include/hunter_base/hunter_base.hpp index ee0ac32..f8604ec 100644 --- a/src/hunter_sdk/hunter_base/include/hunter_base/hunter_base.hpp +++ b/src/hunter_sdk/hunter_base/include/hunter_base/hunter_base.hpp @@ -47,7 +47,7 @@ public: void SetCmdThreadPeriodMs(int32_t period_ms) { cmd_thread_period_ms_ = period_ms; }; // motion control - void SetMotionCommand(double linear_vel, double angular_vel, + void SetMotionCommand(double linear_vel, double steering_angle, HunterMotionCmd::FaultClearFlag fault_clr_flag = HunterMotionCmd::FaultClearFlag::NO_FAULT); // get robot state diff --git a/src/hunter_sdk/hunter_base/include/hunter_base/hunter_types.hpp b/src/hunter_sdk/hunter_base/include/hunter_base/hunter_types.hpp index 7ab420b..c35af4f 100644 --- a/src/hunter_sdk/hunter_base/include/hunter_base/hunter_types.hpp +++ b/src/hunter_sdk/hunter_base/include/hunter_base/hunter_types.hpp @@ -66,8 +66,8 @@ struct HunterMotionCmd static constexpr double max_linear_velocity = 1.5; // 1.5 m/s static constexpr double min_linear_velocity = -1.5; // -1.5 m/s - static constexpr double max_angular_velocity = 0.5235; // 0.5235 rad/s - static constexpr double min_angular_velocity = -0.5235; // -0.5235 rad/s + static constexpr double max_steering_angle = 0.4622; // 0.4622 rad ~= 26.5 degree + static constexpr double min_steering_angle = -0.4622; // -0.4622 rad }; } // namespace wescore diff --git a/src/hunter_sdk/hunter_base/src/hunter_base.cpp b/src/hunter_sdk/hunter_base/src/hunter_base.cpp index 0818978..c040d21 100644 --- a/src/hunter_sdk/hunter_base/src/hunter_base.cpp +++ b/src/hunter_sdk/hunter_base/src/hunter_base.cpp @@ -132,7 +132,7 @@ HunterState HunterBase::GetHunterState() { } void HunterBase::SetMotionCommand( - double linear_vel, double angular_vel, + double linear_vel, double steering_angle, HunterMotionCmd::FaultClearFlag fault_clr_flag) { // make sure cmd thread is started before attempting to send commands if (!cmd_thread_started_) StartCmdThread(); @@ -141,16 +141,16 @@ void HunterBase::SetMotionCommand( linear_vel = HunterMotionCmd::min_linear_velocity; if (linear_vel > HunterMotionCmd::max_linear_velocity) linear_vel = HunterMotionCmd::max_linear_velocity; - if (angular_vel < HunterMotionCmd::min_angular_velocity) - angular_vel = HunterMotionCmd::min_angular_velocity; - if (angular_vel > HunterMotionCmd::max_angular_velocity) - angular_vel = HunterMotionCmd::max_angular_velocity; + if (steering_angle < HunterMotionCmd::min_steering_angle) + steering_angle = HunterMotionCmd::min_steering_angle; + if (steering_angle > HunterMotionCmd::max_steering_angle) + steering_angle = HunterMotionCmd::max_steering_angle; std::lock_guard guard(motion_cmd_mutex_); current_motion_cmd_.linear_velocity = static_cast( linear_vel / HunterMotionCmd::max_linear_velocity * 100.0); current_motion_cmd_.angular_velocity = static_cast( - angular_vel / HunterMotionCmd::max_angular_velocity * 100.0); + steering_angle / HunterMotionCmd::max_steering_angle * 100.0); current_motion_cmd_.fault_clear_flag = fault_clr_flag; } @@ -229,7 +229,7 @@ void HunterBase::UpdateHunterState(const HunterMessage &status_msg, // std::cout << "motor 1 driver feedback received" << std::endl; const MotorDriverStatusMessage &msg = status_msg.body.motor_driver_status_msg; - for (int i = 0; i < 4; ++i) { + for (int i = 0; i < 3; ++i) { state.motor_states[status_msg.body.motor_driver_status_msg.motor_id] .current = (static_cast(msg.data.status.current.low_byte) |