HiepLM/AMR_T800
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

git commit -m "first commit for v2"

Browse Source
This commit is contained in:
HiepLM
2025-12-29 16:21:22 +07:00
commit aa3d832d5c
1807 changed files with 307078 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

112
Devices/Libraries/Ros/md_controller/include/md_controller/define.h Executable file
View File

@@ -0,0 +1,112 @@
#ifndef __MD_CTRL_DEFINE_H_INCLUDED
#define __MD_CTRL_DEFINE_H_INCLUDED
#define HEADER_RMID_SEND 0xB7
#define HEADER_TMID_SEND 0XAC
#define MAX_BUFFER_LENGHT 2084
#define PACKAGE_MSG_MAIN_DATA 22
#define MAX_ID_LENGHT 253
#define TIME_PUBLISH_TO_SEND 7500 //us
/* Frame of messgae package*/
typedef struct _Header
{
uint8_t RMID = HEADER_RMID_SEND;
uint8_t TMID;
}header_t;
typedef struct _ID_Number
{
uint8_t adr[MAX_ID_LENGHT];
uint8_t size;
}id_number_t;
typedef struct _Parameter_ID
{
/* data */
uint8_t PID;
}param_id_t;
typedef struct Data_number
{
uint8_t NUM;
}dt_num_t;
/***********************************************************************************************************/
typedef struct _filter
{
uint8_t _buffer[MAX_BUFFER_LENGHT];
uint8_t result[PACKAGE_MSG_MAIN_DATA];
unsigned long r_index;
unsigned long w_index;
unsigned long c_flag;
}filter_t;
enum PID_COMMAND
{
CMD_TQ_OFF = 2, // Tq-off, motor free state
CMD_BRAKE = 4, // Erectric brake
CMD_MAIN_BC_ON = 5, // PID_MAIN_DATA broadcasting ON
CMD_MAIN_BC_OFF = 6, // broadcasing OFF
CMD_ALARM_RESET = 8, // Reset alarm
CMD_POSI_RESET = 10, // Position reset(set position to zero)
CMD_MONITOR_BC_ON = 11, // PID_MONITOR broadcasting ON
CMD_MONITOR_BC_OFF = 12, // Broadcasting off
CMD_IO_MONITOR_ON = 13, // PID_IO_MONITOR BC ON
CMD_IO_MONITOR_OFF = 14, // PID_IO_MONITOR BC OFF
CMD_FAN_ON = 15, // Fan ON(motor cooling fan)
CMD_FAN_OFF = 16, // Fan OFF
CMD_CLUTCH_ON = 17, // Mechanical brake(clutch) ON
CMD_CLUTCH_OFF = 18, // Mechanical breka OFF
CMD_TAR_VEL_OFF = 20, // Erase target vel, set by PID_TAR_VEL
CMD_SLOW_START_OFF = 21, // Erase target slow/start value
CMD_SLOW_DOWN_OFF = 22, // Erase target slow/down vaule
CMD_CAN_RESEND_ON = 23, // Send CAN data to RS485 serial port.
CMD_CAN_RESEND_OFF = 24, // Turn off resending of CAN data
CMD_MAX_TQ_OFF = 25, // Erase target limit load(max. current)
CMD_ENC_OFF = 26, // Cancel the use of encoder sensor.
CMD_LOW_SPEED_LIMIT_OF = 27, // Cancel the set of low speed limit
F_HIGH = 28, // Cancel the set of high speed limit.
CMD_SPEED_LI_OFF =29, // Cancel the set of low/high speed limits
CMD_CURVE_OFF = 31, // Cancel set of curve fitting func.
CMD_STEP_OFF = 32, // Cancel step input mode
CMD_UICOM_OFF = 44, // I/O control(ctrl 11pin cnt) available
CMD_UICOM_ON = 45, // I/O control disable(when comm. is used)
CMD_MAX_RP_OFF = 46, // Cancel max. speed set by DIP SW
CMD_HALL_TY_OFF = 47, // Cancel set of motor hall type
CMD_LOW_POT_OFF = 48, // Cancel set of low limit of POT input
CMD_HIGH_POT_OFF = 49, // Cancel set of high limit of POT input
CMD_MAIN_BC_ON2 = 50, // PID_MAIN_DATA, BC ON for 2nd motor
CMD_MAIN_BC_OFF2 = 51, // PID_MAIN_DATA, BC OFF for 2nd motor
CMD_MONIT_BC_ON2 = 52, // PID_MONITOR, BC ON for 2nd motor
CMD_MONIT_BC_OFF2 = 53, // PID_MONITOR, BC OFF for 2nd motor
CMD_IO_MONIT_BC_ON2 = 54, // PID_IO_MONITOR, BC ON for 2nd motor
CMD_IO_MONIT_BC_OFF2 = 55 // PID_IO_MONITOR, BC OFF for 2nd motor
};
enum _BIT
{
_OFF = 0x00u,
_ON = 0x01u
};
enum _BAUD_OF_MD200
{
_9600bps = 1,
_19200bps = 2,
_38400bps = 3,
_57600bps = 4,
_115200bps = 5
};
typedef enum EXIT_CODES_ENUM
{
EXIT_OK = 0,
EXIT_ERROR = 1,
EXIT_CONFIG_ERROR = -1
} EXIT_CODES;
#endif // __MD_CTRL_DEFINE_H_INCLUDED

94
Devices/Libraries/Ros/md_controller/include/md_controller/diagnostic.h Executable file
View File

@@ -0,0 +1,94 @@
#ifndef __MD_CTRL_DIAGNOTICS_H_INCLUDED
#define __MD_CTRL_DIAGNOTICS_H_INCLUDED
#include <ros/ros.h>
namespace MD
{
typedef enum DIAGNOSTIC_STATUS_ENUM
{
OK, // status okay, no errors
EXIT, // delta exiting
ERROR_STATUS, // device signaled an error
CONFIGURATION_ERROR, // invalid configuration, check configuration files
INITIALIZATION_ERROR, // initialization of driver failed
INTERNAL_ERROR // internal error, should never happen
} DIAGNOSTIC_STATUS;
/*
* class Diagnostic publishes diagnostic messages for sick_line_guidance
*/
class Diagnostic
{
public:
/*
* Initializes the global diagnostic handler.
*
* @param[in] nh ros::NodeHandle
* @param[in] publish_topic ros topic to publish diagnostic messages
* @param[in] component description of the component reporting
*/
static void init(ros::NodeHandle &nh, const std::string & publish_topic, const std::string & component)
{
g_diagnostic_handler.init(nh, publish_topic, component);
}
/*
* Updates and reports the current status.
*
* @param[in] status current status (OK, ERROR, ...)
* @param[in] message optional diagnostic message
*/
static void update(DIAGNOSTIC_STATUS status, const std::string & message = "")
{
g_diagnostic_handler.update(status, message);
}
protected:
/*
* class DiagnosticImpl implements diagnostic for sick_line_guidance
*/
class DiagnosticImpl
{
public:
/*
* Constructor.
*/
DiagnosticImpl();
/*
* Initialization.
*
* @param[in] nh ros::NodeHandle
* @param[in] publish_topic ros topic to publish diagnostic messages
* @param[in] component description of the component reporting
*/
void init(ros::NodeHandle & nh, const std::string & publish_topic, const std::string & component);
/*
* Updates and reports the current status.
*
* @param[in] status current status (OK, ERROR, ...)
* @param[in] message optional diagnostic message
*/
void update(DIAGNOSTIC_STATUS status, const std::string & message = "");
protected:
/*
* member data.
*/
bool m_diagnostic_initialized; // flag indicating proper initialization of diagnostic
ros::Publisher m_diagnostic_publisher; // publishes diagnostic messages
std::string m_diagnostic_component; // name of the component publishing diagnostic messages
}; // class DiagnosticImpl
static DiagnosticImpl g_diagnostic_handler; // singleton, implements the diagnostic for sick_line_guidance
}; // class Diagnostic
}
#endif // __MD_CTRL_DIAGNOTICS_H_INCLUDED

78
Devices/Libraries/Ros/md_controller/include/md_controller/md_controller.h Executable file
View File

@@ -0,0 +1,78 @@
#ifndef __MD_CTRL_H_INCLUDED
#define __MD_CTRL_H_INCLUDED
#include <ros/ros.h>
#include <vector>
#include "libserial/rs485.h"
#include "md_controller/define.h"
#include "md_controller/diagnostic.h"
#include <boost/thread/thread.hpp>
namespace MD
{
// shortcut to read a member from a XmlRpcValue, or to return a defaultvalue, it the member does not exist
template<class T> static T readMember(XmlRpc::XmlRpcValue & value, const std::string & member, const T & defaultvalue)
{
if(value.hasMember(member))
return value[member];
return defaultvalue;
}
class MdController: public rs485
{
public:
MdController(ros::NodeHandlePtr nh, const char *devfile, unsigned int baud, parity_t parity, data_bits_t data_bit,stop_bits_t stop_bit);
virtual ~MdController();
// virtual void set_slave_id(int *ids, uint8_t lenght);
// virtual void publish_speed(int16_t *speed);
// virtual void main_BC_ON();
// virtual void main_BC_OFF();
/* COMMAND */
virtual void pid_defaut_set(uint8_t address);
virtual void pid_tq_off(uint8_t address);
virtual void pid_break(uint8_t address);
virtual void pid_stop(uint8_t address);
virtual void pid_command(uint8_t address, PID_COMMAND cmd);
virtual void pid_alarm_reset(uint8_t address);
virtual void pid_posi_reset(uint8_t address);
virtual void pid_main_bc_state(uint8_t address, _BIT data);
virtual void pid_monitor_bc_state(uint8_t address, _BIT data);
virtual void pid_preset_save(uint8_t address, uint8_t data);
virtual void pid_preset_recall(uint8_t address, uint8_t data);
virtual void pid_vel_cmd(uint8_t address, int16_t speed);
virtual void pid_vel_cmd2(uint8_t address, int16_t high_speed);
virtual void pid_open_vel_cmd(uint8_t address, int16_t data);
bool err{};
int err_no{};
private:
id_number_t slave_id;
uint8_t rate = 100;
boost::thread ThreadOne,ThreadTwo;
bool theadEnable;
filter_t cs;
ros::NodeHandlePtr nh_;
XmlRpc::XmlRpcValue drivers;
virtual uint8_t GetcheckSum(uint length, uint8_t *to_send);
virtual void buidRequest(uint8_t *to_send,header_t hd ,uint8_t add, param_id_t pid, dt_num_t num) const;
virtual int Read(uint8_t *read_buf, ssize_t &num);
virtual int Write(header_t hd ,uint8_t address, param_id_t pid, dt_num_t num, uint8_t *value);
virtual bool check_baudrate(unsigned int baud);
virtual void set_bad_con(void);
// virtual void loadParam(void);
/*Write*/
virtual void pid_id_setting(uint8_t id_setting);
virtual void pid_baudrate_setting(uint8_t address, _BAUD_OF_MD200 baudrate_setting);
/* READ */
// static void cmdMainBCdoStuff(void *payload, uint8_t *publish_rate);
};
} // namespace MD
#endif // __MD_CTRL_H_INCLUDED

70
Devices/Libraries/Ros/md_controller/include/md_controller/md_controller_subscriber.h Executable file
View File

@@ -0,0 +1,70 @@
#ifndef __MD_CTRL_SUBSCRIBER_H_INCLUDED
#define __MD_CTRL_SUBSCRIBER_H_INCLUDED
#include <ros/ros.h>
#include <boost/thread.hpp>
#include <std_msgs/Float32.h>
namespace MD
{
class Subscriber
{
public:
/*
* @brief Constructor.
* @param[in] nh ros::NodeHandle
* @param[in] ros_topic topic for ros messages
* @param[in] subscribe_queue_size buffer size for ros messages
* @param[in] wait_time Time for checking to Wheel stop (1 second)
*/
Subscriber(ros::NodeHandle & nh, const std::string & ros_topic, int subscribe_queue_size = 1, double schedule_delay = 0.05, double schedule_delay_lastest = 0.5);
Subscriber(ros::NodeHandle & nh, XmlRpc::XmlRpcValue::iterator object);
/**
* @brief returns true, if publishing of a wheel topic is publishing
*
*/
bool isWheelTriggered(void);
/**
* @brief returns true, if not subcribe wheel
*/
bool isWheelLastestTriggered(void);
/**
* @brief schedules WheelController function
* @param[in] schedule if true, publishing is scheduled, otherwise a possibly pending schedule is removed.
*/
void scheduleWheelController(bool schedule);
/*
* @brief Destructor.
*/
virtual ~Subscriber();
boost::mutex m_measurement_mutex;
int16_t speed;
uint8_t slave_id;
double ratio;
protected:
/*
* @brief Callbacks for ros messages from wheel topic.
*/
virtual void wheelCallback(const std_msgs::Float32::Ptr & msg);
ros::NodeHandle nh_;
std::string topic_name_;
int m_subscribe_queue_size_;
ros::Subscriber control_wheel_sub_;
ros::Time publish_time_;
ros::Time publish_lastest_time_;
ros::Duration schedule_delay_;
ros::Duration schedule_lastest_delay_;
boost::mutex publish_mutex_;
};
}
#endif // __MD_CTRL_SUBSCRIBER_H_INCLUDED