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

saved work

Browse Source
This commit is contained in:
Ruixiang Du
2021-07-12 19:02:41 +08:00
parent 10d9fc734e
commit 6cbdebc38a
14 changed files with 337 additions and 93 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
include/ugv_sdk/details/interface/agilex_message.h
View File

@@ -42,7 +42,7 @@ typedef struct {
typedef struct {
uint8_t motion_mode;
} MotionModeMessage;
} MotionModeCommandMessage;
// V1-only messages
typedef struct {
@@ -56,6 +56,8 @@ typedef struct {
bool set_neutral;
} ValueSetCommandMessageV1;
typedef ValueSetCommandMessageV1 ValueSetStateMessageV1;
/**************** Feedback messages *****************/
#define SYSTEM_ERROR_MOTOR_DRIVER_MASK ((uint16_t)0x0100)
@@ -128,6 +130,7 @@ typedef struct {
// V1-only messages
typedef struct {
uint8_t motor_id;
float current;
int16_t rpm;
float temperature;
@@ -263,7 +266,7 @@ typedef enum {
AgxMsgMotionCommand,
AgxMsgLightCommand,
AgxMsgBrakingCommand,
AgxMsgSetMotionMode,
AgxMsgSetMotionModeCommand,
// state feedback
AgxMsgSystemState,
AgxMsgMotionState,
@@ -292,6 +295,7 @@ typedef enum {
// V1-only messages
AgxMsgMotionCommandV1,
AgxMsgValueSetCommandV1,
AgxMsgValueSetStateV1,
AgxMsgActuatorStateV1
} MsgType;
@@ -302,7 +306,7 @@ typedef struct {
MotionCommandMessage motion_command_msg;
LightCommandMessage light_command_msg;
BrakingCommandMessage braking_command_msg;
MotionModeMessage motion_mode_msg;
MotionModeCommandMessage motion_mode_msg;
// state feedback
SystemStateMessage system_state_msg;
MotionStateMessage motion_state_msg;
@@ -331,6 +335,7 @@ typedef struct {
// V1-only messages
MotionCommandMessageV1 v1_motion_command_msg;
ValueSetCommandMessageV1 v1_value_set_command_msg;
ValueSetStateMessageV1 v1_value_set_state_msg;
ActuatorStateMessageV1 v1_actuator_state_msg;
} body;
} AgxMessage;

197
include/ugv_sdk/details/protocol_v1/agilex_protocol_v1.h
View File

@@ -1,197 +0,0 @@
/*
* agilex_protocol_v1.h
*
* Created on: Jul 09, 2021 20:34
* Description:
*
* Copyright (c) 2021 Ruixiang Du (rdu)
*/
#ifndef AGILEX_PROTOCOL_V1_H
#define AGILEX_PROTOCOL_V1_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
// define endianess of the platform
#if (!defined(USE_LITTLE_ENDIAN) && !defined(USE_BIG_ENDIAN))
#define USE_LITTLE_ENDIAN
#endif
#ifdef USE_BIG_ENDIAN
#error "BIG ENDIAN IS CURRENTLY NOT SUPPORTED"
#endif
/*---------------------------- Motor IDs -------------------------------*/
#define ACTUATOR1_ID ((uint8_t)0x00)
#define ACTUATOR2_ID ((uint8_t)0x01)
#define ACTUATOR3_ID ((uint8_t)0x02)
#define ACTUATOR4_ID ((uint8_t)0x03)
/*--------------------------- Message IDs ------------------------------*/
// CAN: control group
#define CAN_MSG_MOTION_COMMAND_ID ((uint32_t)0x130)
#define CAN_MSG_LIGHT_COMMAND_ID ((uint32_t)0x140)
#define CAN_MSG_VALUE_SET_COMMAND_ID ((uint32_t)0x211)
// CAN: state feedback group
#define CAN_MSG_MOTION_STATE_ID ((uint32_t)0x131)
#define CAN_MSG_LIGHT_STATE_ID ((uint32_t)0x141)
#define CAN_MSG_SYSTEM_STATE_ID ((uint32_t)0x151)
#define CAN_MSG_ACTUATOR1_STATE_ID ((uint32_t)0x200)
#define CAN_MSG_ACTUATOR2_STATE_ID ((uint32_t)0x201)
#define CAN_MSG_ACTUATOR3_STATE_ID ((uint32_t)0x202)
#define CAN_MSG_ACTUATOR4_STATE_ID ((uint32_t)0x203)
/*------------------------ Frame Memory Layout -------------------------*/
/* No padding in the struct */
// reference: https://stackoverflow.com/questions/3318410/pragma-pack-effect
#pragma pack(push, 1)
#ifdef USE_LITTLE_ENDIAN
typedef struct {
uint8_t high_byte;
uint8_t low_byte;
} struct16_t;
typedef struct {
uint8_t msb;
uint8_t high_byte;
uint8_t low_byte;
uint8_t lsb;
} struct32_t;
#elif defined(USE_BIG_ENDIAN)
typedef struct {
uint8_t low_byte;
uint8_t high_byte;
} struct16_t;
typedef struct {
uint8_t lsb;
uint8_t low_byte;
uint8_t high_byte;
uint8_t msb;
} struct32_t;
#endif
// Control messages
#define CTRL_MODE_REMOTE ((uint8_t)0x00)
#define CTRL_MODE_CMD_CAN ((uint8_t)0x01)
#define CTRL_MODE_CMD_UART ((uint8_t)0x02)
#define CTRL_MODE_COMMANDED ((uint8_t)0x03)
#define ERROR_CLR_NONE ((uint8_t)0x00)
#define ERROR_CLR_BAT_UNDER_VOL ((uint8_t)0x01)
#define ERROR_CLR_BAT_OVER_VOL ((uint8_t)0x02)
#define ERROR_CLR_MOTOR1_COMM ((uint8_t)0x03)
#define ERROR_CLR_MOTOR2_COMM ((uint8_t)0x04)
#define ERROR_CLR_MOTOR3_COMM ((uint8_t)0x05)
#define ERROR_CLR_MOTOR4_COMM ((uint8_t)0x06)
#define ERROR_CLR_MOTOR_DRV_OVERHEAT ((uint8_t)0x07)
#define ERROR_CLR_MOTOR_OVERCURRENT ((uint8_t)0x08)
typedef struct {
uint8_t control_mode;
uint8_t error_clear_byte;
int8_t linear_percentage;
int8_t angular_percentage;
uint8_t reserved0;
uint8_t reserved1;
uint8_t count;
uint8_t checksum;
} MotionCommandFrame;
#define LIGHT_ENABLE_CMD_CTRL ((uint8_t)0x01)
#define LIGHT_DISABLE_CMD_CTRL ((uint8_t)0x00)
#define LIGHT_MODE_CONST_OFF ((uint8_t)0x00)
#define LIGHT_MODE_CONST_ON ((uint8_t)0x01)
#define LIGHT_MODE_BREATH ((uint8_t)0x02)
#define LIGHT_MODE_CUSTOM ((uint8_t)0x03)
typedef struct {
uint8_t enable_cmd_ctrl;
uint8_t front_mode;
uint8_t front_custom;
uint8_t rear_mode;
uint8_t rear_custom;
uint8_t reserved0;
uint8_t count;
uint8_t checksum;
} LightCommandFrame;
typedef struct {
uint8_t set_neutral;
uint8_t reserved0;
uint8_t reserved1;
uint8_t reserved2;
uint8_t reserved3;
uint8_t reserved4;
uint8_t count;
uint8_t checksum;
} ValueSetCommandFrame;
// State feedback messages
#define VEHICLE_STATE_NORMAL ((uint8_t)0x00)
#define VEHICLE_STATE_ESTOP ((uint8_t)0x01)
#define VEHICLE_STATE_EXCEPTION ((uint8_t)0x02)
#define ERROR_CAN_CHECKSUM_ERROR ((uint16_t)0x0100)
#define ERROR_MOTOR_DRV_OVERHEAT_W ((uint16_t)0x0200)
#define ERROR_MOTOR_OVERCURRENT_W ((uint16_t)0x0400)
#define ERROR_BAT_UNDER_VOL_W ((uint16_t)0x0800)
#define ERROR_RC_SIGNAL_LOSS ((uint16_t)0x1000)
#define ERROR_HIGH_BYTE_RESERVED2 ((uint16_t)0x2000)
#define ERROR_HIGH_BYTE_RESERVED3 ((uint16_t)0x4000)
#define ERROR_HIGH_BYTE_RESERVED4 ((uint16_t)0x8000)
#define ERROR_BAT_UNDER_VOL_F ((uint16_t)0x0001)
#define ERROR_BAT_OVER_VOL_F ((uint16_t)0x0002)
#define ERROR_MOTOR1_COMM_F ((uint16_t)0x0004)
#define ERROR_MOTOR2_COMM_F ((uint16_t)0x0008)
#define ERROR_MOTOR3_COMM_F ((uint16_t)0x0010)
#define ERROR_MOTOR4_COMM_F ((uint16_t)0x0020)
#define ERROR_MOTOR_DRV_OVERHEAT_F ((uint16_t)0x0040)
#define ERROR_MOTOR_OVERCURRENT_F ((uint16_t)0x0080)
typedef struct {
uint8_t vehicle_state;
uint8_t control_mode;
struct16_t battery_voltage;
struct16_t error_code;
uint8_t count;
uint8_t checksum;
} SystemStateFrame;
typedef struct {
struct16_t linear;
struct16_t angular;
uint8_t reserved0;
uint8_t reserved1;
uint8_t count;
uint8_t checksum;
} MotionStateFrame;
typedef LightCommandFrame LightStateFrame;
typedef struct {
struct16_t current;
struct16_t rpm;
int8_t temperature;
uint8_t reserved0;
uint8_t count;
uint8_t checksum;
} ActuatorStateFrame;
#pragma pack(pop)
#ifdef __cplusplus
}
#endif
#endif /* AGILEX_PROTOCOL_V1_H */

40
include/ugv_sdk/details/protocol_v1/diffdrive_model_v1_parser.hpp → include/ugv_sdk/details/protocol_v1/protocol_v1_parser.hpp
View File

@@ -17,7 +17,7 @@
namespace westonrobot {
template <typename RobotLimitsType>
class DiffDriveModelV1Parser : public ParserInterface {
class ProtocolV1Parser : public ParserInterface {
public:
bool DecodeMessage(const struct can_frame *rx_frame,
AgxMessage *msg) override {
@@ -27,21 +27,22 @@ class DiffDriveModelV1Parser : public ParserInterface {
void EncodeMessage(const AgxMessage *msg,
struct can_frame *tx_frame) override {
AgxMessage msg_v1 = *msg;
if (msg->type == AgxMsgMotionCommand) {
float linear = msg->body.motion_command_msg.linear_velocity;
float angular = msg->body.motion_command_msg.angular_velocity;
if (linear > ScoutV2Limits::max_linear_velocity)
linear = ScoutV2Limits::max_linear_velocity;
else if (linear < ScoutV2Limits::min_linear_velocity)
linear = ScoutV2Limits::min_linear_velocity;
if (angular > ScoutV2Limits::max_angular_velocity)
angular = ScoutV2Limits::max_angular_velocity;
else if (angular < ScoutV2Limits::min_angular_velocity)
angular = ScoutV2Limits::min_angular_velocity;
msg_v1.body.motion_command_msg.linear_velocity =
linear / ScoutV2Limits::max_linear_velocity * 100.0;
msg_v1.body.motion_command_msg.angular_velocity = static_cast<int8_t>(
angular / ScoutV2Limits::max_angular_velocity * 100.0);
if (msg->type == AgxMsgMotionCommandV1) {
float linear = msg->body.v1_motion_command_msg.linear;
float angular = msg->body.v1_motion_command_msg.angular;
if (linear > RobotLimitsType::max_linear)
linear = RobotLimitsType::max_linear;
else if (linear < RobotLimitsType::min_linear)
linear = RobotLimitsType::min_linear;
if (angular > RobotLimitsType::max_angular)
angular = RobotLimitsType::max_angular;
else if (angular < RobotLimitsType::min_angular)
angular = RobotLimitsType::min_angular;
msg_v1.body.v1_motion_command_msg.linear =
linear / RobotLimitsType::max_linear * 100.0;
msg_v1.body.v1_motion_command_msg.angular =
angular / RobotLimitsType::max_angular * 100.0;
}
EncodeCanFrameV1(&msg_v1, tx_frame);
}
@@ -59,8 +60,11 @@ class DiffDriveModelV1Parser : public ParserInterface {
uint8_t CalculateChecksum(uint8_t *buf, uint8_t len) override {}
};
using ScoutProtocolV1Parser = DiffDriveModelV1Parser<ScoutV2Limits>;
using ScoutMiniProtocolV1Parser = DiffDriveModelV1Parser<ScoutV2Limits>;
using ScoutProtocolV1Parser = ProtocolV1Parser<ScoutV2Limits>;
using ScoutMiniProtocolV1Parser = ProtocolV1Parser<ScoutV2Limits>;
using HunterProtocolV1Parser = ProtocolV1Parser<HunterV1Limits>;
} // namespace westonrobot
#endif /* SCOUT_PROTOCOL_V1_PARSER_HPP */

23
include/ugv_sdk/details/protocol_v1/robot_limits.hpp
View File

@@ -12,17 +12,24 @@
namespace westonrobot {
struct ScoutV2Limits {
static constexpr double max_linear_velocity = 1.5; // 1.5 m/s
static constexpr double min_linear_velocity = -max_linear_velocity;
static constexpr double max_angular_velocity = 0.5235; // 0.5235 rad/s
static constexpr double min_angular_velocity = -max_angular_velocity;
static constexpr double max_linear = 1.5; // m/s
static constexpr double min_linear = -max_linear;
static constexpr double max_angular = 0.5235; // rad/s
static constexpr double min_angular = -max_angular;
};
struct ScoutMiniLimits {
static constexpr double max_linear_velocity = 3.0; // 3.0 m/s
static constexpr double min_linear_velocity = -max_linear_velocity;
static constexpr double max_angular_velocity = 2.5235; // 2.5235 rad/s
static constexpr double min_angular_velocity = -max_angular_velocity;
static constexpr double max_linear = 3.0; // m/s
static constexpr double min_linear = -max_linear;
static constexpr double max_angular = 2.5235; // rad/s
static constexpr double min_angular = -max_angular;
};
struct HunterV1Limits {
static constexpr double max_linear = 1.5; // m/s
static constexpr double min_linear = -max_linear;
static constexpr double max_angular = 25.5; // degree
static constexpr double min_angular = -max_angular;
};
} // namespace westonrobot

2
include/ugv_sdk/details/robot_base/agilex_base.hpp
View File

@@ -106,7 +106,7 @@ class AgilexBase : public RobotInterface {
void SetMotionMode(uint8_t mode) {
if (can_connected_) {
AgxMessage msg;
msg.type = AgxMsgSetMotionMode;
msg.type = AgxMsgSetMotionModeCommand;
msg.body.motion_mode_msg.motion_mode = mode;
// send to can bus

2
include/ugv_sdk/details/robot_base/scout_base.hpp
View File

@@ -114,7 +114,7 @@ class ScoutBase : public AgilexBase<Parser>, public ScoutInterface {
};
} // namespace westonrobot
#include "ugv_sdk/details/protocol_v1/diffdrive_model_v1_parser.hpp"
#include "ugv_sdk/details/protocol_v1/protocol_v1_parser.hpp"
#include "ugv_sdk/details/protocol_v2/protocol_v2_parser.hpp"
namespace westonrobot {