From 411fbb9a1e7c43c37e52a41d67c7c96ed1fc96cc Mon Sep 17 00:00:00 2001 From: Ruixiang Du Date: Mon, 21 Dec 2020 11:29:48 +0800 Subject: [PATCH] added scout mini support --- README.md | 3 + .../apps/scout_monitor/src/scout_monitor.cpp | 4 +- ugv_sdk/include/ugv_sdk/scout/scout_base.hpp | 5 +- ugv_sdk/include/ugv_sdk/scout/scout_types.hpp | 177 +++++++++--------- ugv_sdk/src/scout_base.cpp | 46 +++-- 5 files changed, 132 insertions(+), 103 deletions(-) diff --git a/README.md b/README.md index 2594560..3e7583b 100755 --- a/README.md +++ b/README.md @@ -15,7 +15,10 @@ This software package provides a C++ interface to communicate with the mobile pl Supported robot platforms * **Scout**: skid-steer mobile base +* **Scout-Mini**: skid-steer mobile base * **Hunter**: ackermann mobile base +* **Tracer**: differential-drive mobile base +* **Bunker**: tracked mobile base Supported environments diff --git a/ugv_sdk/apps/scout_monitor/src/scout_monitor.cpp b/ugv_sdk/apps/scout_monitor/src/scout_monitor.cpp index d11baf5..307d027 100644 --- a/ugv_sdk/apps/scout_monitor/src/scout_monitor.cpp +++ b/ugv_sdk/apps/scout_monitor/src/scout_monitor.cpp @@ -279,7 +279,7 @@ void ScoutMonitor::ShowVehicleState(int y, int x) for (int i = linear_axis_origin_y + 1; i < linear_axis_negative_y; ++i) mvwprintw(body_info_win_, i, linear_axis_x, "-"); mvwprintw(body_info_win_, linear_axis_negative_y, linear_axis_x, "v"); - double linear_percentage = scout_state_.linear_velocity / ScoutMotionCmd::max_linear_velocity; + double linear_percentage = scout_state_.linear_velocity / ScoutCmdLimits::max_linear_velocity; int linear_bars = std::abs(static_cast(linear_percentage * 5)) + 1; if (std::abs(scout_state_.linear_velocity) < 0.001) linear_bars = 0; @@ -317,7 +317,7 @@ void ScoutMonitor::ShowVehicleState(int y, int x) mvwprintw(body_info_win_, angular_axis_y, i, "-"); mvwprintw(body_info_win_, angular_axis_y, angular_axis_positive_x, ">"); - double angular_percentage = scout_state_.angular_velocity / ScoutMotionCmd::max_angular_velocity; + double angular_percentage = scout_state_.angular_velocity / ScoutCmdLimits::max_angular_velocity; int angular_bars = std::abs(static_cast(angular_percentage * 5)) + 1; if (std::abs(scout_state_.angular_velocity) < 0.001) angular_bars = 0; diff --git a/ugv_sdk/include/ugv_sdk/scout/scout_base.hpp b/ugv_sdk/include/ugv_sdk/scout/scout_base.hpp index 81763a7..aba4645 100644 --- a/ugv_sdk/include/ugv_sdk/scout/scout_base.hpp +++ b/ugv_sdk/include/ugv_sdk/scout/scout_base.hpp @@ -26,7 +26,8 @@ namespace westonrobot { class ScoutBase : public MobileBase { public: - ScoutBase() : MobileBase(){}; + ScoutBase(bool is_scout_mini = false) + : MobileBase(), is_scout_mini_(is_scout_mini){}; ~ScoutBase() = default; public: @@ -43,6 +44,8 @@ class ScoutBase : public MobileBase { ScoutState GetScoutState(); private: + bool is_scout_mini_ = false; + // serial port buffer uint8_t tx_cmd_len_; uint8_t tx_buffer_[SCOUT_CMD_BUF_LEN]; diff --git a/ugv_sdk/include/ugv_sdk/scout/scout_types.hpp b/ugv_sdk/include/ugv_sdk/scout/scout_types.hpp index 9fefcec..c13607e 100644 --- a/ugv_sdk/include/ugv_sdk/scout/scout_types.hpp +++ b/ugv_sdk/include/ugv_sdk/scout/scout_types.hpp @@ -1,9 +1,9 @@ -/* +/* * scout_state.hpp - * + * * Created on: Jun 11, 2019 08:48 - * Description: - * + * Description: + * * Copyright (c) 2019 Ruixiang Du (rdu) */ @@ -13,100 +13,105 @@ #include #include -namespace westonrobot -{ -struct ScoutState -{ - enum MotorID - { - FRONT_RIGHT = 0, - FRONT_LEFT = 1, - REAR_LEFT = 2, - REAR_RIGHT = 3 - }; +namespace westonrobot { +struct ScoutState { + enum MotorID { + FRONT_RIGHT = 0, + FRONT_LEFT = 1, + REAR_LEFT = 2, + REAR_RIGHT = 3 + }; - struct MotorState - { - double current = 0; // in A - double rpm = 0; - double temperature = 0; - }; + struct MotorState { + double current = 0; // in A + double rpm = 0; + double temperature = 0; + }; - struct LightState - { - uint8_t mode = 0; - uint8_t custom_value = 0; - }; + struct LightState { + uint8_t mode = 0; + uint8_t custom_value = 0; + }; - // base state - uint8_t base_state = 0; - uint8_t control_mode = 0; - uint16_t fault_code = 0; - double battery_voltage = 0.0; + // base state + uint8_t base_state = 0; + uint8_t control_mode = 0; + uint16_t fault_code = 0; + double battery_voltage = 0.0; - // motor state - static constexpr uint8_t motor_num = 4; - MotorState motor_states[motor_num]; + // motor state + static constexpr uint8_t motor_num = 4; + MotorState motor_states[motor_num]; - // light state - bool light_control_enabled = false; - LightState front_light_state; - LightState rear_light_state; + // light state + bool light_control_enabled = false; + LightState front_light_state; + LightState rear_light_state; - // motion state - double linear_velocity = 0; - double angular_velocity = 0; + // motion state + double linear_velocity = 0; + double angular_velocity = 0; }; -struct ScoutMotionCmd -{ - enum class FaultClearFlag - { - NO_FAULT = 0x00, - BAT_UNDER_VOL = 0x01, - BAT_OVER_VOL = 0x02, - MOTOR1_COMM = 0x03, - MOTOR2_COMM = 0x04, - MOTOR3_COMM = 0x05, - MOTOR4_COMM = 0x06, - MOTOR_DRV_OVERHEAT = 0x07, - MOTOR_OVERCURRENT = 0x08 - }; +struct ScoutMotionCmd { + enum class FaultClearFlag { + NO_FAULT = 0x00, + BAT_UNDER_VOL = 0x01, + BAT_OVER_VOL = 0x02, + MOTOR1_COMM = 0x03, + MOTOR2_COMM = 0x04, + MOTOR3_COMM = 0x05, + MOTOR4_COMM = 0x06, + MOTOR_DRV_OVERHEAT = 0x07, + MOTOR_OVERCURRENT = 0x08 + }; - ScoutMotionCmd(int8_t linear = 0, int8_t angular = 0, - FaultClearFlag fault_clr_flag = FaultClearFlag::NO_FAULT) - : linear_velocity(linear), angular_velocity(angular), - fault_clear_flag(fault_clr_flag) {} + ScoutMotionCmd(int8_t linear = 0, int8_t angular = 0, + FaultClearFlag fault_clr_flag = FaultClearFlag::NO_FAULT) + : linear_velocity(linear), + angular_velocity(angular), + fault_clear_flag(fault_clr_flag) {} - int8_t linear_velocity; - int8_t angular_velocity; - FaultClearFlag fault_clear_flag; - - 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 + int8_t linear_velocity; + int8_t angular_velocity; + FaultClearFlag fault_clear_flag; }; -struct ScoutLightCmd -{ - enum class LightMode - { - CONST_OFF = 0x00, - CONST_ON = 0x01, - BREATH = 0x02, - CUSTOM = 0x03 - }; - - ScoutLightCmd() = default; - ScoutLightCmd(LightMode f_mode, uint8_t f_value, LightMode r_mode, uint8_t r_value) : front_mode(f_mode), front_custom_value(f_value), - rear_mode(r_mode), rear_custom_value(r_value) {} - - LightMode front_mode; - uint8_t front_custom_value; - LightMode rear_mode; - uint8_t rear_custom_value; +struct ScoutCmdLimits { + 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 }; -} // namespace westonrobot + +struct ScoutMiniCmdLimits { + static constexpr double max_linear_velocity = 3.0; // 3.0 m/s + static constexpr double min_linear_velocity = -3.0; // -3.0 m/s + static constexpr double max_angular_velocity = 2.5235; // 2.5235 rad/s + static constexpr double min_angular_velocity = -2.5235; // -2.5235 rad/s +}; + +struct ScoutLightCmd { + enum class LightMode { + CONST_OFF = 0x00, + CONST_ON = 0x01, + BREATH = 0x02, + CUSTOM = 0x03 + }; + + ScoutLightCmd() = default; + ScoutLightCmd(LightMode f_mode, uint8_t f_value, LightMode r_mode, + uint8_t r_value) + : front_mode(f_mode), + front_custom_value(f_value), + rear_mode(r_mode), + rear_custom_value(r_value) {} + + LightMode front_mode; + uint8_t front_custom_value; + LightMode rear_mode; + uint8_t rear_custom_value; +}; +} // namespace westonrobot #endif /* SCOUT_STATE_HPP */ diff --git a/ugv_sdk/src/scout_base.cpp b/ugv_sdk/src/scout_base.cpp index 875ace2..59df476 100644 --- a/ugv_sdk/src/scout_base.cpp +++ b/ugv_sdk/src/scout_base.cpp @@ -145,21 +145,39 @@ void ScoutBase::SetMotionCommand( // make sure cmd thread is started before attempting to send commands if (!cmd_thread_started_) StartCmdThread(); - if (linear_vel < ScoutMotionCmd::min_linear_velocity) - linear_vel = ScoutMotionCmd::min_linear_velocity; - if (linear_vel > ScoutMotionCmd::max_linear_velocity) - linear_vel = ScoutMotionCmd::max_linear_velocity; - if (angular_vel < ScoutMotionCmd::min_angular_velocity) - angular_vel = ScoutMotionCmd::min_angular_velocity; - if (angular_vel > ScoutMotionCmd::max_angular_velocity) - angular_vel = ScoutMotionCmd::max_angular_velocity; + if (!is_scout_mini_) { + if (linear_vel < ScoutCmdLimits::min_linear_velocity) + linear_vel = ScoutCmdLimits::min_linear_velocity; + if (linear_vel > ScoutCmdLimits::max_linear_velocity) + linear_vel = ScoutCmdLimits::max_linear_velocity; + if (angular_vel < ScoutCmdLimits::min_angular_velocity) + angular_vel = ScoutCmdLimits::min_angular_velocity; + if (angular_vel > ScoutCmdLimits::max_angular_velocity) + angular_vel = ScoutCmdLimits::max_angular_velocity; - std::lock_guard guard(motion_cmd_mutex_); - current_motion_cmd_.linear_velocity = static_cast( - linear_vel / ScoutMotionCmd::max_linear_velocity * 100.0); - current_motion_cmd_.angular_velocity = static_cast( - angular_vel / ScoutMotionCmd::max_angular_velocity * 100.0); - current_motion_cmd_.fault_clear_flag = fault_clr_flag; + std::lock_guard guard(motion_cmd_mutex_); + current_motion_cmd_.linear_velocity = static_cast( + linear_vel / ScoutCmdLimits::max_linear_velocity * 100.0); + current_motion_cmd_.angular_velocity = static_cast( + angular_vel / ScoutCmdLimits::max_angular_velocity * 100.0); + current_motion_cmd_.fault_clear_flag = fault_clr_flag; + } else { + if (linear_vel < ScoutMiniCmdLimits::min_linear_velocity) + linear_vel = ScoutMiniCmdLimits::min_linear_velocity; + if (linear_vel > ScoutMiniCmdLimits::max_linear_velocity) + linear_vel = ScoutMiniCmdLimits::max_linear_velocity; + if (angular_vel < ScoutMiniCmdLimits::min_angular_velocity) + angular_vel = ScoutMiniCmdLimits::min_angular_velocity; + if (angular_vel > ScoutMiniCmdLimits::max_angular_velocity) + angular_vel = ScoutMiniCmdLimits::max_angular_velocity; + + std::lock_guard guard(motion_cmd_mutex_); + current_motion_cmd_.linear_velocity = static_cast( + linear_vel / ScoutMiniCmdLimits::max_linear_velocity * 100.0); + current_motion_cmd_.angular_velocity = static_cast( + angular_vel / ScoutMiniCmdLimits::max_angular_velocity * 100.0); + current_motion_cmd_.fault_clear_flag = fault_clr_flag; + } FeedCmdTimeoutWatchdog(); }