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

code compiles with new structure

Browse Source
This commit is contained in:
Ruixiang Du
2020-06-17 11:18:04 +08:00
parent 8bbca03528
commit 44b89ee5f9
4100 changed files with 292 additions and 489458 deletions
Download Patch File Download Diff File Expand all files Collapse all files

130
include/wrp_sdk/async_io/async_can.hpp Normal file
View File

@@ -0,0 +1,130 @@
/*
* async_can.hpp
*
* Created on: Jun 10, 2019 02:16
* Description: code based on uavcan and libmavconn
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
/*
* Copyright (c) 2016 UAVCAN Team
*
* Distributed under the MIT License, available in the file LICENSE.
*
*/
/*
* libmavconn
* Copyright 2013,2014,2015,2016 Vladimir Ermakov, All rights reserved.
*
* This file is part of the mavros package and subject to the license terms
* in the top-level LICENSE file of the mavros repository.
* https://github.com/mavlink/mavros/tree/master/LICENSE.md
*/
#ifndef ASYNC_CAN_HPP
#define ASYNC_CAN_HPP
#include <linux/can.h>
#include <atomic>
#include <chrono>
#include <thread>
#include <mutex>
#include <deque>
#include <functional>
#include <iostream>
#include "asio.hpp"
#include "asio/posix/basic_stream_descriptor.hpp"
// #include "async_io/device_error.hpp"
// #include "async_io/msg_buffer.hpp"
namespace wescore
{
using steady_clock = std::chrono::steady_clock;
using lock_guard = std::lock_guard<std::recursive_mutex>;
class ASyncCAN : public std::enable_shared_from_this<ASyncCAN>
{
public:
static constexpr auto DEFAULT_DEVICE = "can1";
static constexpr std::size_t MAX_TXQ_SIZE = 1000;
using ReceiveCallback = std::function<void(can_frame *rx_frame)>;
using ClosedCallback = std::function<void(void)>;
using Ptr = std::shared_ptr<ASyncCAN>;
using ConstPtr = std::shared_ptr<ASyncCAN const>;
using WeakPtr = std::weak_ptr<ASyncCAN>;
struct IOStat
{
std::size_t tx_total_frames; // total bytes transferred
std::size_t rx_total_frames; // total bytes received
float tx_speed; // current transfer speed [Frames/s]
float rx_speed; // current receive speed [Frames/s]
};
public:
ASyncCAN(std::string device = DEFAULT_DEVICE);
~ASyncCAN();
// do not allow copy
ASyncCAN(const ASyncCAN &other) = delete;
ASyncCAN &operator=(const ASyncCAN &other) = delete;
std::size_t conn_id;
int can_fd_;
static Ptr open_url(std::string url);
void open(std::string device);
void close();
void set_receive_callback(ReceiveCallback cb) { receive_cb = cb; }
void set_closed_callback(ClosedCallback cb) { port_closed_cb = cb; }
inline bool is_open() { return true; }
IOStat get_iostat();
void send_frame(const can_frame &tx_frame);
private:
// monotonic counter (increment only)
bool can_interface_opened_ = false;
static std::atomic<std::size_t> conn_id_counter;
struct can_frame rcv_frame;
// port statistics
std::atomic<std::size_t> tx_total_frames;
std::atomic<std::size_t> rx_total_frames;
std::size_t last_tx_total_frames;
std::size_t last_rx_total_frames;
std::chrono::time_point<steady_clock> last_iostat;
std::recursive_mutex iostat_mutex;
// io service
asio::io_service io_service;
asio::posix::basic_stream_descriptor<> stream;
std::thread io_thread;
// callback objects
ClosedCallback port_closed_cb;
ReceiveCallback receive_cb;
// internal processing
void do_read(struct can_frame &rec_frame, asio::posix::basic_stream_descriptor<> &stream);
void do_write(bool check_tx_state);
void call_receive_callback(can_frame *rx_frame);
void default_receive_callback(can_frame *rx_frame);
void iostat_tx_add(std::size_t bytes);
void iostat_rx_add(std::size_t bytes);
};
} // namespace wescore
#endif /* ASYNC_CAN_HPP */

145
include/wrp_sdk/async_io/async_serial.hpp Normal file
View File

@@ -0,0 +1,145 @@
/*
* async_serial.hpp
*
* Created on: Nov 23, 2018 22:18
* Description: asynchronous serial communication using asio
* adapted from code in libmavconn
*
* Main changes: 1. Removed dependency on Boost (asio standalone
* and C++ STL only)
* 2. Removed dependency on console-bridge
* 3. Removed mavlink related code
* 4. Removed UDP/TCP related code
*
* Author: Vladimir Ermakov <vooon341@gmail.com>
* Ruixiang Du <ruixiang.du@gmail.com>
*
* Additioanl reference:
* [1] http://www.webalice.it/fede.tft/serial_port/serial_port.html
*
*/
/*
* libmavconn
* Copyright 2013,2014,2015,2016 Vladimir Ermakov, All rights reserved.
*
* This file is part of the mavros package and subject to the license terms
* in the top-level LICENSE file of the mavros repository.
* https://github.com/mavlink/mavros/tree/master/LICENSE.md
*/
#ifndef ASYNC_SERIAL_HPP
#define ASYNC_SERIAL_HPP
#include <atomic>
#include <chrono>
#include <thread>
#include <mutex>
#include <deque>
#include <functional>
#include "asio.hpp"
#include "wrp_sdk/async_io/device_error.hpp"
#include "wrp_sdk/async_io/msg_buffer.hpp"
namespace wescore
{
using steady_clock = std::chrono::steady_clock;
using lock_guard = std::lock_guard<std::recursive_mutex>;
/// Note: instance of ASyncSerial MUST be managed by a std::shared_ptr<ASyncSerial>
class ASyncSerial : public std::enable_shared_from_this<ASyncSerial>
{
public:
static constexpr auto DEFAULT_DEVICE = "/dev/ttyUSB0";
static constexpr auto DEFAULT_BAUDRATE = 115200;
static constexpr std::size_t MAX_TXQ_SIZE = 1000;
/**
* @param buf: pointer to a buffer
* @param bufsize: size of the buffer (the value should be constant in most cases)
* @param bytes_received: number of bytes received inside the buffer
*/
using ReceiveCallback = std::function<void(uint8_t *buf, const size_t bufsize, size_t bytes_received)>;
using ClosedCallback = std::function<void(void)>;
using Ptr = std::shared_ptr<ASyncSerial>;
using ConstPtr = std::shared_ptr<ASyncSerial const>;
using WeakPtr = std::weak_ptr<ASyncSerial>;
struct IOStat
{
std::size_t tx_total_bytes; //!< total bytes transferred
std::size_t rx_total_bytes; //!< total bytes received
float tx_speed; //!< current transfer speed [B/s]
float rx_speed; //!< current receive speed [B/s]
};
public:
ASyncSerial(std::string device = DEFAULT_DEVICE, unsigned baudrate = DEFAULT_BAUDRATE, bool hwflow = false);
~ASyncSerial();
// do not allow copy
ASyncSerial(const ASyncSerial &other) = delete;
ASyncSerial &operator=(const ASyncSerial &other) = delete;
std::size_t conn_id;
static Ptr open_url(std::string url);
void open(std::string device = "", unsigned baudrate = 0, bool hwflow = false);
void close();
void set_baud(unsigned baudrate) { serial_dev_.set_option(asio::serial_port_base::baud_rate(baudrate)); }
void send_bytes(const uint8_t *bytes, size_t length);
void set_receive_callback(ReceiveCallback cb) { receive_cb = cb; }
void set_closed_callback(ClosedCallback cb) { port_closed_cb = cb; }
inline bool is_open() { return serial_dev_.is_open(); }
IOStat get_iostat();
private:
// monotonic counter (increment only)
static std::atomic<std::size_t> conn_id_counter;
// port properties
std::string device_ = DEFAULT_DEVICE;
unsigned baudrate_ = DEFAULT_BAUDRATE;
bool hwflow_ = false;
// port statistics
std::atomic<std::size_t> tx_total_bytes;
std::atomic<std::size_t> rx_total_bytes;
std::size_t last_tx_total_bytes;
std::size_t last_rx_total_bytes;
std::chrono::time_point<steady_clock> last_iostat;
std::recursive_mutex iostat_mutex;
// io service
asio::io_service io_service;
std::thread io_thread;
asio::serial_port serial_dev_;
std::atomic<bool> tx_in_progress;
std::deque<MsgBuffer> tx_q;
std::array<uint8_t, MsgBuffer::MAX_SIZE> rx_buf;
std::recursive_mutex mutex;
// callback objects
ClosedCallback port_closed_cb;
ReceiveCallback receive_cb;
// internal processing
void do_read();
void do_write(bool check_tx_state);
void call_receive_callback(uint8_t *buf, const std::size_t bufsize, std::size_t bytes_received);
void default_receive_callback(uint8_t *buf, const std::size_t bufsize, std::size_t bytes_received);
void iostat_tx_add(std::size_t bytes);
void iostat_rx_add(std::size_t bytes);
};
} // namespace wescore
#endif /* ASYNC_SERIAL_HPP */

57
include/wrp_sdk/async_io/device_error.hpp Normal file
View File

@@ -0,0 +1,57 @@
/**
* @brief MAVConn device error class
* @file device_error.hpp
* @author Vladimir Ermakov <vooon341@gmail.com>
*/
/*
* libmavconn
* Copyright 2013,2014,2015,2016 Vladimir Ermakov, All rights reserved.
*
* This file is part of the mavros package and subject to the license terms
* in the top-level LICENSE file of the mavros repository.
* https://github.com/mavlink/mavros/tree/master/LICENSE.md
*/
#ifndef DEVICE_ERROR_HPP
#define DEVICE_ERROR_HPP
#include <string>
#include <cstring>
#include <stdexcept>
namespace wescore
{
class DeviceError : public std::runtime_error
{
public:
template <typename T>
DeviceError(const char *module, T msg) : std::runtime_error(make_message(module, msg))
{
}
template <typename T>
static std::string make_message(const char *module, T msg)
{
std::ostringstream ss;
ss << "DeviceError:" << module << ":" << msg_to_string(msg);
return ss.str();
}
static std::string msg_to_string(const char *description)
{
return description;
}
static std::string msg_to_string(int errnum)
{
return std::strerror(errnum);
}
static std::string msg_to_string(std::system_error &err)
{
return err.what();
}
};
} // namespace mavconn
#endif /* DEVICE_ERROR_HPP */

69
include/wrp_sdk/async_io/msg_buffer.hpp Normal file
View File

@@ -0,0 +1,69 @@
/**
* @brief MAVConn message buffer class (internal)
* @file msgbuffer.hpp
* @author Vladimir Ermakov <vooon341@gmail.com>
*/
/*
* libmavconn
* Copyright 2014,2015,2016 Vladimir Ermakov, All rights reserved.
*
* This file is part of the mavros package and subject to the license terms
* in the top-level LICENSE file of the mavros repository.
* https://github.com/mavlink/mavros/tree/master/LICENSE.md
*/
#pragma once
#include <sys/types.h>
#include <cstdint>
#include <cstring>
#include <cassert>
namespace wescore
{
/**
* @brief Message buffer for internal use in libmavconn
*/
struct MsgBuffer
{
//! Maximum buffer size with padding for CRC bytes (280 + padding)
static constexpr ssize_t MAX_PACKET_LEN = 255;
static constexpr ssize_t MAX_SIZE = MAX_PACKET_LEN + 16;
uint8_t data[MAX_SIZE];
ssize_t len;
ssize_t pos;
MsgBuffer() : pos(0),
len(0)
{
}
/**
* @brief Buffer constructor for send_bytes()
* @param[in] nbytes should be less than MAX_SIZE
*/
MsgBuffer(const uint8_t *bytes, ssize_t nbytes) : pos(0),
len(nbytes)
{
assert(0 < nbytes && nbytes < MAX_SIZE);
std::memcpy(data, bytes, nbytes);
}
virtual ~MsgBuffer()
{
pos = 0;
len = 0;
}
uint8_t *dpos()
{
return data + pos;
}
ssize_t nbytes()
{
return len - pos;
}
};
} // namespace wescore

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

@@ -0,0 +1,99 @@
/*
* 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 <functional>
#include "wrp_sdk/async_io/async_can.hpp"
#include "wrp_sdk/async_io/async_serial.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 wescore
{
class HunterBase
{
public:
HunterBase() = default;
~HunterBase();
// do not allow copy
HunterBase(const HunterBase &hunter) = delete;
HunterBase &operator=(const HunterBase &hunter) = delete;
public:
// connect to roboot from CAN
void Connect(std::string dev_name);
// disconnect from roboot, only valid for serial port
void Disconnect();
// cmd thread runs at 100Hz (10ms) by default
void SetCmdThreadPeriodMs(int32_t period_ms) { cmd_thread_period_ms_ = period_ms; };
// motion control
void SetMotionCommand(double linear_vel, double steering_angle,
HunterMotionCmd::FaultClearFlag fault_clr_flag = HunterMotionCmd::FaultClearFlag::NO_FAULT);
// get robot state
HunterState GetHunterState();
private:
// hardware communication interface
std::shared_ptr<ASyncCAN> can_if_;
std::shared_ptr<ASyncSerial> serial_if_;
// CAN priority higher than serial if both connected
bool can_connected_ = false;
bool serial_connected_ = false;
// serial port related variables
uint8_t tx_cmd_len_;
uint8_t tx_buffer_[HUNTER_CMD_BUF_LEN];
// cmd/status update related variables
std::thread cmd_thread_;
std::mutex hunter_state_mutex_;
std::mutex motion_cmd_mutex_;
HunterState hunter_state_;
HunterMotionCmd current_motion_cmd_;
int32_t cmd_thread_period_ms_ = 10;
bool cmd_thread_started_ = false;
// 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 StartCmdThread();
void ControlLoop(int32_t period_ms);
void SendMotionCmd(uint8_t count);
void ParseCANFrame(can_frame *rx_frame);
void ParseUARTBuffer(uint8_t *buf, const size_t bufsize, size_t bytes_received);
void NewStatusMsgReceivedCallback(const HunterMessage &msg);
public:
static void UpdateHunterState(const HunterMessage &status_msg, HunterState &state);
};
} // namespace wescore
#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 */

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

@@ -0,0 +1,74 @@
/*
* 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 wescore
{
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
MotorState motor_states[3];
// 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 wescore
#endif /* HUNTER_TYPES_HPP */

110
include/wrp_sdk/platforms/scout/scout_base.hpp Normal file
View File

@@ -0,0 +1,110 @@
/*
* scout_base.hpp
*
* Created on: Jun 04, 2019 01:22
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#ifndef SCOUT_BASE_HPP
#define SCOUT_BASE_HPP
#include <string>
#include <cstdint>
#include <thread>
#include <mutex>
#include <functional>
#include "wrp_sdk/async_io/async_can.hpp"
#include "wrp_sdk/async_io/async_serial.hpp"
#include "wrp_sdk/platforms/scout/scout_protocol.h"
#include "wrp_sdk/platforms/scout/scout_can_parser.h"
#include "wrp_sdk/platforms/scout/scout_uart_parser.h"
#include "wrp_sdk/platforms/scout/scout_types.hpp"
namespace wescore
{
class ScoutBase
{
public:
ScoutBase() = default;
~ScoutBase();
// do not allow copy
ScoutBase(const ScoutBase &scout) = delete;
ScoutBase &operator=(const ScoutBase &scout) = delete;
public:
// connect to roboot from CAN or serial
void Connect(std::string dev_name, int32_t baud_rate = 0);
// disconnect from roboot, only valid for serial port
void Disconnect();
// cmd thread runs at 100Hz (10ms) by default
void SetCmdThreadPeriodMs(int32_t period_ms) { cmd_thread_period_ms_ = period_ms; };
// motion control
void SetMotionCommand(double linear_vel, double angular_vel,
ScoutMotionCmd::FaultClearFlag fault_clr_flag = ScoutMotionCmd::FaultClearFlag::NO_FAULT);
// light control
void SetLightCommand(ScoutLightCmd cmd);
void DisableLightCmdControl();
// get robot state
ScoutState GetScoutState();
private:
// hardware communication interface
std::shared_ptr<ASyncCAN> can_if_;
std::shared_ptr<ASyncSerial> serial_if_;
// CAN priority higher than serial if both connected
bool can_connected_ = false;
bool serial_connected_ = false;
// serial port related variables
uint8_t tx_cmd_len_;
uint8_t tx_buffer_[SCOUT_CMD_BUF_LEN];
// cmd/status update related variables
std::thread cmd_thread_;
std::mutex scout_state_mutex_;
std::mutex motion_cmd_mutex_;
std::mutex light_cmd_mutex_;
ScoutState scout_state_;
ScoutMotionCmd current_motion_cmd_;
ScoutLightCmd current_light_cmd_;
int32_t cmd_thread_period_ms_ = 10;
bool cmd_thread_started_ = false;
bool light_ctrl_enabled_ = false;
bool light_ctrl_requested_ = false;
// 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 StartCmdThread();
void ControlLoop(int32_t period_ms);
void SendMotionCmd(uint8_t count);
void SendLightCmd(uint8_t count);
void ParseCANFrame(can_frame *rx_frame);
void ParseUARTBuffer(uint8_t *buf, const size_t bufsize, size_t bytes_received);
void NewStatusMsgReceivedCallback(const ScoutMessage &msg);
public:
static void UpdateScoutState(const ScoutMessage &status_msg, ScoutState &state);
};
} // namespace wescore
#endif /* SCOUT_BASE_HPP */

42
include/wrp_sdk/platforms/scout/scout_can_parser.h Normal file
View File

@@ -0,0 +1,42 @@
/*
* scout_can_parser.h
*
* Created on: Aug 31, 2019 04:23
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#ifndef SCOUT_CAN_PARSER_H
#define SCOUT_CAN_PARSER_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include <stdbool.h>
#include "wrp_sdk/platforms/scout/scout_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 DecodeScoutMsgFromCAN(const struct can_frame *rx_frame, ScoutMessage *msg);
void EncodeScoutMsgToCAN(const ScoutMessage *msg, struct can_frame *tx_frame);
uint8_t CalcScoutCANChecksum(uint16_t id, uint8_t *data, uint8_t dlc);
#ifdef __cplusplus
}
#endif
#endif /* SCOUT_CAN_PARSER_H */

275
include/wrp_sdk/platforms/scout/scout_protocol.h Normal file
View File

@@ -0,0 +1,275 @@
/*
* scout_protocol.h
*
* Created on: Aug 07, 2019 21:49
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#ifndef SCOUT_PROTOCOL_H
#define SCOUT_PROTOCOL_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#define SCOUT_CMD_BUF_LEN 32
#define SCOUT_STATUS_BUF_LEN 32
#define SCOUT_FRAME_SIZE 13
#define SCOUT_MOTOR1_ID ((uint8_t)0x00)
#define SCOUT_MOTOR2_ID ((uint8_t)0x01)
#define SCOUT_MOTOR3_ID ((uint8_t)0x02)
#define SCOUT_MOTOR4_ID ((uint8_t)0x03)
// UART Definitions
#define UART_FRAME_SYSTEM_STATUS_ID ((uint8_t)0x01)
#define UART_FRAME_MOTION_STATUS_ID ((uint8_t)0x02)
#define UART_FRAME_MOTOR1_DRIVER_STATUS_ID ((uint8_t)0x03)
#define UART_FRAME_MOTOR2_DRIVER_STATUS_ID ((uint8_t)0x04)
#define UART_FRAME_MOTOR3_DRIVER_STATUS_ID ((uint8_t)0x05)
#define UART_FRAME_MOTOR4_DRIVER_STATUS_ID ((uint8_t)0x06)
#define UART_FRAME_LIGHT_STATUS_ID ((uint8_t)0x07)
#define UART_FRAME_MOTION_CONTROL_ID ((uint8_t)0x01)
#define UART_FRAME_LIGHT_CONTROL_ID ((uint8_t)0x02)
// CAN Definitions
#define CAN_MSG_MOTION_CONTROL_CMD_ID ((uint32_t)0x130)
#define CAN_MSG_MOTION_CONTROL_STATUS_ID ((uint32_t)0x131)
#define CAN_MSG_LIGHT_CONTROL_CMD_ID ((uint32_t)0x140)
#define CAN_MSG_LIGHT_CONTROL_STATUS_ID ((uint32_t)0x141)
#define CAN_MSG_SYSTEM_STATUS_STATUS_ID ((uint32_t)0x151)
#define CAN_MSG_MOTOR1_DRIVER_STATUS_ID ((uint32_t)0x200)
#define CAN_MSG_MOTOR2_DRIVER_STATUS_ID ((uint32_t)0x201)
#define CAN_MSG_MOTOR3_DRIVER_STATUS_ID ((uint32_t)0x202)
#define CAN_MSG_MOTOR4_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)
// Light Control
#define LIGHT_DISABLE_CTRL ((uint8_t)0x00)
#define LIGHT_ENABLE_CTRL ((uint8_t)0x01)
#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)
// 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_MOTOR_DRV_OVERHEAT_W ((uint16_t)0x0200)
#define FAULT_MOTOR_OVERCURRENT_W ((uint16_t)0x0400)
#define FAULT_BAT_UNDER_VOL_W ((uint16_t)0x0800)
#define FAULT_RC_SIGNAL_LOSS ((uint16_t)0x1000)
#define FAULT_HIGH_BYTE_RESERVED2 ((uint16_t)0x2000)
#define FAULT_HIGH_BYTE_RESERVED3 ((uint16_t)0x4000)
#define FAULT_HIGH_BYTE_RESERVED4 ((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;
} MotionControlMessage;
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;
// Light Control
typedef struct {
union
{
struct
{
uint8_t light_ctrl_enable;
uint8_t front_light_mode;
uint8_t front_light_custom;
uint8_t rear_light_mode;
uint8_t rear_light_custom;
uint8_t reserved0;
uint8_t count;
uint8_t checksum;
} cmd;
uint8_t raw[8];
} data;
} LightControlMessage;
typedef struct {
union
{
struct
{
uint8_t light_ctrl_enable;
uint8_t front_light_mode;
uint8_t front_light_custom;
uint8_t rear_light_mode;
uint8_t rear_light_custom;
uint8_t reserved0;
uint8_t count;
uint8_t checksum;
} status;
uint8_t raw[8];
} data;
} LightStatusMessage;
// 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
{
ScoutMsgNone = 0x00,
// status messages
ScoutMotionStatusMsg = 0x01,
ScoutLightStatusMsg = 0x02,
ScoutSystemStatusMsg = 0x03,
ScoutMotorDriverStatusMsg = 0x04,
// control messages
ScoutMotionControlMsg = 0x21,
ScoutLightControlMsg = 0x22
} ScoutMsgType;
typedef struct
{
ScoutMsgType type;
union {
// status messages
MotionStatusMessage motion_status_msg;
LightStatusMessage light_status_msg;
SystemStatusMessage system_status_msg;
MotorDriverStatusMessage motor_driver_status_msg;
// control messages
MotionControlMessage motion_control_msg;
LightControlMessage light_control_msg;
} body;
} ScoutMessage;
#pragma pack(pop)
#ifdef __cplusplus
}
#endif
#endif /* SCOUT_PROTOCOL_H */

111
include/wrp_sdk/platforms/scout/scout_types.hpp Normal file
View File

@@ -0,0 +1,111 @@
/*
* scout_state.hpp
*
* Created on: Jun 11, 2019 08:48
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#ifndef SCOUT_STATE_HPP
#define SCOUT_STATE_HPP
#include <cstdint>
#include <iostream>
namespace wescore
{
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 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;
// motor state
MotorState motor_states[4];
// 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;
};
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) {}
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
};
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 wescore
#endif /* SCOUT_STATE_HPP */

32
include/wrp_sdk/platforms/scout/scout_uart_parser.h Normal file
View File

@@ -0,0 +1,32 @@
/*
* scout_uart_parser.h
*
* Created on: Aug 14, 2019 12:01
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#ifndef SCOUT_UART_PARSER_H
#define SCOUT_UART_PARSER_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include <string.h>
#include <stdbool.h>
#include "wrp_sdk/platforms/scout/scout_protocol.h"
bool DecodeScoutMsgFromUART(uint8_t c, ScoutMessage *msg);
void EncodeScoutMsgToUART(const ScoutMessage *msg, uint8_t *buf, uint8_t *len);
uint8_t CalcScoutUARTChecksum(uint8_t *buf, uint8_t len);
#ifdef __cplusplus
}
#endif
#endif /* SCOUT_UART_PARSER_H */

109
include/wrp_sdk/platforms/tracer/tracer_base.hpp Normal file
View File

@@ -0,0 +1,109 @@
/*
* tracer_base.hpp
*
* Created on: Apr 14, 2020 10:21
* Description:
*
* Copyright (c) 2020 Ruixiang Du (rdu)
*/
#ifndef TRACER_BASE_HPP
#define TRACER_BASE_HPP
#include <string>
#include <cstdint>
#include <thread>
#include <mutex>
#include <functional>
#include "async_io/async_can.hpp"
#include "async_io/async_serial.hpp"
#include "tracer_protocol/tracer_protocol.h"
#include "tracer_protocol/tracer_can_parser.h"
#include "tracer_base/tracer_types.hpp"
namespace wescore
{
class TracerBase
{
public:
TracerBase() = default;
~TracerBase();
// do not allow copy
TracerBase(const TracerBase &tracer) = delete;
TracerBase &operator=(const TracerBase &tracer) = delete;
public:
// connect to roboot from CAN
void Connect(std::string dev_name);
// disconnect from roboot, only valid for serial port
void Disconnect();
// cmd thread runs at 100Hz (10ms) by default
void SetCmdThreadPeriodMs(int32_t period_ms) { cmd_thread_period_ms_ = period_ms; };
// motion control
void SetMotionCommand(double linear_vel, double angular_vel,
TracerMotionCmd::FaultClearFlag fault_clr_flag = TracerMotionCmd::FaultClearFlag::NO_FAULT);
// light control
void SetLightCommand(TracerLightCmd cmd);
void DisableLightCmdControl();
// get robot state
TracerState GetTracerState();
private:
// hardware communication interface
std::shared_ptr<ASyncCAN> can_if_;
std::shared_ptr<ASyncSerial> serial_if_;
// CAN priority higher than serial if both connected
bool can_connected_ = false;
bool serial_connected_ = false;
// serial port related variables
uint8_t tx_cmd_len_;
uint8_t tx_buffer_[TRACER_CMD_BUF_LEN];
// cmd/status update related variables
std::thread cmd_thread_;
std::mutex tracer_state_mutex_;
std::mutex motion_cmd_mutex_;
std::mutex light_cmd_mutex_;
TracerState tracer_state_;
TracerMotionCmd current_motion_cmd_;
TracerLightCmd current_light_cmd_;
int32_t cmd_thread_period_ms_ = 10;
bool cmd_thread_started_ = false;
bool light_ctrl_enabled_ = false;
bool light_ctrl_requested_ = false;
// 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 StartCmdThread();
void ControlLoop(int32_t period_ms);
void SendMotionCmd(uint8_t count);
void SendLightCmd(uint8_t count);
void ParseCANFrame(can_frame *rx_frame);
void ParseUARTBuffer(uint8_t *buf, const size_t bufsize, size_t bytes_received);
void NewStatusMsgReceivedCallback(const TracerMessage &msg);
public:
static void UpdateTracerState(const TracerMessage &status_msg, TracerState &state);
};
} // namespace wescore
#endif /* TRACER_BASE_HPP */

42
include/wrp_sdk/platforms/tracer/tracer_can_parser.h Normal file
View File

@@ -0,0 +1,42 @@
/*
* tracer_can_parser.h
*
* Created on: Apr 14, 2020 10:52
* Description:
*
* Copyright (c) 2020 Ruixiang Du (rdu)
*/
#ifndef TRACER_CAN_PARSER_H
#define TRACER_CAN_PARSER_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include <stdbool.h>
#include "tracer_protocol/tracer_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 DecodeTracerMsgFromCAN(const struct can_frame *rx_frame, TracerMessage *msg);
void EncodeTracerMsgToCAN(const TracerMessage *msg, struct can_frame *tx_frame);
uint8_t CalcTracerCANChecksum(uint16_t id, uint8_t *data, uint8_t dlc);
#ifdef __cplusplus
}
#endif
#endif /* TRACER_CAN_PARSER_H */

259
include/wrp_sdk/platforms/tracer/tracer_protocol.h Normal file
View File

@@ -0,0 +1,259 @@
/*
* tracer_protocol.h
*
* Created on: Apr 14, 2020 10:34
* Description:
*
* Copyright (c) 2020 Ruixiang Du (rdu)
*/
#ifndef TRACER_PROTOCOL_H
#define TRACER_PROTOCOL_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#define TRACER_CMD_BUF_LEN 32
#define TRACER_STATUS_BUF_LEN 32
#define TRACER_FRAME_SIZE 13
#define TRACER_MOTOR1_ID ((uint8_t)0x00)
#define TRACER_MOTOR2_ID ((uint8_t)0x01)
// CAN Definitions
#define CAN_MSG_MOTION_CMD_ID ((uint32_t)0x130)
#define CAN_MSG_MOTION_STATUS_ID ((uint32_t)0x131)
#define CAN_MSG_LIGHT_CONTROL_CMD_ID ((uint32_t)0x140)
#define CAN_MSG_LIGHT_CONTROL_STATUS_ID ((uint32_t)0x141)
#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)
/*--------------------- 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)
// Light Control
#define LIGHT_DISABLE_CTRL ((uint8_t)0x00)
#define LIGHT_ENABLE_CTRL ((uint8_t)0x01)
#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)
// 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_RESERVED1 ((uint16_t)0x0010)
#define FAULT_RESERVED2 ((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;
// Light Control
typedef struct {
union
{
struct
{
uint8_t light_ctrl_enable;
uint8_t front_light_mode;
uint8_t front_light_custom;
uint8_t rear_light_mode;
uint8_t rear_light_custom;
uint8_t reserved0;
uint8_t count;
uint8_t checksum;
} cmd;
uint8_t raw[8];
} data;
} LightControlMessage;
typedef struct {
union
{
struct
{
uint8_t light_ctrl_enable;
uint8_t front_light_mode;
uint8_t front_light_custom;
uint8_t rear_light_mode;
uint8_t rear_light_custom;
uint8_t reserved0;
uint8_t count;
uint8_t checksum;
} status;
uint8_t raw[8];
} data;
} LightStatusMessage;
// 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
{
TracerMsgNone = 0x00,
// status messages
TracerMotionStatusMsg = 0x01,
TracerLightStatusMsg = 0x02,
TracerSystemStatusMsg = 0x03,
TracerMotorDriverStatusMsg = 0x04,
// control messages
TracerMotionCmdMsg = 0x21,
TracerLightControlMsg = 0x22
} TracerMsgType;
typedef struct
{
TracerMsgType type;
union {
// status messages
MotionStatusMessage motion_status_msg;
LightStatusMessage light_status_msg;
SystemStatusMessage system_status_msg;
MotorDriverStatusMessage motor_driver_status_msg;
// control messages
MotionCmdMessage motion_cmd_msg;
LightControlMessage light_control_msg;
} body;
} TracerMessage;
#pragma pack(pop)
#ifdef __cplusplus
}
#endif
#endif /* TRACER_PROTOCOL_H */

111
include/wrp_sdk/platforms/tracer/tracer_types.hpp Normal file
View File

@@ -0,0 +1,111 @@
/*
* tracer_types.hpp
*
* Created on: Apr 14, 2020 10:22
* Description:
*
* Copyright (c) 2020 Ruixiang Du (rdu)
*/
#ifndef TRACER_TYPES_HPP
#define TRACER_TYPES_HPP
#include <cstdint>
#include <iostream>
namespace wescore
{
struct TracerState
{
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 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;
// motor state
MotorState motor_states[4];
// 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;
};
struct TracerMotionCmd
{
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
};
TracerMotionCmd(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
};
struct TracerLightCmd
{
enum class LightMode
{
CONST_OFF = 0x00,
CONST_ON = 0x01,
BREATH = 0x02,
CUSTOM = 0x03
};
TracerLightCmd() = default;
TracerLightCmd(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 wescore
#endif /* TRACER_TYPES_HPP */