mirrors/ugv_sdk
1
0
Fork 0
mirror of https://github.com/westonrobot/ugv_sdk synced 2023-04-08 06:32:14 +08:00
Code Issues Packages Projects Releases Wiki Activity

added scout mini support

Browse Source
This commit is contained in:
Ruixiang Du
2020-12-21 11:29:48 +08:00
parent a7460f197b
commit 411fbb9a1e
5 changed files with 132 additions and 103 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
README.md
View File

@@ -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

4
ugv_sdk/apps/scout_monitor/src/scout_monitor.cpp
View File

@@ -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<int>(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<int>(angular_percentage * 5)) + 1;
if (std::abs(scout_state_.angular_velocity) < 0.001)
angular_bars = 0;

5
ugv_sdk/include/ugv_sdk/scout/scout_base.hpp
View File

@@ -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];

169
ugv_sdk/include/ugv_sdk/scout/scout_types.hpp
View File

@@ -13,100 +13,105 @@
#include <cstdint>
#include <iostream>
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 */

46
ugv_sdk/src/scout_base.cpp
View File

@@ -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<std::mutex> guard(motion_cmd_mutex_);
current_motion_cmd_.linear_velocity = static_cast<int8_t>(
linear_vel / ScoutMotionCmd::max_linear_velocity * 100.0);
current_motion_cmd_.angular_velocity = static_cast<int8_t>(
angular_vel / ScoutMotionCmd::max_angular_velocity * 100.0);
current_motion_cmd_.fault_clear_flag = fault_clr_flag;
std::lock_guard<std::mutex> guard(motion_cmd_mutex_);
current_motion_cmd_.linear_velocity = static_cast<int8_t>(
linear_vel / ScoutCmdLimits::max_linear_velocity * 100.0);
current_motion_cmd_.angular_velocity = static_cast<int8_t>(
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<std::mutex> guard(motion_cmd_mutex_);
current_motion_cmd_.linear_velocity = static_cast<int8_t>(
linear_vel / ScoutMiniCmdLimits::max_linear_velocity * 100.0);
current_motion_cmd_.angular_velocity = static_cast<int8_t>(
angular_vel / ScoutMiniCmdLimits::max_angular_velocity * 100.0);
current_motion_cmd_.fault_clear_flag = fault_clr_flag;
}
FeedCmdTimeoutWatchdog();
}