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

merged with next

Browse Source
This commit is contained in:
Ruixiang Du
2020-06-23 15:39:55 +08:00
parent 0006a1cd62 b93fdebe40
commit 1259c5e740
4105 changed files with 576 additions and 489684 deletions
Download Patch File Download Diff File Expand all files Collapse all files

69
include/wrp_sdk/platforms/hunter/hunter_base.hpp Normal file
View File

@@ -0,0 +1,69 @@
/*
* hunter_base.hpp
*
* Created on: Apr 01, 2020 09:43
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#ifndef HUNTER_BASE_HPP
#define HUNTER_BASE_HPP
#include <string>
#include <cstdint>
#include <thread>
#include <mutex>
#include "wrp_sdk/platforms/common/mobile_base.hpp"
#include "wrp_sdk/platforms/hunter/hunter_protocol.h"
#include "wrp_sdk/platforms/hunter/hunter_can_parser.h"
#include "wrp_sdk/platforms/hunter/hunter_types.hpp"
namespace westonrobot {
class HunterBase : public MobileBase {
public:
HunterBase() : MobileBase(){};
~HunterBase() = default;
// get robot state
HunterState GetHunterState();
// motion control
void SetMotionCommand(double linear_vel, double steering_angle,
HunterMotionCmd::FaultClearFlag fault_clr_flag =
HunterMotionCmd::FaultClearFlag::NO_FAULT);
private:
// serial port related variables
uint8_t tx_cmd_len_;
uint8_t tx_buffer_[HUNTER_CMD_BUF_LEN];
// cmd/status update related variables
std::mutex hunter_state_mutex_;
std::mutex motion_cmd_mutex_;
HunterState hunter_state_;
HunterMotionCmd current_motion_cmd_;
// internal functions
void ConfigureCANBus(const std::string &can_if_name = "can1");
void ConfigureSerial(const std::string uart_name = "/dev/ttyUSB0",
int32_t baud_rate = 115200);
void SendRobotCmd() override;
void ParseCANFrame(can_frame *rx_frame) override;
void ParseUARTBuffer(uint8_t *buf, const size_t bufsize,
size_t bytes_received) override{};
void SendMotionCmd(uint8_t count);
void NewStatusMsgReceivedCallback(const HunterMessage &msg);
public:
static void UpdateHunterState(const HunterMessage &status_msg,
HunterState &state);
};
} // namespace westonrobot
#endif /* HUNTER_BASE_HPP */

42
include/wrp_sdk/platforms/hunter/hunter_can_parser.h Normal file
View File

@@ -0,0 +1,42 @@
/*
* hunter_can_parser.h
*
* Created on: Jan 02, 2020 12:36
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#ifndef HUNTER_CAN_PARSER_H
#define HUNTER_CAN_PARSER_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include <stdbool.h>
#include "wrp_sdk/platforms/hunter/hunter_protocol.h"
#ifdef __linux__
#include <linux/can.h>
#else
struct can_frame
{
uint32_t can_id;
uint8_t can_dlc;
uint8_t data[8]__attribute__((aligned(8)));
};
#endif
bool DecodeHunterMsgFromCAN(const struct can_frame *rx_frame, HunterMessage *msg);
void EncodeHunterMsgToCAN(const HunterMessage *msg, struct can_frame *tx_frame);
uint8_t CalcHunterCANChecksum(uint16_t id, uint8_t *data, uint8_t dlc);
#ifdef __cplusplus
}
#endif
#endif /* HUNTER_CAN_PARSER_H */

257
include/wrp_sdk/platforms/hunter/hunter_protocol.h Normal file
View File

@@ -0,0 +1,257 @@
/*
* hunter_protocol.h
*
* Created on: Jan 02, 2020 12:06
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#ifndef HUNTER_PROTOCOL_H
#define HUNTER_PROTOCOL_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#define HUNTER_CMD_BUF_LEN 32
#define HUNTER_STATUS_BUF_LEN 32
#define HUNTER_FRAME_SIZE 13
#define HUNTER_MOTOR1_ID ((uint8_t)0x00)
#define HUNTER_MOTOR2_ID ((uint8_t)0x01)
#define HUNTER_MOTOR3_ID ((uint8_t)0x02)
// CAN Definitions
#define CAN_MSG_MOTION_CMD_ID ((uint32_t)0x130)
#define CAN_MSG_MOTION_STATUS_ID ((uint32_t)0x131)
#define CAN_MSG_CONFIG_CMD_ID ((uint32_t)0x210)
#define CAN_MSG_CONFIG_STATUS_ID ((uint32_t)0x211)
#define CAN_MSG_SYSTEM_STATUS_STATUS_ID ((uint32_t)0x151)
#define CAN_MSG_MOTOR1_DRIVER_STATUS_ID ((uint32_t)0x201)
#define CAN_MSG_MOTOR2_DRIVER_STATUS_ID ((uint32_t)0x202)
#define CAN_MSG_MOTOR3_DRIVER_STATUS_ID ((uint32_t)0x203)
/*--------------------- Control/State Constants ------------------------*/
// Motion Control
#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 FAULT_CLR_NONE ((uint8_t)0x00)
#define FAULT_CLR_BAT_UNDER_VOL ((uint8_t)0x01)
#define FAULT_CLR_BAT_OVER_VOL ((uint8_t)0x02)
#define FAULT_CLR_MOTOR1_COMM ((uint8_t)0x03)
#define FAULT_CLR_MOTOR2_COMM ((uint8_t)0x04)
#define FAULT_CLR_MOTOR3_COMM ((uint8_t)0x05)
#define FAULT_CLR_MOTOR4_COMM ((uint8_t)0x06)
#define FAULT_CLR_MOTOR_DRV_OVERHEAT ((uint8_t)0x07)
#define FAULT_CLR_MOTOR_OVERCURRENT ((uint8_t)0x08)
// System Configuration
#define STEERING_ZERO_CONFIG_FAIL ((uint8_t)0x00)
#define STEERING_ZERO_CONFIG_SUCCESS ((uint8_t)0xaa)
// System Status Feedback
#define BASE_STATE_NORMAL ((uint8_t)0x00)
#define BASE_STATE_ESTOP ((uint8_t)0x01)
#define BASE_STATE_EXCEPTION ((uint8_t)0x02)
#define FAULT_CAN_CHECKSUM_ERROR ((uint16_t)0x0100)
#define FAULT_FRONT_STEER_ENCODER_F ((uint16_t)0x0200)
#define FAULT_RC_SIGNAL_LOSS ((uint16_t)0x0400)
#define FAULT_HIGH_BYTE_RESERVED1 ((uint16_t)0x0800)
#define FAULT_HIGH_BYTE_RESERVED2 ((uint16_t)0x1000)
#define FAULT_HIGH_BYTE_RESERVED3 ((uint16_t)0x2000)
#define FAULT_HIGH_BYTE_RESERVED4 ((uint16_t)0x4000)
#define FAULT_HIGH_BYTE_RESERVED5 ((uint16_t)0x8000)
#define FAULT_BAT_UNDER_VOL_F ((uint16_t)0x0001)
#define FAULT_BAT_OVER_VOL_F ((uint16_t)0x0002)
#define FAULT_MOTOR1_COMM_F ((uint16_t)0x0004)
#define FAULT_MOTOR2_COMM_F ((uint16_t)0x0008)
#define FAULT_MOTOR3_COMM_F ((uint16_t)0x0010)
#define FAULT_MOTOR4_COMM_F ((uint16_t)0x0020)
#define FAULT_MOTOR_DRV_OVERHEAT_F ((uint16_t)0x0040)
#define FAULT_MOTOR_OVERCURRENT_F ((uint16_t)0x0080)
/*-------------------- Control/Feedback Messages -----------------------*/
/* No padding in the struct */
// reference: https://stackoverflow.com/questions/3318410/pragma-pack-effect
#pragma pack(push, 1)
// Note: id could be different for UART and CAN protocol
// Motion Control
typedef struct {
union
{
struct
{
uint8_t control_mode;
uint8_t fault_clear_flag;
int8_t linear_velocity_cmd;
int8_t angular_velocity_cmd;
uint8_t reserved0;
uint8_t reserved1;
uint8_t count;
uint8_t checksum;
} cmd;
uint8_t raw[8];
} data;
} MotionCmdMessage;
typedef struct {
union
{
struct
{
struct
{
uint8_t high_byte;
uint8_t low_byte;
} linear_velocity;
struct
{
uint8_t high_byte;
uint8_t low_byte;
} angular_velocity;
uint8_t reserved0;
uint8_t reserved1;
uint8_t count;
uint8_t checksum;
} status;
uint8_t raw[8];
} data;
} MotionStatusMessage;
// System Status Feedback
typedef struct {
union
{
struct
{
uint8_t base_state;
uint8_t control_mode;
struct
{
uint8_t high_byte;
uint8_t low_byte;
} battery_voltage;
struct
{
uint8_t high_byte;
uint8_t low_byte;
} fault_code;
uint8_t count;
uint8_t checksum;
} status;
uint8_t raw[8];
} data;
} SystemStatusMessage;
// System Configuration
typedef struct {
union
{
struct
{
uint8_t set_zero_steering;
uint8_t reserved0;
uint8_t reserved1;
uint8_t reserved2;
uint8_t reserved3;
uint8_t reserved4;
uint8_t count;
uint8_t checksum;
} status;
uint8_t raw[8];
} data;
} ConfigCmdMessage;
// System Configuration Status Feedback
typedef struct {
union
{
struct
{
uint8_t set_zero_steering;
uint8_t reserved0;
uint8_t reserved1;
uint8_t reserved2;
uint8_t reserved3;
uint8_t reserved4;
uint8_t count;
uint8_t checksum;
} status;
uint8_t raw[8];
} data;
} ConfigStatusMessage;
// Motor Driver Feedback
typedef struct
{
uint8_t motor_id;
union {
struct
{
struct
{
uint8_t high_byte;
uint8_t low_byte;
} current;
struct
{
uint8_t high_byte;
uint8_t low_byte;
} rpm;
int8_t temperature;
uint8_t reserved0;
uint8_t count;
uint8_t checksum;
} status;
uint8_t raw[8];
} data;
} MotorDriverStatusMessage;
// For convenience to access status/control message
typedef enum
{
HunterMsgNone = 0x00,
// status messages
HunterMotionStatusMsg = 0x01,
HunterSystemStatusMsg = 0x03,
HunterMotorDriverStatusMsg = 0x04,
HunterConfigStatusMsg = 0x05,
// control messages
HunterMotionCmdMsg = 0x21,
HunterConfigCmdMsg = 0x22
} HunterMsgType;
typedef struct
{
HunterMsgType type;
union {
// status messages
MotionStatusMessage motion_status_msg;
SystemStatusMessage system_status_msg;
ConfigStatusMessage config_status_msg;
MotorDriverStatusMessage motor_driver_status_msg;
// control messages
MotionCmdMessage motion_cmd_msg;
ConfigCmdMessage config_cmd_msg;
} body;
} HunterMessage;
#pragma pack(pop)
#ifdef __cplusplus
}
#endif
#endif /* HUNTER_PROTOCOL_H */

75
include/wrp_sdk/platforms/hunter/hunter_types.hpp Normal file
View File

@@ -0,0 +1,75 @@
/*
* hunter_types.hpp
*
* Created on: Apr 01, 2020 09:43
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#ifndef HUNTER_TYPES_HPP
#define HUNTER_TYPES_HPP
#include <cstdint>
#include <iostream>
namespace westonrobot
{
struct HunterState
{
struct MotorState
{
double current = 0; // in A
double rpm = 0;
double temperature = 0;
};
// base state
uint8_t base_state = 0;
uint8_t control_mode = 0;
uint16_t fault_code = 0;
double battery_voltage = 0.0;
uint8_t set_zero_steering = 0;
// motor state
static constexpr uint8_t motor_num = 3;
MotorState motor_states[motor_num];
// motion state
double linear_velocity = 0;
double steering_angle = 0;
};
struct HunterMotionCmd
{
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
};
HunterMotionCmd(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_steering_angle = 0.4622; // 0.4622 rad ~= 26.5 degree
static constexpr double min_steering_angle = -0.4622; // -0.4622 rad
};
} // namespace westonrobot
#endif /* HUNTER_TYPES_HPP */