diff --git a/include/ugv_sdk/details/interface/agilex_message.h b/include/ugv_sdk/details/interface/agilex_message.h index f621c45..469db45 100644 --- a/include/ugv_sdk/details/interface/agilex_message.h +++ b/include/ugv_sdk/details/interface/agilex_message.h @@ -240,6 +240,10 @@ typedef struct { ControlMode mode; } ControlModeConfigMessage; +typedef struct { + BrakeMode mode; +} BrakeModeConfigMessage; + typedef struct { bool set_as_neutral; } SteerNeutralRequestMessage; @@ -295,6 +299,7 @@ typedef enum { AgxMsgSteerNeutralRequest, AgxMsgSteerNeutralResponse, AgxMsgStateResetConfig, + AgxMsgBrakeModeConfig, // V1-only messages AgxMsgMotionCommandV1, AgxMsgValueSetCommandV1, @@ -333,6 +338,7 @@ typedef struct { VersionRequestMessage version_request_msg; VersionResponseMessage version_response_msg; ControlModeConfigMessage control_mode_config_msg; + BrakeModeConfigMessage brake_mode_config_msg; SteerNeutralRequestMessage steer_neutral_request_msg; SteerNeutralResponseMessage steer_neutral_response_msg; StateResetConfigMessage state_reset_config_msg; diff --git a/include/ugv_sdk/details/interface/agilex_types.h b/include/ugv_sdk/details/interface/agilex_types.h index 627738c..dfb97c0 100644 --- a/include/ugv_sdk/details/interface/agilex_types.h +++ b/include/ugv_sdk/details/interface/agilex_types.h @@ -41,6 +41,12 @@ typedef enum { CONTROL_MODE_UART = 0x02 } ControlMode; +typedef enum { + // CONTROL_MODE_STANDBY = 0x00, + BRAKE_MODE_UNLOCK = 0x00, + BRAKE_MODE_LOCK = 0x01 +} BrakeMode; + typedef enum { RC_SWITCH_UP = 0, RC_SWITCH_MIDDLE, diff --git a/include/ugv_sdk/details/interface/hunter_interface.hpp b/include/ugv_sdk/details/interface/hunter_interface.hpp index a07ce2c..a3c5f0a 100644 --- a/include/ugv_sdk/details/interface/hunter_interface.hpp +++ b/include/ugv_sdk/details/interface/hunter_interface.hpp @@ -39,6 +39,8 @@ struct HunterInterface { virtual void SetMotionCommand(double linear_vel, double steering_angle) = 0; + virtual void SetBrakeMode(BrakeMode mode) = 0; + // get robot state virtual HunterCoreState GetRobotState() = 0; virtual HunterActuatorState GetActuatorState() = 0; diff --git a/include/ugv_sdk/details/interface/robot_common_interface.hpp b/include/ugv_sdk/details/interface/robot_common_interface.hpp index df382fb..7c8537d 100644 --- a/include/ugv_sdk/details/interface/robot_common_interface.hpp +++ b/include/ugv_sdk/details/interface/robot_common_interface.hpp @@ -93,6 +93,8 @@ class RobotCommonInterface { "Only a derived version of this function with actual implementation " "is supposed to be used."); }; + + virtual void EnableBrakeMode(BrakeMode mode){}; }; } // namespace westonrobot diff --git a/include/ugv_sdk/details/robot_base/agilex_base.hpp b/include/ugv_sdk/details/robot_base/agilex_base.hpp index 7ebeab0..3385c2e 100644 --- a/include/ugv_sdk/details/robot_base/agilex_base.hpp +++ b/include/ugv_sdk/details/robot_base/agilex_base.hpp @@ -268,6 +268,17 @@ class AgilexBase : public RobotCommonInterface { break; } } + + void EnableBrakedMode(BrakeMode mode) { + // construct message + AgxMessage msg; + msg.type = AgxMsgBrakeModeConfig; + msg.body.brake_mode_config_msg.mode = mode; + + // encode msg to can frame and send to bus + can_frame frame; + if (parser_.EncodeMessage(&msg, &frame)) can_->SendFrame(frame); + } }; } // namespace westonrobot diff --git a/include/ugv_sdk/details/robot_base/hunter_base.hpp b/include/ugv_sdk/details/robot_base/hunter_base.hpp index 2f3fce7..4d28274 100644 --- a/include/ugv_sdk/details/robot_base/hunter_base.hpp +++ b/include/ugv_sdk/details/robot_base/hunter_base.hpp @@ -36,6 +36,7 @@ class HunterBase : public AgilexBase, public HunterInterface { angular_vel); } + // get robot state HunterCoreState GetRobotState() override { auto state = AgilexBase::GetRobotCoreStateMsgGroup(); @@ -60,6 +61,10 @@ class HunterBase : public AgilexBase, public HunterInterface { } return hunter_actuator; } + + void SetBrakeMode(BrakeMode mode) override{ + AgilexBase::EnableBrakeMode(mode); + } }; } // namespace westonrobot diff --git a/include/ugv_sdk/mobile_robot/hunter_robot.hpp b/include/ugv_sdk/mobile_robot/hunter_robot.hpp index 8c529e8..bcc5b2d 100644 --- a/include/ugv_sdk/mobile_robot/hunter_robot.hpp +++ b/include/ugv_sdk/mobile_robot/hunter_robot.hpp @@ -25,6 +25,8 @@ class HunterRobot : public RobotCommonInterface, public HunterInterface { void EnableCommandedMode() override; + void SetBrakeMode(BrakeMode mode) override; + void SetMotionCommand(double linear_vel, double angular_vel) override; void ResetRobotState() override; diff --git a/src/mobile_robot/hunter_robot.cpp b/src/mobile_robot/hunter_robot.cpp index a9e2733..d4c44d9 100644 --- a/src/mobile_robot/hunter_robot.cpp +++ b/src/mobile_robot/hunter_robot.cpp @@ -38,6 +38,11 @@ void HunterRobot::SetMotionCommand(double linear_vel, double angular_vel) { hunter->SetMotionCommand(linear_vel, angular_vel); } +void HunterRobot::SetBrakeMode(BrakeMode mode) { + auto hunter = dynamic_cast(robot_); + hunter->SetBrakeMode(mode); +} + HunterCoreState HunterRobot::GetRobotState() { auto hunter = dynamic_cast(robot_); return hunter->GetRobotState(); diff --git a/src/protocol_v2/agilex_msg_parser_v2.c b/src/protocol_v2/agilex_msg_parser_v2.c index f8fff49..dfa6c78 100644 --- a/src/protocol_v2/agilex_msg_parser_v2.c +++ b/src/protocol_v2/agilex_msg_parser_v2.c @@ -563,6 +563,21 @@ bool EncodeCanFrameV2(const AgxMessage *msg, struct can_frame *tx_frame) { memcpy(tx_frame->data, (uint8_t *)(&frame), tx_frame->can_dlc); break; } + case AgxMsgBrakeModeConfig: { + tx_frame->can_id = CAN_MSG_BRAKING_COMMAND_ID; + tx_frame->can_dlc = 8; + BrakeModeConfigFrame frame; + frame.mode = msg->body.control_mode_config_msg.mode; + frame.reserved0 = 0; + frame.reserved1 = 0; + frame.reserved2 = 0; + frame.reserved3 = 0; + frame.reserved4 = 0; + frame.reserved5 = 0; + frame.reserved6 = 0; + memcpy(tx_frame->data, (uint8_t *)(&frame), tx_frame->can_dlc); + break; + } case AgxMsgSteerNeutralRequest: { tx_frame->can_id = CAN_MSG_STEER_NEUTRAL_REQUEST_ID; tx_frame->can_dlc = 8; diff --git a/src/protocol_v2/agilex_protocol_v2.h b/src/protocol_v2/agilex_protocol_v2.h index 8cfe8b9..467bcd5 100644 --- a/src/protocol_v2/agilex_protocol_v2.h +++ b/src/protocol_v2/agilex_protocol_v2.h @@ -360,6 +360,17 @@ typedef struct { uint8_t reserved6; } ControlModeConfigFrame; +typedef struct { + uint8_t mode; + uint8_t reserved0; + uint8_t reserved1; + uint8_t reserved2; + uint8_t reserved3; + uint8_t reserved4; + uint8_t reserved5; + uint8_t reserved6; +} BrakeModeConfigFrame; + typedef struct { uint8_t set_as_neutral; uint8_t reserved0;