diff --git a/HiveCoreR0/src/ethercat_driver_ros2/ethercat_driver/include/ethercat_driver/ethercat_driver.hpp b/HiveCoreR0/src/ethercat_driver_ros2/ethercat_driver/include/ethercat_driver/ethercat_driver.hpp index 8862c0a..a4131fd 100644 --- a/HiveCoreR0/src/ethercat_driver_ros2/ethercat_driver/include/ethercat_driver/ethercat_driver.hpp +++ b/HiveCoreR0/src/ethercat_driver_ros2/ethercat_driver/include/ethercat_driver/ethercat_driver.hpp @@ -14,7 +14,7 @@ #ifndef ETHERCAT_DRIVER__ETHERCAT_DRIVER_HPP_ #define ETHERCAT_DRIVER__ETHERCAT_DRIVER_HPP_ - +#include #include #include #include @@ -64,7 +64,7 @@ public: ETHERCAT_DRIVER_PUBLIC hardware_interface::return_type write(const rclcpp::Time &, const rclcpp::Duration &) override; - + void watch_state(); private: std::vector> getEcModuleParam( std::string & urdf, std::string component_name, std::string component_type); @@ -86,6 +86,7 @@ private: ethercat_interface::EcMaster master_; std::mutex ec_mutex_; bool activated_; + }; } // namespace ethercat_driver diff --git a/HiveCoreR0/src/ethercat_driver_ros2/ethercat_driver/src/ethercat_driver.cpp b/HiveCoreR0/src/ethercat_driver_ros2/ethercat_driver/src/ethercat_driver.cpp index deea06b..5365d69 100644 --- a/HiveCoreR0/src/ethercat_driver_ros2/ethercat_driver/src/ethercat_driver.cpp +++ b/HiveCoreR0/src/ethercat_driver_ros2/ethercat_driver/src/ethercat_driver.cpp @@ -13,7 +13,6 @@ // limitations under the License. #include "ethercat_driver/ethercat_driver.hpp" - #include #include #include @@ -258,7 +257,29 @@ EthercatDriver::export_command_interfaces() } return command_interfaces; } - +void EthercatDriver::watch_state(){ + int print_cnt=0; + while(true){ + int xpos=master_.get_state(nullptr); + if(xpos>-1){ + for (auto i = 0ul; i < ec_modules_.size(); i++) { + uint16_t pos=std::stod(ec_module_parameters_[i]["position"]); + master_.set_state(pos,"OP"); + } + } + if(xpos>-1){ + print_cnt+=1; + if(print_cnt%10==0){ + std::cout<<"check state:"<watch_state(); + }); + ws.detach(); // calculate next shot. carry over nanoseconds into microseconds. t.tv_nsec += master_.getInterval(); while (t.tv_nsec >= 1000000000) { @@ -346,7 +380,7 @@ CallbackReturn EthercatDriver::on_activate( t.tv_sec++; } } - + //////std::cout<<"###################:"< header file. */ +#define HAVE_DLFCN_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_INTTYPES_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_STDINT_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_STDIO_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_STDLIB_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_STRINGS_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_STRING_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_SYS_STAT_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_SYS_TYPES_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_UNISTD_H 1 + +/* Define to the sub-directory where libtool stores uninstalled libraries. */ +#define LT_OBJDIR ".libs/" + +/* Name of package */ +#define PACKAGE "ethercat" + +/* Define to the address where bug reports for this package should be sent. */ +#define PACKAGE_BUGREPORT "fp@igh.de" + +/* Define to the full name of this package. */ +#define PACKAGE_NAME "ethercat" + +/* Define to the full name and version of this package. */ +#define PACKAGE_STRING "ethercat 1.5.3" + +/* Define to the one symbol short name of this package. */ +#define PACKAGE_TARNAME "ethercat" + +/* Define to the home page for this package. */ +#define PACKAGE_URL "" + +/* Define to the version of this package. */ +#define PACKAGE_VERSION "1.5.3" + +/* Define to 1 if all of the C90 standard headers exist (not just the ones + required in a freestanding environment). This macro is provided for + backward compatibility; new code need not use it. */ +#define STDC_HEADERS 1 + +/* Version number of package */ +#define VERSION "1.5.3" diff --git a/HiveCoreR0/src/ethercat_driver_ros2/ethercat_interface/include/ethercat_interface/ec_master.hpp b/HiveCoreR0/src/ethercat_driver_ros2/ethercat_interface/include/ethercat_interface/ec_master.hpp index 26ae64f..0bee253 100644 --- a/HiveCoreR0/src/ethercat_driver_ros2/ethercat_interface/include/ethercat_interface/ec_master.hpp +++ b/HiveCoreR0/src/ethercat_driver_ros2/ethercat_interface/include/ethercat_interface/ec_master.hpp @@ -33,7 +33,8 @@ class EcMaster public: explicit EcMaster(const int master = 0); virtual ~EcMaster(); - + int set_state(int pos,std::string sta); + int get_state(uint16_t* res); /** \brief add a slave device to the master * alias and position can be found by running the following command * /opt/etherlab/bin$ sudo ./ethercat slaves @@ -82,7 +83,9 @@ public: void setCtrlFrequency(double frequency) { + std::cout << "set control frequency to " << frequency << std::endl; interval_ = 1000000000.0 / frequency; + std::cout << "new interval_ " << interval_<< std::endl; } uint32_t getInterval() {return interval_;} diff --git a/HiveCoreR0/src/ethercat_driver_ros2/ethercat_interface/include/ethercat_interface/globals.h b/HiveCoreR0/src/ethercat_driver_ros2/ethercat_interface/include/ethercat_interface/globals.h new file mode 100644 index 0000000..a0c820d --- /dev/null +++ b/HiveCoreR0/src/ethercat_driver_ros2/ethercat_interface/include/ethercat_interface/globals.h @@ -0,0 +1,305 @@ +/****************************************************************************** + * + * Copyright (C) 2006-2021 Florian Pose, Ingenieurgemeinschaft IgH + * + * This file is part of the IgH EtherCAT master. + * + * The file is free software; you can redistribute it and/or modify it under + * the terms of the GNU Lesser General Public License as published by the + * Free Software Foundation; version 2.1 of the License. + * + * This file is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public + * License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with this file. If not, see . + * + *****************************************************************************/ + +/** \file + * Global definitions and macros. + */ + +/*****************************************************************************/ + +#ifndef __EC_MASTER_GLOBALS_H__ +#define __EC_MASTER_GLOBALS_H__ + +#include "../globals.h" +#include "../include/ecrt.h" + +/****************************************************************************** + * EtherCAT master + *****************************************************************************/ + +/** Datagram timeout in microseconds. */ +#define EC_IO_TIMEOUT 500 + +/** Time to send a byte in nanoseconds. + * + * t_ns = 1 / (100 MBit/s / 8 bit/byte) = 80 ns/byte + */ +#define EC_BYTE_TRANSMISSION_TIME_NS 80 + +/** Number of state machine retries on datagram timeout. */ +#define EC_FSM_RETRIES 3 + +/** Seconds to wait before fetching SDO dictionary + after slave entered PREOP state. */ +#define EC_WAIT_SDO_DICT 3 + +/** Minimum size of a buffer used with ec_state_string(). */ +#define EC_STATE_STRING_SIZE 32 + +/** Maximum SII size in words, to avoid infinite reading. */ +#define EC_MAX_SII_SIZE 4096 + +/** Number of statistic rate intervals to maintain. */ +#define EC_RATE_COUNT 3 + +/****************************************************************************** + * EtherCAT protocol + *****************************************************************************/ + +/** Size of an EtherCAT frame header. */ +#define EC_FRAME_HEADER_SIZE 2 + +/** Size of an EtherCAT datagram header. */ +#define EC_DATAGRAM_HEADER_SIZE 10 + +/** Size of an EtherCAT datagram footer. */ +#define EC_DATAGRAM_FOOTER_SIZE 2 + +/** Size of the EtherCAT address field. */ +#define EC_ADDR_LEN 4 + +/** Resulting maximum data size of a single datagram in a frame. */ +#define EC_MAX_DATA_SIZE (ETH_DATA_LEN - EC_FRAME_HEADER_SIZE \ + - EC_DATAGRAM_HEADER_SIZE - EC_DATAGRAM_FOOTER_SIZE) + +/** Mailbox header size. */ +#define EC_MBOX_HEADER_SIZE 6 + +/** Word offset of first SII category. */ +#define EC_FIRST_SII_CATEGORY_OFFSET 0x40 + +/** Size of a sync manager configuration page. */ +#define EC_SYNC_PAGE_SIZE 8 + +/** Maximum number of FMMUs per slave. */ +#define EC_MAX_FMMUS 16 + +/** Size of an FMMU configuration page. */ +#define EC_FMMU_PAGE_SIZE 16 + +/** Number of DC sync signals. */ +#define EC_SYNC_SIGNAL_COUNT 2 + +/** Size of the datagram description string. + * + * This is also used as the maximum lenth of EoE device names. + **/ +#define EC_DATAGRAM_NAME_SIZE 20 + +/** Slave state mask. + * + * Apply this mask to a slave state byte to get the slave state without + * the error flag. + */ +#define EC_SLAVE_STATE_MASK 0x0F + +/** State of an EtherCAT slave. + */ +typedef enum { + EC_SLAVE_STATE_UNKNOWN = 0x00, + /**< unknown state */ + EC_SLAVE_STATE_INIT = 0x01, + /**< INIT state (no mailbox communication, no IO) */ + EC_SLAVE_STATE_PREOP = 0x02, + /**< PREOP state (mailbox communication, no IO) */ + EC_SLAVE_STATE_BOOT = 0x03, + /**< Bootstrap state (mailbox communication, firmware update) */ + EC_SLAVE_STATE_SAFEOP = 0x04, + /**< SAFEOP (mailbox communication and input update) */ + EC_SLAVE_STATE_OP = 0x08, + /**< OP (mailbox communication and input/output update) */ + EC_SLAVE_STATE_ACK_ERR = 0x10 + /**< Acknowledge/Error bit (no actual state) */ +} ec_slave_state_t; + +/** Supported mailbox protocols. + */ +enum { + EC_MBOX_AOE = 0x01, /**< ADS over EtherCAT */ + EC_MBOX_EOE = 0x02, /**< Ethernet over EtherCAT */ + EC_MBOX_COE = 0x04, /**< CANopen over EtherCAT */ + EC_MBOX_FOE = 0x08, /**< File-Access over EtherCAT */ + EC_MBOX_SOE = 0x10, /**< Servo-Profile over EtherCAT */ + EC_MBOX_VOE = 0x20 /**< Vendor specific */ +}; + +/** Slave information interface CANopen over EtherCAT details flags. + */ +typedef struct { + uint8_t enable_sdo : 1; /**< Enable SDO access. */ + uint8_t enable_sdo_info : 1; /**< SDO information service available. */ + uint8_t enable_pdo_assign : 1; /**< PDO mapping configurable. */ + uint8_t enable_pdo_configuration : 1; /**< PDO configuration possible. */ + uint8_t enable_upload_at_startup : 1; /**< ?. */ + uint8_t enable_sdo_complete_access : 1; /**< Complete access possible. */ +} ec_sii_coe_details_t; + +/** Slave information interface general flags. + */ +typedef struct { + uint8_t enable_safeop : 1; /**< ?. */ + uint8_t enable_not_lrw : 1; /**< Slave does not support LRW. */ +} ec_sii_general_flags_t; + +/** EtherCAT slave distributed clocks range. + */ +typedef enum { + EC_DC_32, /**< 32 bit. */ + EC_DC_64 /*< 64 bit for system time, system time offset and + port 0 receive time. */ +} ec_slave_dc_range_t; + +/** EtherCAT slave sync signal configuration. + */ +typedef struct { + uint32_t cycle_time; /**< Cycle time [ns]. */ + int32_t shift_time; /**< Shift time [ns]. */ +} ec_sync_signal_t; + +/** Access states for SDO entries. + * + * The access rights are managed per AL state. + */ +enum { + EC_SDO_ENTRY_ACCESS_PREOP, /**< Access rights in PREOP. */ + EC_SDO_ENTRY_ACCESS_SAFEOP, /**< Access rights in SAFEOP. */ + EC_SDO_ENTRY_ACCESS_OP, /**< Access rights in OP. */ + EC_SDO_ENTRY_ACCESS_COUNT /**< Number of states. */ +}; + +/** Master devices. + */ +typedef enum { + EC_DEVICE_MAIN, /**< Main device. */ + EC_DEVICE_BACKUP /**< Backup device */ +} ec_device_index_t; + +extern const char *ec_device_names[2]; // only main and backup! + +/*****************************************************************************/ + +/** Convenience macro for printing EtherCAT-specific information to syslog. + * + * This will print the message in \a fmt with a prefixed "EtherCAT: ". + * + * \param fmt format string (like in printf()) + * \param args arguments (optional) + */ +#define EC_INFO(fmt, args...) \ + printk(KERN_INFO "EtherCAT: " fmt, ##args) + +/** Convenience macro for printing EtherCAT-specific errors to syslog. + * + * This will print the message in \a fmt with a prefixed "EtherCAT ERROR: ". + * + * \param fmt format string (like in printf()) + * \param args arguments (optional) + */ +#define EC_ERR(fmt, args...) \ + printk(KERN_ERR "EtherCAT ERROR: " fmt, ##args) + +/** Convenience macro for printing EtherCAT-specific warnings to syslog. + * + * This will print the message in \a fmt with a prefixed "EtherCAT WARNING: ". + * + * \param fmt format string (like in printf()) + * \param args arguments (optional) + */ +#define EC_WARN(fmt, args...) \ + printk(KERN_WARNING "EtherCAT WARNING: " fmt, ##args) + +/** Convenience macro for printing EtherCAT debug messages to syslog. + * + * This will print the message in \a fmt with a prefixed "EtherCAT DEBUG: ". + * + * \param fmt format string (like in printf()) + * \param args arguments (optional) + */ +#define EC_DBG(fmt, args...) \ + printk(KERN_DEBUG "EtherCAT DEBUG: " fmt, ##args) + +/*****************************************************************************/ + +/** Absolute value. + */ +#define EC_ABS(X) ((X) >= 0 ? (X) : -(X)) + +/*****************************************************************************/ + +extern char *ec_master_version_str; + +/*****************************************************************************/ + +unsigned int ec_master_count(void); +void ec_print_data(const uint8_t *, size_t); +void ec_print_data_diff(const uint8_t *, const uint8_t *, size_t); +size_t ec_state_string(uint8_t, char *, uint8_t); +ssize_t ec_mac_print(const uint8_t *, char *); +int ec_mac_is_zero(const uint8_t *); + +ec_master_t *ecrt_request_master_err(unsigned int); + +/*****************************************************************************/ + +/** Code/Message pair. + * + * Some EtherCAT datagrams support reading a status code to display a certain + * message. This type allows to map a code to a message string. + */ +typedef struct { + uint32_t code; /**< Code. */ + const char *message; /**< Message belonging to \a code. */ +} ec_code_msg_t; + +/*****************************************************************************/ + +/** Generic request state. + * + * \attention If ever changing this, please be sure to adjust the \a + * state_table in master/sdo_request.c. + */ +typedef enum { + EC_INT_REQUEST_INIT, + EC_INT_REQUEST_QUEUED, + EC_INT_REQUEST_BUSY, + EC_INT_REQUEST_SUCCESS, + EC_INT_REQUEST_FAILURE +} ec_internal_request_state_t; + +/*****************************************************************************/ + +extern const ec_request_state_t ec_request_state_translation_table[]; + +/*****************************************************************************/ + +/** Origin type. + */ +typedef enum { + EC_ORIG_INTERNAL, /**< Internal. */ + EC_ORIG_EXTERNAL /**< External. */ +} ec_origin_t; + +/*****************************************************************************/ + +typedef struct ec_slave ec_slave_t; /**< \see ec_slave. */ + +/*****************************************************************************/ + +#endif diff --git a/HiveCoreR0/src/ethercat_driver_ros2/ethercat_interface/include/ethercat_interface/ioctl.h b/HiveCoreR0/src/ethercat_driver_ros2/ethercat_interface/include/ethercat_interface/ioctl.h new file mode 100644 index 0000000..20c4de6 --- /dev/null +++ b/HiveCoreR0/src/ethercat_driver_ros2/ethercat_interface/include/ethercat_interface/ioctl.h @@ -0,0 +1,823 @@ +/****************************************************************************** + * + * Copyright (C) 2006-2021 Florian Pose, Ingenieurgemeinschaft IgH + * + * This file is part of the IgH EtherCAT master. + * + * The file is free software; you can redistribute it and/or modify it under + * the terms of the GNU Lesser General Public License as published by the + * Free Software Foundation; version 2.1 of the License. + * + * This file is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public + * License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with this file. If not, see . + * + *****************************************************************************/ + +/** + \file + EtherCAT master character device IOCTL commands. +*/ + +/*****************************************************************************/ + +#ifndef __EC_IOCTL_H__ +#define __EC_IOCTL_H__ + +#include + +#include "globals.h" + +/*****************************************************************************/ + +/** \cond */ + +#define EC_IOCTL_TYPE 0xa4 + +#define EC_IO(nr) _IO(EC_IOCTL_TYPE, nr) +#define EC_IOR(nr, type) _IOR(EC_IOCTL_TYPE, nr, type) +#define EC_IOW(nr, type) _IOW(EC_IOCTL_TYPE, nr, type) +#define EC_IOWR(nr, type) _IOWR(EC_IOCTL_TYPE, nr, type) + +/** EtherCAT master ioctl() version magic. + * + * Increment this when changing the ioctl interface! + */ +#define EC_IOCTL_VERSION_MAGIC 33 + +// Command-line tool +#define EC_IOCTL_MODULE EC_IOR(0x00, ec_ioctl_module_t) +#define EC_IOCTL_MASTER EC_IOR(0x01, ec_ioctl_master_t) +#define EC_IOCTL_SLAVE EC_IOWR(0x02, ec_ioctl_slave_t) +#define EC_IOCTL_SLAVE_SYNC EC_IOWR(0x03, ec_ioctl_slave_sync_t) +#define EC_IOCTL_SLAVE_SYNC_PDO EC_IOWR(0x04, ec_ioctl_slave_sync_pdo_t) +#define EC_IOCTL_SLAVE_SYNC_PDO_ENTRY EC_IOWR(0x05, ec_ioctl_slave_sync_pdo_entry_t) +#define EC_IOCTL_DOMAIN EC_IOWR(0x06, ec_ioctl_domain_t) +#define EC_IOCTL_DOMAIN_FMMU EC_IOWR(0x07, ec_ioctl_domain_fmmu_t) +#define EC_IOCTL_DOMAIN_DATA EC_IOWR(0x08, ec_ioctl_domain_data_t) +#define EC_IOCTL_MASTER_DEBUG EC_IO(0x09) +#define EC_IOCTL_MASTER_RESCAN EC_IO(0x0a) +#define EC_IOCTL_SLAVE_STATE EC_IOW(0x0b, ec_ioctl_slave_state_t) +#define EC_IOCTL_SLAVE_SDO EC_IOWR(0x0c, ec_ioctl_slave_sdo_t) +#define EC_IOCTL_SLAVE_SDO_ENTRY EC_IOWR(0x0d, ec_ioctl_slave_sdo_entry_t) +#define EC_IOCTL_SLAVE_SDO_UPLOAD EC_IOWR(0x0e, ec_ioctl_slave_sdo_upload_t) +#define EC_IOCTL_SLAVE_SDO_DOWNLOAD EC_IOWR(0x0f, ec_ioctl_slave_sdo_download_t) +#define EC_IOCTL_SLAVE_SII_READ EC_IOWR(0x10, ec_ioctl_slave_sii_t) +#define EC_IOCTL_SLAVE_SII_WRITE EC_IOW(0x11, ec_ioctl_slave_sii_t) +#define EC_IOCTL_SLAVE_REG_READ EC_IOWR(0x12, ec_ioctl_slave_reg_t) +#define EC_IOCTL_SLAVE_REG_WRITE EC_IOW(0x13, ec_ioctl_slave_reg_t) +#define EC_IOCTL_SLAVE_FOE_READ EC_IOWR(0x14, ec_ioctl_slave_foe_t) +#define EC_IOCTL_SLAVE_FOE_WRITE EC_IOW(0x15, ec_ioctl_slave_foe_t) +#define EC_IOCTL_SLAVE_SOE_READ EC_IOWR(0x16, ec_ioctl_slave_soe_read_t) +#define EC_IOCTL_SLAVE_SOE_WRITE EC_IOWR(0x17, ec_ioctl_slave_soe_write_t) +#define EC_IOCTL_CONFIG EC_IOWR(0x18, ec_ioctl_config_t) +#define EC_IOCTL_CONFIG_PDO EC_IOWR(0x19, ec_ioctl_config_pdo_t) +#define EC_IOCTL_CONFIG_PDO_ENTRY EC_IOWR(0x1a, ec_ioctl_config_pdo_entry_t) +#define EC_IOCTL_CONFIG_SDO EC_IOWR(0x1b, ec_ioctl_config_sdo_t) +#define EC_IOCTL_CONFIG_IDN EC_IOWR(0x1c, ec_ioctl_config_idn_t) +#define EC_IOCTL_CONFIG_FLAG EC_IOWR(0x1d, ec_ioctl_config_flag_t) +#ifdef EC_EOE +#define EC_IOCTL_EOE_HANDLER EC_IOWR(0x1e, ec_ioctl_eoe_handler_t) +#endif + +// Application interface +#define EC_IOCTL_REQUEST EC_IO(0x1f) +#define EC_IOCTL_CREATE_DOMAIN EC_IO(0x20) +#define EC_IOCTL_CREATE_SLAVE_CONFIG EC_IOWR(0x21, ec_ioctl_config_t) +#define EC_IOCTL_SELECT_REF_CLOCK EC_IOW(0x22, uint32_t) +#define EC_IOCTL_ACTIVATE EC_IOR(0x23, ec_ioctl_master_activate_t) +#define EC_IOCTL_DEACTIVATE EC_IO(0x24) +#define EC_IOCTL_SEND EC_IO(0x25) +#define EC_IOCTL_RECEIVE EC_IO(0x26) +#define EC_IOCTL_MASTER_STATE EC_IOR(0x27, ec_master_state_t) +#define EC_IOCTL_MASTER_LINK_STATE EC_IOWR(0x28, ec_ioctl_link_state_t) +#define EC_IOCTL_APP_TIME EC_IOW(0x29, uint64_t) +#define EC_IOCTL_SYNC_REF EC_IO(0x2a) +#define EC_IOCTL_SYNC_REF_TO EC_IOW(0x2b, uint64_t) +#define EC_IOCTL_SYNC_SLAVES EC_IO(0x2c) +#define EC_IOCTL_REF_CLOCK_TIME EC_IOR(0x2d, uint32_t) +#define EC_IOCTL_SYNC_MON_QUEUE EC_IO(0x2e) +#define EC_IOCTL_SYNC_MON_PROCESS EC_IOR(0x2f, uint32_t) +#define EC_IOCTL_RESET EC_IO(0x30) +#define EC_IOCTL_SC_SYNC EC_IOW(0x31, ec_ioctl_config_t) +#define EC_IOCTL_SC_WATCHDOG EC_IOW(0x32, ec_ioctl_config_t) +#define EC_IOCTL_SC_ADD_PDO EC_IOW(0x33, ec_ioctl_config_pdo_t) +#define EC_IOCTL_SC_CLEAR_PDOS EC_IOW(0x34, ec_ioctl_config_pdo_t) +#define EC_IOCTL_SC_ADD_ENTRY EC_IOW(0x35, ec_ioctl_add_pdo_entry_t) +#define EC_IOCTL_SC_CLEAR_ENTRIES EC_IOW(0x36, ec_ioctl_config_pdo_t) +#define EC_IOCTL_SC_REG_PDO_ENTRY EC_IOWR(0x37, ec_ioctl_reg_pdo_entry_t) +#define EC_IOCTL_SC_REG_PDO_POS EC_IOWR(0x38, ec_ioctl_reg_pdo_pos_t) +#define EC_IOCTL_SC_DC EC_IOW(0x39, ec_ioctl_config_t) +#define EC_IOCTL_SC_SDO EC_IOW(0x3a, ec_ioctl_sc_sdo_t) +#define EC_IOCTL_SC_EMERG_SIZE EC_IOW(0x3b, ec_ioctl_sc_emerg_t) +#define EC_IOCTL_SC_EMERG_POP EC_IOWR(0x3c, ec_ioctl_sc_emerg_t) +#define EC_IOCTL_SC_EMERG_CLEAR EC_IOW(0x3d, ec_ioctl_sc_emerg_t) +#define EC_IOCTL_SC_EMERG_OVERRUNS EC_IOWR(0x3e, ec_ioctl_sc_emerg_t) +#define EC_IOCTL_SC_SDO_REQUEST EC_IOWR(0x3f, ec_ioctl_sdo_request_t) +#define EC_IOCTL_SC_SOE_REQUEST EC_IOWR(0x40, ec_ioctl_soe_request_t) +#define EC_IOCTL_SC_REG_REQUEST EC_IOWR(0x41, ec_ioctl_reg_request_t) +#define EC_IOCTL_SC_VOE EC_IOWR(0x42, ec_ioctl_voe_t) +#define EC_IOCTL_SC_STATE EC_IOWR(0x43, ec_ioctl_sc_state_t) +#define EC_IOCTL_SC_IDN EC_IOW(0x44, ec_ioctl_sc_idn_t) +#define EC_IOCTL_SC_FLAG EC_IOW(0x45, ec_ioctl_sc_flag_t) +#define EC_IOCTL_DOMAIN_SIZE EC_IO(0x46) +#define EC_IOCTL_DOMAIN_OFFSET EC_IO(0x47) +#define EC_IOCTL_DOMAIN_PROCESS EC_IO(0x48) +#define EC_IOCTL_DOMAIN_QUEUE EC_IO(0x49) +#define EC_IOCTL_DOMAIN_STATE EC_IOWR(0x4a, ec_ioctl_domain_state_t) +#define EC_IOCTL_SDO_REQUEST_INDEX EC_IOWR(0x4b, ec_ioctl_sdo_request_t) +#define EC_IOCTL_SDO_REQUEST_TIMEOUT EC_IOWR(0x4c, ec_ioctl_sdo_request_t) +#define EC_IOCTL_SDO_REQUEST_STATE EC_IOWR(0x4d, ec_ioctl_sdo_request_t) +#define EC_IOCTL_SDO_REQUEST_READ EC_IOWR(0x4e, ec_ioctl_sdo_request_t) +#define EC_IOCTL_SDO_REQUEST_WRITE EC_IOWR(0x4f, ec_ioctl_sdo_request_t) +#define EC_IOCTL_SDO_REQUEST_DATA EC_IOWR(0x50, ec_ioctl_sdo_request_t) +#define EC_IOCTL_SOE_REQUEST_IDN EC_IOWR(0x51, ec_ioctl_soe_request_t) +#define EC_IOCTL_SOE_REQUEST_TIMEOUT EC_IOWR(0x52, ec_ioctl_soe_request_t) +#define EC_IOCTL_SOE_REQUEST_STATE EC_IOWR(0x53, ec_ioctl_soe_request_t) +#define EC_IOCTL_SOE_REQUEST_READ EC_IOWR(0x54, ec_ioctl_soe_request_t) +#define EC_IOCTL_SOE_REQUEST_WRITE EC_IOWR(0x55, ec_ioctl_soe_request_t) +#define EC_IOCTL_SOE_REQUEST_DATA EC_IOWR(0x56, ec_ioctl_soe_request_t) +#define EC_IOCTL_REG_REQUEST_DATA EC_IOWR(0x57, ec_ioctl_reg_request_t) +#define EC_IOCTL_REG_REQUEST_STATE EC_IOWR(0x58, ec_ioctl_reg_request_t) +#define EC_IOCTL_REG_REQUEST_WRITE EC_IOWR(0x59, ec_ioctl_reg_request_t) +#define EC_IOCTL_REG_REQUEST_READ EC_IOWR(0x5a, ec_ioctl_reg_request_t) +#define EC_IOCTL_VOE_SEND_HEADER EC_IOW(0x5b, ec_ioctl_voe_t) +#define EC_IOCTL_VOE_REC_HEADER EC_IOWR(0x5c, ec_ioctl_voe_t) +#define EC_IOCTL_VOE_READ EC_IOW(0x5d, ec_ioctl_voe_t) +#define EC_IOCTL_VOE_READ_NOSYNC EC_IOW(0x5e, ec_ioctl_voe_t) +#define EC_IOCTL_VOE_WRITE EC_IOWR(0x5f, ec_ioctl_voe_t) +#define EC_IOCTL_VOE_EXEC EC_IOWR(0x60, ec_ioctl_voe_t) +#define EC_IOCTL_VOE_DATA EC_IOWR(0x61, ec_ioctl_voe_t) +#define EC_IOCTL_SET_SEND_INTERVAL EC_IOW(0x62, size_t) + +/*****************************************************************************/ + +#define EC_IOCTL_STRING_SIZE 64 + +/*****************************************************************************/ + +typedef struct { + uint32_t ioctl_version_magic; + uint32_t master_count; +} ec_ioctl_module_t; + +/*****************************************************************************/ + +typedef struct { + uint32_t slave_count; + uint32_t config_count; + uint32_t domain_count; + uint32_t eoe_handler_count; + uint8_t phase; + uint8_t active; + uint8_t scan_busy; + struct ec_ioctl_device { + uint8_t address[6]; + uint8_t attached; + uint8_t link_state; + uint64_t tx_count; + uint64_t rx_count; + uint64_t tx_bytes; + uint64_t rx_bytes; + uint64_t tx_errors; + int32_t tx_frame_rates[EC_RATE_COUNT]; + int32_t rx_frame_rates[EC_RATE_COUNT]; + int32_t tx_byte_rates[EC_RATE_COUNT]; + int32_t rx_byte_rates[EC_RATE_COUNT]; + } devices[EC_MAX_NUM_DEVICES]; + uint32_t num_devices; + uint64_t tx_count; + uint64_t rx_count; + uint64_t tx_bytes; + uint64_t rx_bytes; + int32_t tx_frame_rates[EC_RATE_COUNT]; + int32_t rx_frame_rates[EC_RATE_COUNT]; + int32_t tx_byte_rates[EC_RATE_COUNT]; + int32_t rx_byte_rates[EC_RATE_COUNT]; + int32_t loss_rates[EC_RATE_COUNT]; + uint64_t app_time; + uint64_t dc_ref_time; + uint16_t ref_clock; +} ec_ioctl_master_t; + +/*****************************************************************************/ + +typedef struct { + // input + uint16_t position; + + // outputs + unsigned int device_index; + uint32_t vendor_id; + uint32_t product_code; + uint32_t revision_number; + uint32_t serial_number; + uint16_t alias; + uint16_t boot_rx_mailbox_offset; + uint16_t boot_rx_mailbox_size; + uint16_t boot_tx_mailbox_offset; + uint16_t boot_tx_mailbox_size; + uint16_t std_rx_mailbox_offset; + uint16_t std_rx_mailbox_size; + uint16_t std_tx_mailbox_offset; + uint16_t std_tx_mailbox_size; + uint16_t mailbox_protocols; + uint8_t has_general_category; + ec_sii_coe_details_t coe_details; + ec_sii_general_flags_t general_flags; + int16_t current_on_ebus; + struct { + ec_slave_port_desc_t desc; + ec_slave_port_link_t link; + uint32_t receive_time; + uint16_t next_slave; + uint32_t delay_to_next_dc; + } ports[EC_MAX_PORTS]; + uint8_t fmmu_bit; + uint8_t dc_supported; + ec_slave_dc_range_t dc_range; + uint8_t has_dc_system_time; + uint32_t transmission_delay; + uint8_t al_state; + uint8_t error_flag; + uint8_t sync_count; + uint16_t sdo_count; + uint32_t sii_nwords; + char group[EC_IOCTL_STRING_SIZE]; + char image[EC_IOCTL_STRING_SIZE]; + char order[EC_IOCTL_STRING_SIZE]; + char name[EC_IOCTL_STRING_SIZE]; +} ec_ioctl_slave_t; + +/*****************************************************************************/ + +typedef struct { + // inputs + uint16_t slave_position; + uint32_t sync_index; + + // outputs + uint16_t physical_start_address; + uint16_t default_size; + uint8_t control_register; + uint8_t enable; + uint8_t pdo_count; +} ec_ioctl_slave_sync_t; + +/*****************************************************************************/ + +typedef struct { + // inputs + uint16_t slave_position; + uint32_t sync_index; + uint32_t pdo_pos; + + // outputs + uint16_t index; + uint8_t entry_count; + int8_t name[EC_IOCTL_STRING_SIZE]; +} ec_ioctl_slave_sync_pdo_t; + +/*****************************************************************************/ + +typedef struct { + // inputs + uint16_t slave_position; + uint32_t sync_index; + uint32_t pdo_pos; + uint32_t entry_pos; + + // outputs + uint16_t index; + uint8_t subindex; + uint8_t bit_length; + int8_t name[EC_IOCTL_STRING_SIZE]; +} ec_ioctl_slave_sync_pdo_entry_t; + +/*****************************************************************************/ + +typedef struct { + // inputs + uint32_t index; + + // outputs + uint32_t data_size; + uint32_t logical_base_address; + uint16_t working_counter[EC_MAX_NUM_DEVICES]; + uint16_t expected_working_counter; + uint32_t fmmu_count; +} ec_ioctl_domain_t; + +/*****************************************************************************/ + +typedef struct { + // inputs + uint32_t domain_index; + uint32_t fmmu_index; + + // outputs + uint16_t slave_config_alias; + uint16_t slave_config_position; + uint8_t sync_index; + ec_direction_t dir; + uint32_t logical_address; + uint32_t data_size; +} ec_ioctl_domain_fmmu_t; + +/*****************************************************************************/ + +typedef struct { + // inputs + uint32_t domain_index; + uint32_t data_size; + uint8_t *target; +} ec_ioctl_domain_data_t; + +/*****************************************************************************/ + +typedef struct { + // inputs + uint16_t slave_position; + uint8_t al_state; +} ec_ioctl_slave_state_t; + +/*****************************************************************************/ + +typedef struct { + // inputs + uint16_t slave_position; + uint16_t sdo_position; + + // outputs + uint16_t sdo_index; + uint8_t max_subindex; + int8_t name[EC_IOCTL_STRING_SIZE]; +} ec_ioctl_slave_sdo_t; + +/*****************************************************************************/ + +typedef struct { + // inputs + uint16_t slave_position; + int sdo_spec; // positive: index, negative: list position + uint8_t sdo_entry_subindex; + + // outputs + uint16_t data_type; + uint16_t bit_length; + uint8_t read_access[EC_SDO_ENTRY_ACCESS_COUNT]; + uint8_t write_access[EC_SDO_ENTRY_ACCESS_COUNT]; + int8_t description[EC_IOCTL_STRING_SIZE]; +} ec_ioctl_slave_sdo_entry_t; + +/*****************************************************************************/ + +typedef struct { + // inputs + uint16_t slave_position; + uint16_t sdo_index; + uint8_t sdo_entry_subindex; + size_t target_size; + uint8_t *target; + + // outputs + size_t data_size; + uint32_t abort_code; +} ec_ioctl_slave_sdo_upload_t; + +/*****************************************************************************/ + +typedef struct { + // inputs + uint16_t slave_position; + uint16_t sdo_index; + uint8_t sdo_entry_subindex; + uint8_t complete_access; + size_t data_size; + uint8_t *data; + + // outputs + uint32_t abort_code; +} ec_ioctl_slave_sdo_download_t; + +/*****************************************************************************/ + +typedef struct { + // inputs + uint16_t slave_position; + uint16_t offset; + uint32_t nwords; + uint16_t *words; +} ec_ioctl_slave_sii_t; + +/*****************************************************************************/ + +typedef struct { + // inputs + uint16_t slave_position; + uint8_t emergency; + uint16_t address; + size_t size; + uint8_t *data; +} ec_ioctl_slave_reg_t; + +/*****************************************************************************/ + +typedef struct { + // inputs + uint16_t slave_position; + uint16_t offset; + size_t buffer_size; + uint8_t *buffer; + + // outputs + size_t data_size; + uint32_t result; + uint32_t error_code; + char file_name[32]; +} ec_ioctl_slave_foe_t; + +/*****************************************************************************/ + +typedef struct { + // inputs + uint16_t slave_position; + uint8_t drive_no; + uint16_t idn; + size_t mem_size; + uint8_t *data; + + // outputs + size_t data_size; + uint16_t error_code; +} ec_ioctl_slave_soe_read_t; + +/*****************************************************************************/ + +typedef struct { + // inputs + uint16_t slave_position; + uint8_t drive_no; + uint16_t idn; + size_t data_size; + uint8_t *data; + + // outputs + uint16_t error_code; +} ec_ioctl_slave_soe_write_t; + +/*****************************************************************************/ + +typedef struct { + // inputs + uint32_t config_index; + + // outputs + uint16_t alias; + uint16_t position; + uint32_t vendor_id; + uint32_t product_code; + struct { + ec_direction_t dir; + ec_watchdog_mode_t watchdog_mode; + uint32_t pdo_count; + uint8_t config_this; + } syncs[EC_MAX_SYNC_MANAGERS]; + uint16_t watchdog_divider; + uint16_t watchdog_intervals; + uint32_t sdo_count; + uint32_t idn_count; + uint32_t flag_count; + int32_t slave_position; + uint16_t dc_assign_activate; + ec_sync_signal_t dc_sync[EC_SYNC_SIGNAL_COUNT]; +} ec_ioctl_config_t; + +/*****************************************************************************/ + +typedef struct { + // inputs + uint32_t config_index; + uint8_t sync_index; + uint16_t pdo_pos; + + // outputs + uint16_t index; + uint8_t entry_count; + int8_t name[EC_IOCTL_STRING_SIZE]; +} ec_ioctl_config_pdo_t; + +/*****************************************************************************/ + +typedef struct { + // inputs + uint32_t config_index; + uint8_t sync_index; + uint16_t pdo_pos; + uint8_t entry_pos; + + // outputs + uint16_t index; + uint8_t subindex; + uint8_t bit_length; + int8_t name[EC_IOCTL_STRING_SIZE]; +} ec_ioctl_config_pdo_entry_t; + +/*****************************************************************************/ + +/** Maximum size for displayed SDO data. + * \todo Make this dynamic. + */ +#define EC_MAX_SDO_DATA_SIZE 1024 + +typedef struct { + // inputs + uint32_t config_index; + uint32_t sdo_pos; + + // outputs + uint16_t index; + uint8_t subindex; + size_t size; + uint8_t data[EC_MAX_SDO_DATA_SIZE]; + uint8_t complete_access; +} ec_ioctl_config_sdo_t; + +/*****************************************************************************/ + +/** Maximum size for displayed IDN data. + * \todo Make this dynamic. + */ +#define EC_MAX_IDN_DATA_SIZE 1024 + +typedef struct { + // inputs + uint32_t config_index; + uint32_t idn_pos; + + // outputs + uint8_t drive_no; + uint16_t idn; + ec_al_state_t state; + size_t size; + uint8_t data[EC_MAX_IDN_DATA_SIZE]; +} ec_ioctl_config_idn_t; + +/*****************************************************************************/ + +/** Maximum size for key. + */ +#define EC_MAX_FLAG_KEY_SIZE 128 + +typedef struct { + // inputs + uint32_t config_index; + uint32_t flag_pos; + + // outputs + char key[EC_MAX_FLAG_KEY_SIZE]; + int32_t value; +} ec_ioctl_config_flag_t; + +/*****************************************************************************/ + +#ifdef EC_EOE + +typedef struct { + // input + uint16_t eoe_index; + + // outputs + char name[EC_DATAGRAM_NAME_SIZE]; + uint16_t slave_position; + uint8_t open; + uint32_t rx_bytes; + uint32_t rx_rate; + uint32_t tx_bytes; + uint32_t tx_rate; + uint32_t tx_queued_frames; + uint32_t tx_queue_size; +} ec_ioctl_eoe_handler_t; + +#endif + +/*****************************************************************************/ + +typedef struct { + // outputs + void *process_data; + size_t process_data_size; +} ec_ioctl_master_activate_t; + +/*****************************************************************************/ + +typedef struct { + // inputs + uint32_t config_index; + uint16_t pdo_index; + uint16_t entry_index; + uint8_t entry_subindex; + uint8_t entry_bit_length; +} ec_ioctl_add_pdo_entry_t; + +/*****************************************************************************/ + +typedef struct { + // inputs + uint32_t config_index; + uint16_t entry_index; + uint8_t entry_subindex; + uint32_t domain_index; + + // outputs + unsigned int bit_position; +} ec_ioctl_reg_pdo_entry_t; + +/*****************************************************************************/ + +typedef struct { + // inputs + uint32_t config_index; + uint32_t sync_index; + uint32_t pdo_pos; + uint32_t entry_pos; + uint32_t domain_index; + + // outputs + unsigned int bit_position; +} ec_ioctl_reg_pdo_pos_t; + +/*****************************************************************************/ + +typedef struct { + // inputs + uint32_t config_index; + uint16_t index; + uint8_t subindex; + const uint8_t *data; + size_t size; + uint8_t complete_access; +} ec_ioctl_sc_sdo_t; + +/*****************************************************************************/ + +typedef struct { + // inputs + uint32_t config_index; + size_t size; + uint8_t *target; + + // outputs + int32_t overruns; +} ec_ioctl_sc_emerg_t; + +/*****************************************************************************/ + +typedef struct { + // inputs + uint32_t config_index; + + // outputs + ec_slave_config_state_t *state; +} ec_ioctl_sc_state_t; + +/*****************************************************************************/ + +typedef struct { + // inputs + uint32_t config_index; + uint8_t drive_no; + uint16_t idn; + ec_al_state_t al_state; + const uint8_t *data; + size_t size; +} ec_ioctl_sc_idn_t; + +/*****************************************************************************/ + +typedef struct { + // inputs + uint32_t config_index; + size_t key_size; + char *key; + int32_t value; +} ec_ioctl_sc_flag_t; + +/*****************************************************************************/ + +typedef struct { + // inputs + uint32_t domain_index; + + // outputs + ec_domain_state_t *state; +} ec_ioctl_domain_state_t; + +/*****************************************************************************/ + +typedef struct { + // inputs + uint32_t config_index; + + // inputs/outputs + uint32_t request_index; + uint16_t sdo_index; + uint8_t sdo_subindex; + size_t size; + uint8_t *data; + uint32_t timeout; + ec_request_state_t state; +} ec_ioctl_sdo_request_t; + +/*****************************************************************************/ + +typedef struct { + // inputs + uint32_t config_index; + + // inputs/outputs + uint32_t request_index; + uint8_t drive_no; + uint16_t idn; + size_t size; + uint8_t *data; + uint32_t timeout; + ec_request_state_t state; +} ec_ioctl_soe_request_t; + +/*****************************************************************************/ + +typedef struct { + // inputs + uint32_t config_index; + size_t mem_size; + + // inputs/outputs + uint32_t request_index; + uint8_t *data; + ec_request_state_t state; + uint8_t new_data; + uint16_t address; + size_t transfer_size; +} ec_ioctl_reg_request_t; + +/*****************************************************************************/ + +typedef struct { + // inputs + uint32_t config_index; + + // inputs/outputs + uint32_t voe_index; + uint32_t *vendor_id; + uint16_t *vendor_type; + size_t size; + uint8_t *data; + ec_request_state_t state; +} ec_ioctl_voe_t; + +/*****************************************************************************/ + +typedef struct { + // inputs + uint32_t dev_idx; + + // outputs + ec_master_link_state_t *state; +} ec_ioctl_link_state_t; + +/*****************************************************************************/ + +#ifdef __KERNEL__ + +/** Context data structure for file handles. + */ +typedef struct { + unsigned int writable; /**< Device was opened with write permission. */ + unsigned int requested; /**< Master was requested via this file handle. */ + uint8_t *process_data; /**< Total process data area. */ + size_t process_data_size; /**< Size of the \a process_data. */ +} ec_ioctl_context_t; + +long ec_ioctl(ec_master_t *, ec_ioctl_context_t *, unsigned int, + void __user *); + +#ifdef EC_RTDM + +long ec_ioctl_rtdm(ec_master_t *, ec_ioctl_context_t *, unsigned int, + void __user *); +int ec_rtdm_mmap(ec_ioctl_context_t *, void **); + +#endif + +#endif + +/*****************************************************************************/ + +/** \endcond */ + +#endif diff --git a/HiveCoreR0/src/ethercat_driver_ros2/ethercat_interface/include/globals.h b/HiveCoreR0/src/ethercat_driver_ros2/ethercat_interface/include/globals.h new file mode 100644 index 0000000..d2a470e --- /dev/null +++ b/HiveCoreR0/src/ethercat_driver_ros2/ethercat_interface/include/globals.h @@ -0,0 +1,55 @@ +/****************************************************************************** + * + * Copyright (C) 2006-2021 Florian Pose, Ingenieurgemeinschaft IgH + * + * This file is part of the IgH EtherCAT master. + * + * The file is free software; you can redistribute it and/or modify it under + * the terms of the GNU Lesser General Public License as published by the + * Free Software Foundation; version 2.1 of the License. + * + * This file is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public + * License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with this file. If not, see . + * + *****************************************************************************/ + +/** + \file + Global definitions and macros. +*/ + +/*****************************************************************************/ + +#ifndef __EC_GLOBALS_H__ +#define __EC_GLOBALS_H__ + +#include "config.h" + +/****************************************************************************** + * Overall macros + *****************************************************************************/ + +/** Helper macro for EC_STR(), literates a macro argument. + * + * \param X argument to literate. + */ +#define EC_LIT(X) #X + +/** Converts a macro argument to a string. + * + * \param X argument to stringify. + */ +#define EC_STR(X) EC_LIT(X) + +/** Master version string + */ +#define EC_MASTER_VERSION VERSION " " EC_STR(REV) + +/*****************************************************************************/ + +#endif diff --git a/HiveCoreR0/src/ethercat_driver_ros2/ethercat_interface/src/ec_master.cpp b/HiveCoreR0/src/ethercat_driver_ros2/ethercat_interface/src/ec_master.cpp index db31e68..16508df 100644 --- a/HiveCoreR0/src/ethercat_driver_ros2/ethercat_interface/src/ec_master.cpp +++ b/HiveCoreR0/src/ethercat_driver_ros2/ethercat_interface/src/ec_master.cpp @@ -14,7 +14,7 @@ #include "ethercat_interface/ec_master.hpp" #include "ethercat_interface/ec_slave.hpp" - +#include "ethercat_interface/ioctl.h" #include #include #include @@ -25,7 +25,8 @@ #include #include #include - +#include +#include #define EC_NEWTIMEVAL2NANO(TV) \ (((TV).tv_sec - 946684800ULL) * 1000000000ULL + (TV).tv_nsec) @@ -90,9 +91,7 @@ void EcMaster::addSlave(uint16_t alias, uint16_t position, EcSlave * slave) printWarning("Add slave. Failed to get slave configuration."); return; } - // check and setup dc - if (slave->assign_activate_dc_sync()) { struct timespec t; clock_gettime(CLOCK_MONOTONIC, &t); @@ -138,12 +137,12 @@ void EcMaster::addSlave(uint16_t alias, uint16_t position, EcSlave * slave) domain_info = new DomainInfo(master_); domain_info_[domain_index] = domain_info; } - registerPDOInDomain( alias, position, iter.second, domain_info, slave); } + } int EcMaster::configSlaveSdo( @@ -214,7 +213,39 @@ void EcMaster::registerPDOInDomain( ec_pdo_entry_reg_t empty = {0}; domain_info->domain_regs.back() = empty; } +int EcMaster::set_state(int pos,std::string sta){ + uint8_t state=0; + if (sta == "INIT") { + state = 0x01; + } else if (sta == "PREOP") { + state = 0x02; + } else if (sta == "BOOT") { + state = 0x03; + } else if (sta == "SAFEOP") { + state = 0x04; + } else if (sta == "OP") { + state = 0x08; + } else { + std::cout << "Invalid state "< - + @@ -16,7 +16,7 @@ - + @@ -47,14 +47,14 @@ - - + diff --git a/HiveCoreR0/src/robot_control/include/arm_control.hpp b/HiveCoreR0/src/robot_control/include/arm_control.hpp index 6304c5b..873c417 100644 --- a/HiveCoreR0/src/robot_control/include/arm_control.hpp +++ b/HiveCoreR0/src/robot_control/include/arm_control.hpp @@ -10,12 +10,22 @@ namespace robot_control class ArmControl { public: - ArmControl(size_t total_joints, double length1, double length2, double maxSpeed, double maxAcc, const std::vector& minLimits, const std::vector& maxLimits, const std::vector& home_positions); + ArmControl( + size_t total_joints, + double length1, + double length2, + double maxSpeed, + double maxAcc, + const std::vector& minLimits, + const std::vector& maxLimits, + const std::vector& home_positions, + const std::vector& joint_directions + ); ~ArmControl(); - void MoveToTargetPoint(const Eigen::Vector3d& target_pos, double duration); + bool MoveToTargetPoint(std::vector& joint_positions, const Eigen::Vector3d& target_pos, double duration); - void MoveToTargetJoint(const std::vector& target_joint, double duration); + bool MoveToTargetJoint(std::vector& joint_positions, const std::vector& target_joint, double duration); bool GoHome(std::vector& joint_positions, double dt); @@ -34,9 +44,14 @@ namespace robot_control double maxSpeed_; // 最大速度 double maxAcc_; // 最大加速度 + double stopDurationTime_; + double movingDurationTime_; + TrapezoidalTrajectory* trapezoidalTrajectory_; // 轨迹规划器 std::vector joint_home_positions_; + std::vector joint_directions_; + std::vector joint_position_; // 机械臂关节位置 std::vector joint_velocity_; // 机械臂关节速度 std::vector joint_acceleration_; // 机械臂关节加速度 diff --git a/HiveCoreR0/src/robot_control/include/common_enum.hpp b/HiveCoreR0/src/robot_control/include/common_enum.hpp index 65d1303..3cff53f 100644 --- a/HiveCoreR0/src/robot_control/include/common_enum.hpp +++ b/HiveCoreR0/src/robot_control/include/common_enum.hpp @@ -7,6 +7,8 @@ namespace robot_control * @brief 机器人状态枚举（全局通用） */ enum class RobotState { + INIT, + STOPPING, // 停止过程中 STOPPED, // 停止 MOVING_FORWARD, // 前进 MOVING_BACKWARD, // 后退 diff --git a/HiveCoreR0/src/robot_control/include/leg_control.hpp b/HiveCoreR0/src/robot_control/include/leg_control.hpp index 6946249..d87d5a2 100644 --- a/HiveCoreR0/src/robot_control/include/leg_control.hpp +++ b/HiveCoreR0/src/robot_control/include/leg_control.hpp @@ -15,7 +15,17 @@ namespace robot_control class LegControl { public: - LegControl(size_t totalJoints,double LegLength, double maxSpeed, double maxAcc, const std::vector& minLimits, const std::vector& maxLimits, const std::vector& home_positions); + LegControl( + size_t totalJoints, + double LegLength, + double maxSpeed, + double maxAcc, + const std::vector& minLimits, + const std::vector& maxLimits, + const std::vector& home_positions, + const std::vector& joint_directions + ); + ~LegControl(); bool MoveToTargetJoint(std::vector& joint_positions, const std::vector& target_joint, double dt); @@ -35,27 +45,26 @@ namespace robot_control private: size_t total_joints_; - double maxSpeed_; - double maxAcc_; - double LegLength_; - bool is_moving_; - bool is_stopping_; + double stopDurationTime_; + double movingDurationTime_; TrapezoidalTrajectory* trapezoidalTrajectory_; // 轨迹规划器 std::vector joint_home_positions_; + std::vector joint_directions_; + std::vector joint_position_; // 机械臂关节位置 std::vector joint_velocity_; // 机械臂关节速度 std::vector joint_acceleration_; // 机械臂关节加速度 std::vector joint_torque_; // 机械臂关节力矩 - std::vector maxLimits_; std::vector minLimits_; + std::vector maxLimits_; std::vector joint_position_desired_; // 机械臂关节期望位置 diff --git a/HiveCoreR0/src/robot_control/include/motion_parameters.hpp b/HiveCoreR0/src/robot_control/include/motion_parameters.hpp index c4dfc30..fd57ae3 100644 --- a/HiveCoreR0/src/robot_control/include/motion_parameters.hpp +++ b/HiveCoreR0/src/robot_control/include/motion_parameters.hpp @@ -56,12 +56,12 @@ namespace robot_control // 初始化关节索引 wheel_joint_indices_ = {0, 1, 2, 3}; pitch_body_indices_ = {0}; - left_leg_joint_indices_ = {1}; - right_leg_joint_indices_ = {2}; - left_arm_joint_indices_ = {3, 4, 5, 6}; - right_arm_joint_indices_ = {7, 8, 9, 10}; + right_leg_joint_indices_ = {1}; + left_leg_joint_indices_ = {2}; + right_arm_joint_indices_ = {3, 4, 5, 6}; + left_arm_joint_indices_ = {7, 8, 9, 10}; - total_joints_ = 3 ; //left_arm_joint_indices_.size() + right_arm_joint_indices_.size() + pitch_body_indices_.size() + left_leg_joint_indices_.size() + right_leg_joint_indices_.size(); + total_joints_ = 11 ; //left_arm_joint_indices_.size() + right_arm_joint_indices_.size() + pitch_body_indices_.size() + left_leg_joint_indices_.size() + right_leg_joint_indices_.size(); // 初始化关节方向 wheel_joint_directions_ = {1, -1, 1, -1}; @@ -69,7 +69,7 @@ namespace robot_control right_arm_joint_directions_ = {1, 1, 1, 1}; pitch_body_directions_ = {1}; left_leg_joint_directions_ = {1}; - right_leg_joint_directions_ = {1}; + right_leg_joint_directions_ = {-1}; leg_wheel_directions_ = {1, -1, -1, 1}; // 腿部 和 腰部的耦合方向 // 初始化关节状态 @@ -95,6 +95,15 @@ namespace robot_control JointLimit(10, 360.0, -360.0, LimitType::POSITION) }; + left_leg_max_limit_.resize(left_leg_joint_indices_.size()); + left_leg_min_limit_.resize(left_leg_joint_indices_.size()); + right_leg_max_limit_.resize(right_leg_joint_indices_.size()); + right_leg_min_limit_.resize(right_leg_joint_indices_.size()); + left_arm_max_limit_.resize(left_arm_joint_indices_.size()); + left_arm_min_limit_.resize(left_arm_joint_indices_.size()); + right_arm_max_limit_.resize(right_arm_joint_indices_.size()); + right_arm_min_limit_.resize(right_arm_joint_indices_.size()); + for (size_t i = 0; i < left_leg_joint_indices_.size(); i++) { left_leg_max_limit_[i] = joint_limits_[left_leg_joint_indices_[i]].max; @@ -145,6 +154,7 @@ namespace robot_control serial_port_ = "/dev/ttyUSB0"; baud_rate_ = 115200; + }; // 运动参数 diff --git a/HiveCoreR0/src/robot_control/include/robot_control_manager.hpp b/HiveCoreR0/src/robot_control/include/robot_control_manager.hpp index 9f35bba..539d185 100644 --- a/HiveCoreR0/src/robot_control/include/robot_control_manager.hpp +++ b/HiveCoreR0/src/robot_control/include/robot_control_manager.hpp @@ -26,7 +26,7 @@ namespace robot_control void TurnRight(); void MoveUp(double dt); void MoveDown(double dt); - void Stop(double dt); + bool Stop(double dt); bool GoHome(double dt); // void ArmGrab(double dt); @@ -41,6 +41,7 @@ namespace robot_control void init(); MotionParameters motionParams_; + bool isMoving_; // 控制器 ArmControl* leftArmController_; @@ -55,7 +56,7 @@ namespace robot_control std::vector jointAccelerations_; // 关节加速度（弧度/秒^2） std::vector jointEfforts_; // 关节力矩（牛顿米） - bool robotInited_; // 机器人是否已经初始化 + std::vector jointInited_; // 机器人是否已经初始化 std::vector robotHomed_; // 关节是否已经复位 std::vector tempLeftArmCmd; diff --git a/HiveCoreR0/src/robot_control/include/robot_fsm.hpp b/HiveCoreR0/src/robot_control/include/robot_fsm.hpp index 15ad461..32ae4a6 100644 --- a/HiveCoreR0/src/robot_control/include/robot_fsm.hpp +++ b/HiveCoreR0/src/robot_control/include/robot_fsm.hpp @@ -62,6 +62,9 @@ namespace robot_control bool isPaused_; bool isFinished_; bool isRobotEnabled_; + bool enableCommandExecuted_; + + int jogDirection_; // 状态转换表 std::map, RobotState> transitionTable_; diff --git a/HiveCoreR0/src/robot_control/include/trapezoidal_trajectory.hpp b/HiveCoreR0/src/robot_control/include/trapezoidal_trajectory.hpp index 9389279..4bcad6a 100644 --- a/HiveCoreR0/src/robot_control/include/trapezoidal_trajectory.hpp +++ b/HiveCoreR0/src/robot_control/include/trapezoidal_trajectory.hpp @@ -106,6 +106,8 @@ private: void calculateTrajectoryParams(); std::vector start_pos_; // 起始位置 + std::vector stop_pos_; + std::vector stop_vel_; std::vector target_pos_; // 目标位置 std::vector current_pos_; // 当前位置 std::vector current_velocity_; // 当前速度 diff --git a/HiveCoreR0/src/robot_control/src/arm_control.cpp b/HiveCoreR0/src/robot_control/src/arm_control.cpp index b12603f..dc697df 100644 --- a/HiveCoreR0/src/robot_control/src/arm_control.cpp +++ b/HiveCoreR0/src/robot_control/src/arm_control.cpp @@ -5,16 +5,28 @@ namespace robot_control { -ArmControl::ArmControl(size_t total_joints, double length1, double length2, double maxSpeed, double maxAcc, const std::vector& minLimits, const std::vector& maxLimits, const std::vector& home_positions) - : total_joints_(total_joints), - length1_(length1), - length2_(length2), - maxSpeed_(maxSpeed), - maxAcc_(maxAcc), - joint_home_positions_(home_positions), - minLimits_(minLimits), - maxLimits_(maxLimits) +ArmControl::ArmControl( + size_t total_joints, + double length1, + double length2, + double maxSpeed, + double maxAcc, + const std::vector& minLimits, + const std::vector& maxLimits, + const std::vector& home_positions, + const std::vector& joint_directions +): + total_joints_(total_joints), + length1_(length1), + length2_(length2), + maxSpeed_(maxSpeed), + maxAcc_(maxAcc), + joint_home_positions_(home_positions), + joint_directions_(joint_directions), + minLimits_(minLimits), + maxLimits_(maxLimits) { + std::cout << "ArmControl Constructor" << std::endl; if (total_joints_ <= 0) throw std::invalid_argument("Total joints must be positive"); @@ -24,15 +36,14 @@ ArmControl::ArmControl(size_t total_joints, double length1, double length2, doub joint_acceleration_.resize(total_joints_, 0.0); joint_torque_.resize(total_joints_, 0.0); joint_position_desired_.resize(total_joints_, 0.0); - joint_home_positions_.resize(total_joints_, 0.0); - - // 设置零点位置 - joint_home_positions_ = home_positions; // 初始化梯形轨迹规划器 trapezoidalTrajectory_ = new TrapezoidalTrajectory(10.0, 100); is_moving_ = false; + is_stopping_ = false; + stopDurationTime_ = 0.0; + movingDurationTime_ = 0.0; } ArmControl::~ArmControl() @@ -41,7 +52,7 @@ ArmControl::~ArmControl() } -void ArmControl::MoveToTargetPoint(const Eigen::Vector3d& target_pos, double duration) +bool ArmControl::MoveToTargetPoint(std::vector& joint_positions, const Eigen::Vector3d& target_pos, double dt) { // 注意：这里需要运动学逆解将笛卡尔空间目标位置转换为关节空间 // 为简化示例，这里假设已完成逆解，直接使用虚拟关节位置 @@ -51,33 +62,63 @@ void ArmControl::MoveToTargetPoint(const Eigen::Vector3d& target_pos, double dur // joint_target = inverse_kinematics(target_pos); // 使用梯形轨迹规划移动到目标关节位置 - MoveToTargetJoint(joint_target, duration); + return MoveToTargetJoint(joint_positions, joint_target, dt); } -void ArmControl::MoveToTargetJoint(const std::vector& target_joint, double duration) +bool ArmControl::MoveToTargetJoint(std::vector& joint_positions, const std::vector& target_joint, double dt) { - if (target_joint.size() != total_joints_) - throw std::invalid_argument("Target joint vector size mismatch"); - - // 初始化轨迹规划器 - trapezoidalTrajectory_->init(joint_position_, target_joint, duration); - - static double elapsed_time = 0.0; + if (joint_position_.size() != target_joint.size()) + { + throw std::invalid_argument("Joint position and target joint size do not match."); + } - // 更新轨迹 - joint_position_ = trapezoidalTrajectory_->update(elapsed_time); + if (!is_moving_) + { + bool all_joints_reached = true; + const double position_tolerance = 1e-3; + + for (size_t i = 0; i < target_joint.size(); i++) + { + double position_error = std::fabs(joint_position_[i] - target_joint[i]); + if (position_error > position_tolerance) + { + all_joints_reached = false; + break; + } + } + + if (all_joints_reached) + { + joint_positions = target_joint; + + return true; + } + + // 从当前关节位置开始规划轨迹 + trapezoidalTrajectory_->init(joint_position_, target_joint); + movingDurationTime_ = 0.0; + is_moving_ = true; + } + + movingDurationTime_ += dt; + + std::vector desired_pos = trapezoidalTrajectory_->update(movingDurationTime_); + + // 检查是否到达目标位置 + if (trapezoidalTrajectory_->isFinished(movingDurationTime_)) + { + joint_positions = target_joint; + is_moving_ = false; + movingDurationTime_ = 0.0; + + return true; + } + + joint_positions = desired_pos; joint_velocity_ = trapezoidalTrajectory_->getVelocity(); joint_acceleration_ = trapezoidalTrajectory_->getAcceleration(); - - // 更新时间 - if (trapezoidalTrajectory_->isFinished(elapsed_time)) - { - elapsed_time = 0.0; - } - else - { - elapsed_time += 0.01; // 假设控制周期为10ms - } + + return false; } bool ArmControl::GoHome(std::vector& joint_positions, double dt) @@ -154,35 +195,27 @@ bool ArmControl::Stop(std::vector& joint_positions, double dt) if (is_moving_ && !is_stopping_) { is_stopping_ = true; - - // 从当前关节位置开始规划轨迹 + std::cout << "arm control stop1" << std::endl; trapezoidalTrajectory_->StopCalculate(); + std::cout << "arm control stop2" << std::endl; + stopDurationTime_ = 0.0; } if (is_stopping_) { - static double elapsed_time = 0.0; - - std::vector desired_pos = trapezoidalTrajectory_->StopUpdate(dt); - - elapsed_time += dt; - - if (trapezoidalTrajectory_->stopFinished(elapsed_time)) + stopDurationTime_ += dt; + joint_positions = trapezoidalTrajectory_->StopUpdate(stopDurationTime_); + if (trapezoidalTrajectory_->stopFinished(stopDurationTime_)) { - joint_positions = joint_position_; is_moving_ = false; is_stopping_ = false; - elapsed_time = 0.0; - + stopDurationTime_ = 0.0; return true; } - - joint_positions = desired_pos; - return false; } - return true; // 如果没有移动或停止，直接返回true + return true; } void ArmControl::UpdateJointStates( diff --git a/HiveCoreR0/src/robot_control/src/leg_control.cpp b/HiveCoreR0/src/robot_control/src/leg_control.cpp index f5d7060..da34152 100644 --- a/HiveCoreR0/src/robot_control/src/leg_control.cpp +++ b/HiveCoreR0/src/robot_control/src/leg_control.cpp @@ -12,15 +12,17 @@ LegControl::LegControl( double maxAcc, const std::vector& minLimits, const std::vector& maxLimits, - const std::vector& home_positions + const std::vector& home_positions, + const std::vector& joint_directions ) : total_joints_(totalJoints), maxSpeed_(maxSpeed), maxAcc_(maxAcc), LegLength_(LegLength), joint_home_positions_(home_positions), - maxLimits_(maxLimits), - minLimits_(minLimits) + joint_directions_(joint_directions), + minLimits_(minLimits), + maxLimits_(maxLimits) { // 初始化关节状态向量 joint_position_.resize(total_joints_, 0.0); @@ -33,11 +35,15 @@ LegControl::LegControl( trapezoidalTrajectory_ = new TrapezoidalTrajectory(maxSpeed, maxAcc); is_moving_ = false; - is_stopping_ = false; + stopDurationTime_ = 0.0; + movingDurationTime_ = 0.0; } -LegControl::~LegControl() = default; +LegControl::~LegControl() +{ + delete trapezoidalTrajectory_; +} bool LegControl::MoveToTargetJoint(std::vector& joint_positions, const std::vector& target_joint, double dt) { @@ -45,18 +51,16 @@ bool LegControl::MoveToTargetJoint(std::vector& joint_positions, const s { throw std::invalid_argument("Joint position and target joint size do not match."); } - - static double elapsed_time = 0.0; if (!is_moving_) { bool all_joints_reached = true; - const double home_tolerance = 1e-3; + const double position_tolerance = 1e-3; for (size_t i = 0; i < target_joint.size(); i++) { double position_error = std::fabs(joint_position_[i] - target_joint[i]); - if (position_error > home_tolerance) + if (position_error > position_tolerance) { all_joints_reached = false; break; @@ -72,32 +76,26 @@ bool LegControl::MoveToTargetJoint(std::vector& joint_positions, const s // 从当前关节位置开始规划轨迹 trapezoidalTrajectory_->init(joint_position_, target_joint); - elapsed_time = 0.0; + movingDurationTime_ = 0.0; is_moving_ = true; - - std::cout << "Leg Moving to target joint! " << std::endl; } - std::vector desired_pos = trapezoidalTrajectory_->update(elapsed_time); - elapsed_time += dt; + movingDurationTime_ += dt; + + std::vector desired_pos = trapezoidalTrajectory_->update(movingDurationTime_); // 检查是否到达目标位置 - if (trapezoidalTrajectory_->isFinished(elapsed_time)) + if (trapezoidalTrajectory_->isFinished(movingDurationTime_)) { joint_positions = target_joint; is_moving_ = false; - elapsed_time = 0.0; - - std::cout << "Leg moving finished" << std::endl; + movingDurationTime_ = 0.0; return true; } - else - { - joint_velocity_ = trapezoidalTrajectory_->getVelocity(); - joint_acceleration_ = trapezoidalTrajectory_->getAcceleration(); - joint_positions = desired_pos; - } + joint_positions = desired_pos; + joint_velocity_ = trapezoidalTrajectory_->getVelocity(); + joint_acceleration_ = trapezoidalTrajectory_->getAcceleration(); return false; } @@ -108,7 +106,14 @@ bool LegControl::MoveUp(std::vector& joint_positions, double dt) { for (size_t i = 0; i < joint_position_.size(); i++) { - joint_position_desired_[i] = maxLimits_[i]; + if (joint_directions_[i] == 1) + { + joint_position_desired_[i] = maxLimits_[i]; + } + else + { + joint_position_desired_[i] = minLimits_[i]; + } } } @@ -121,7 +126,14 @@ bool LegControl::MoveDown(std::vector& joint_positions, double dt) { for (size_t i = 0; i < joint_position_.size(); i++) { - joint_position_desired_[i] = minLimits_[i]; + if (joint_directions_[i] == 1) + { + joint_position_desired_[i] = minLimits_[i]; + } + else + { + joint_position_desired_[i] = maxLimits_[i]; + } } } @@ -133,34 +145,21 @@ bool LegControl::Stop(std::vector& joint_positions, double dt) if (is_moving_ && !is_stopping_) { is_stopping_ = true; - - // 从当前关节位置开始规划轨迹 trapezoidalTrajectory_->StopCalculate(); - - std::cout << "Leg stopping " << std::endl; + stopDurationTime_ = 0.0; } if (is_stopping_) { - static double elapsed_time = 0.0; - - std::vector desired_pos = trapezoidalTrajectory_->StopUpdate(dt); - - elapsed_time += dt; - - if (trapezoidalTrajectory_->stopFinished(elapsed_time)) + stopDurationTime_ += dt; + joint_positions = trapezoidalTrajectory_->StopUpdate(stopDurationTime_); + if (trapezoidalTrajectory_->stopFinished(stopDurationTime_)) { - joint_positions = joint_position_; is_moving_ = false; is_stopping_ = false; - elapsed_time = 0.0; - - std::cout << "Leg stopping finished" << std::endl; + stopDurationTime_ = 0.0; return true; } - - joint_positions = desired_pos; - return false; } diff --git a/HiveCoreR0/src/robot_control/src/main.cpp b/HiveCoreR0/src/robot_control/src/main.cpp index 04c5efa..1eafc55 100644 --- a/HiveCoreR0/src/robot_control/src/main.cpp +++ b/HiveCoreR0/src/robot_control/src/main.cpp @@ -1,5 +1,6 @@ #include #include "robot_fsm.hpp" +#include /** * @brief 程序入口函数 diff --git a/HiveCoreR0/src/robot_control/src/plot_joint_trajectpry_multi.py b/HiveCoreR0/src/robot_control/src/plot_joint_trajectpry_multi.py index 0fe0c62..cda956a 100644 --- a/HiveCoreR0/src/robot_control/src/plot_joint_trajectpry_multi.py +++ b/HiveCoreR0/src/robot_control/src/plot_joint_trajectpry_multi.py @@ -47,7 +47,7 @@ def plot_joint_trajectories(file_path): plt.figure(figsize=(12, 8)) for i in range(num_plots): - plt.plot(relative_times, joint_data[i], label=f'关节 {i+1}') + plt.plot(joint_data[i], label=f'关节 {i+1}') # 图形配置 plt.xlabel('时间 (秒)') diff --git a/HiveCoreR0/src/robot_control/src/robot_control_manager.cpp b/HiveCoreR0/src/robot_control/src/robot_control_manager.cpp index 71e53b0..d03d088 100644 --- a/HiveCoreR0/src/robot_control/src/robot_control_manager.cpp +++ b/HiveCoreR0/src/robot_control/src/robot_control_manager.cpp @@ -12,10 +12,7 @@ RobotControlManager::RobotControlManager() void RobotControlManager::init() { - robotInited_ = false; - - // Initialize the robot control manager - cout << "Initializing robot control manager..." << std::endl; + jointInited_.resize(motionParams_.total_joints_, false); // Initialize the robot control manager motionParams_ = MotionParameters(); @@ -34,7 +31,8 @@ void RobotControlManager::init() motionParams_.max_joint_acceleration_, motionParams_.left_arm_min_limit_, motionParams_.left_arm_max_limit_, - motionParams_.left_arm_home_positions_ + motionParams_.left_arm_home_positions_, + motionParams_.left_arm_joint_indices_ ); rightArmController_ = new ArmControl( @@ -45,7 +43,8 @@ void RobotControlManager::init() motionParams_.max_joint_acceleration_, motionParams_.right_arm_min_limit_, motionParams_.right_arm_max_limit_, - motionParams_.right_arm_home_positions_ + motionParams_.right_arm_home_positions_, + motionParams_.right_arm_joint_indices_ ); leftLegController_ = new LegControl( @@ -55,7 +54,8 @@ void RobotControlManager::init() motionParams_.max_joint_acceleration_, motionParams_.left_leg_min_limit_, motionParams_.left_leg_max_limit_, - motionParams_.left_leg_home_positions_ + motionParams_.left_leg_home_positions_, + motionParams_.left_leg_joint_directions_ ); rightLegController_ = new LegControl( @@ -65,7 +65,8 @@ void RobotControlManager::init() motionParams_.max_joint_acceleration_, motionParams_.right_leg_min_limit_, motionParams_.right_leg_max_limit_, - motionParams_.right_leg_home_positions_ + motionParams_.right_leg_home_positions_, + motionParams_.right_leg_joint_directions_ ); @@ -129,9 +130,19 @@ void RobotControlManager::CheckJointLimits(Float64MultiArray &tempJointValues) } } +bool isAllTrueManual(const std::vector& vec) { + for (bool element : vec) { // 范围 for 循环遍历每个元素 + if (!element) { // 若存在 false，直接返回 false + return false; + } + } + return true; // 所有元素都是 true +} + +bool isPrint = false; + void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState::SharedPtr msg) { - std::cout << "Update joint states" << std::endl; jointStates_ = *msg; jointPositions_.resize(motionParams_.total_joints_); jointVelocities_.resize(motionParams_.total_joints_); @@ -140,36 +151,46 @@ void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState:: for (size_t i = 0; i < motionParams_.total_joints_; i++) { jointPositions_[i] = jointStates_.position[i] * motionParams_.pulse_to_degree_; // convert to degree - jointVelocities_[i] = 0; //jointStates_.velocity[i] * motionParams_.pulse_to_degree_; - jointEfforts_[i] = 0; //jointStates_.effort[i] ; // TODO: check the factory + jointVelocities_[i] = jointStates_.velocity[i] * motionParams_.pulse_to_degree_; + jointEfforts_[i] = 0.0; //jointStates_.effort[i] ; // TODO: check the factory } - std::cout << "Joint positions: "; - // TODO: This block can be deleted - if(!robotInited_) + for (size_t i = 0; i < motionParams_.total_joints_; i++) { - robotInited_ = true; - for (size_t i = 0; i < motionParams_.total_joints_; i++) - { + if(!jointInited_[i]) + { jointCommands_.data[i] = jointStates_.position[i]* motionParams_.pulse_to_degree_; - std::cout << "Robot inited joint " << i + 1 << jointCommands_.data[i] << std::endl; - } - return; + + if(jointCommands_.data[i] != 0) + { + jointInited_[i] = true; + } + } } - // // Update the arm controller - // size_t armJointSize = motionParams_.left_arm_joint_indices_.size(); - // std::vector armPositions(jointPositions_.begin() + motionParams_.left_arm_joint_indices_[0], jointPositions_.begin() + motionParams_.left_arm_joint_indices_[0] + armJointSize); - // std::vector armVelocities(jointVelocities_.begin() + motionParams_.left_arm_joint_indices_[0], jointVelocities_.begin() + motionParams_.left_arm_joint_indices_[0] + armJointSize); - // std::vector armEfforts(jointEfforts_.begin() + motionParams_.left_arm_joint_indices_[0], jointEfforts_.begin() + motionParams_.left_arm_joint_indices_[0] + armJointSize); - // leftArmController_->UpdateJointStates(armPositions, armVelocities, armEfforts); + if (isAllTrueManual(jointInited_) && !isPrint) + { + isPrint = true; + std::cout << "All joints are initialized" << std::endl; + for (size_t i = 0; i < motionParams_.total_joints_; i++) + { + std::cout << "Joint positions: " << i + 1 << " " << jointCommands_.data[i] << std::endl; + } + } + + // Update the arm controller + size_t armJointSize = motionParams_.left_arm_joint_indices_.size(); + std::vector armPositions(jointPositions_.begin() + motionParams_.left_arm_joint_indices_[0], jointPositions_.begin() + motionParams_.left_arm_joint_indices_[0] + armJointSize); + std::vector armVelocities(jointVelocities_.begin() + motionParams_.left_arm_joint_indices_[0], jointVelocities_.begin() + motionParams_.left_arm_joint_indices_[0] + armJointSize); + std::vector armEfforts(jointEfforts_.begin() + motionParams_.left_arm_joint_indices_[0], jointEfforts_.begin() + motionParams_.left_arm_joint_indices_[0] + armJointSize); + leftArmController_->UpdateJointStates(armPositions, armVelocities, armEfforts); - // armPositions = std::vector(jointPositions_.begin() + motionParams_.right_arm_joint_indices_[0], jointPositions_.begin() + motionParams_.right_arm_joint_indices_[0] + armJointSize); - // armVelocities = std::vector(jointVelocities_.begin() + motionParams_.right_arm_joint_indices_[0], jointVelocities_.begin() + motionParams_.right_arm_joint_indices_[0] + armJointSize); - // armEfforts = std::vector(jointEfforts_.begin() + motionParams_.right_arm_joint_indices_[0], jointEfforts_.begin() + motionParams_.right_arm_joint_indices_[0] + armJointSize); - // rightArmController_->UpdateJointStates(armPositions, armVelocities, armEfforts); + armPositions = std::vector(jointPositions_.begin() + motionParams_.right_arm_joint_indices_[0], jointPositions_.begin() + motionParams_.right_arm_joint_indices_[0] + armJointSize); + armVelocities = std::vector(jointVelocities_.begin() + motionParams_.right_arm_joint_indices_[0], jointVelocities_.begin() + motionParams_.right_arm_joint_indices_[0] + armJointSize); + armEfforts = std::vector(jointEfforts_.begin() + motionParams_.right_arm_joint_indices_[0], jointEfforts_.begin() + motionParams_.right_arm_joint_indices_[0] + armJointSize); + rightArmController_->UpdateJointStates(armPositions, armVelocities, armEfforts); // Update the leg controller size_t legJointSize = motionParams_.left_leg_joint_indices_.size(); @@ -182,6 +203,7 @@ void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState:: legVelocities = std::vector(jointVelocities_.begin() + motionParams_.right_leg_joint_indices_[0], jointVelocities_.begin() + motionParams_.right_leg_joint_indices_[0] + legJointSize); legEfforts = std::vector(jointEfforts_.begin() + motionParams_.right_leg_joint_indices_[0], jointEfforts_.begin() + motionParams_.right_leg_joint_indices_[0] + legJointSize); rightLegController_->UpdateJointStates(legPositions, legVelocities, legEfforts); + } MotionParameters RobotControlManager::GetMotionParameters() @@ -213,35 +235,39 @@ void RobotControlManager::TurnRight() wheelController_ -> ExecuteCommand(); } -void RobotControlManager::Stop(double dt) +bool RobotControlManager::Stop(double dt) { wheelController_ -> setLinearAngularSpeed(0.0, 0.0); wheelController_ -> ExecuteCommand(); - leftLegController_->Stop(tempLeftLegCmd, dt); + bool leftLegStopped = leftLegController_->Stop(tempLeftLegCmd, dt); - rightLegController_->Stop(tempRightLegCmd, dt); + bool rightLegStopped = rightLegController_->Stop(tempRightLegCmd, dt); - leftArmController_->Stop(tempLeftArmCmd, dt); + bool leftArmStopped = leftArmController_->Stop(tempLeftArmCmd, dt); - rightArmController_->Stop(tempRightArmCmd, dt); + bool rightArmStopped = rightArmController_->Stop(tempRightArmCmd, dt); for (size_t i = 0; i < motionParams_.left_leg_joint_indices_.size(); i++) { - jointCommands_.data[motionParams_.left_leg_joint_indices_[i]] = motionParams_.left_leg_joint_directions_[i] * tempLeftLegCmd[i]; + jointCommands_.data[motionParams_.left_leg_joint_indices_[i]] = tempLeftLegCmd[i]; } for (size_t i = 0; i < motionParams_.right_leg_joint_indices_.size(); i++) { - jointCommands_.data[motionParams_.right_leg_joint_indices_[i]] = motionParams_.right_leg_joint_directions_[i] * tempRightLegCmd[i]; - } - for (size_t i = 0; i < motionParams_.left_arm_joint_indices_.size(); i++) - { - jointCommands_.data[motionParams_.left_arm_joint_indices_[i]] = motionParams_.left_arm_joint_directions_[i] * tempLeftArmCmd[i]; - } - for (size_t i = 0; i < motionParams_.right_arm_joint_indices_.size(); i++) - { - jointCommands_.data[motionParams_.right_arm_joint_indices_[i]] = motionParams_.right_arm_joint_directions_[i] * tempRightArmCmd[i]; + jointCommands_.data[motionParams_.right_leg_joint_indices_[i]] = tempRightLegCmd[i]; } + + //TODO: there is no data from arm + // for (size_t i = 0; i < motionParams_.left_arm_joint_indices_.size(); i++) + // { + // jointCommands_.data[motionParams_.left_arm_joint_indices_[i]] = motionParams_.left_arm_joint_directions_[i] * tempLeftArmCmd[i]; + // } + // for (size_t i = 0; i < motionParams_.right_arm_joint_indices_.size(); i++) + // { + // jointCommands_.data[motionParams_.right_arm_joint_indices_[i]] = motionParams_.right_arm_joint_directions_[i] * tempRightArmCmd[i]; + // } + + return leftLegStopped && rightLegStopped && leftArmStopped && rightArmStopped; } void RobotControlManager::MoveUp(double dt) @@ -254,11 +280,11 @@ void RobotControlManager::MoveUp(double dt) for (size_t i = 0; i < motionParams_.left_leg_joint_indices_.size(); i++) { - jointCommands_.data[motionParams_.left_leg_joint_indices_[i]] = motionParams_.left_leg_joint_directions_[i] * tempLeftLegCmd[i]; + jointCommands_.data[motionParams_.left_leg_joint_indices_[i]] = tempLeftLegCmd[i]; } for (size_t i = 0; i < motionParams_.right_leg_joint_indices_.size(); i++) { - jointCommands_.data[motionParams_.right_leg_joint_indices_[i]] = motionParams_.right_leg_joint_directions_[i] * tempRightLegCmd[i]; + jointCommands_.data[motionParams_.right_leg_joint_indices_[i]] = tempRightLegCmd[i]; } } @@ -272,11 +298,11 @@ void RobotControlManager::MoveDown(double dt) for (size_t i = 0; i < motionParams_.left_leg_joint_indices_.size(); i++) { - jointCommands_.data[motionParams_.left_leg_joint_indices_[i]] = motionParams_.left_leg_joint_directions_[i] * tempLeftLegCmd[i]; + jointCommands_.data[motionParams_.left_leg_joint_indices_[i]] = tempLeftLegCmd[i]; } for (size_t i = 0; i < motionParams_.right_leg_joint_indices_.size(); i++) { - jointCommands_.data[motionParams_.right_leg_joint_indices_[i]] = motionParams_.right_leg_joint_directions_[i] * tempRightLegCmd[i]; + jointCommands_.data[motionParams_.right_leg_joint_indices_[i]] = tempRightLegCmd[i]; } } @@ -300,23 +326,23 @@ bool RobotControlManager::GoHome(double dt) } } - // if(!robotHomed_[2]) - // { - // robotHomed_[2] = leftLegController_->GoHome(tempLeftLegCmd, dt); - // for (size_t i = 0; i < motionParams_.left_leg_joint_indices_.size(); i++) - // { - // jointCommands_.data[motionParams_.left_leg_joint_indices_[i]] = tempLeftLegCmd[i]; - // } - // } + if(!robotHomed_[2]) + { + robotHomed_[2] = leftLegController_->GoHome(tempLeftLegCmd, dt); + for (size_t i = 0; i < motionParams_.left_leg_joint_indices_.size(); i++) + { + jointCommands_.data[motionParams_.left_leg_joint_indices_[i]] = tempLeftLegCmd[i]; + } + } - // if(!robotHomed_[3]) - // { - // robotHomed_[3] = rightLegController_->GoHome(tempRightLegCmd, dt); - // for (size_t i = 0; i < motionParams_.right_leg_joint_indices_.size(); i++) - // { - // jointCommands_.data[motionParams_.right_leg_joint_indices_[i]] = tempRightLegCmd[i]; - // } - // } + if(!robotHomed_[3]) + { + robotHomed_[3] = rightLegController_->GoHome(tempRightLegCmd, dt); + for (size_t i = 0; i < motionParams_.right_leg_joint_indices_.size(); i++) + { + jointCommands_.data[motionParams_.right_leg_joint_indices_[i]] = tempRightLegCmd[i]; + } + } return robotHomed_[0] && robotHomed_[1];// && homeFinished[2] && homeFinished[3]; } diff --git a/HiveCoreR0/src/robot_control/src/robot_fsm.cpp b/HiveCoreR0/src/robot_control/src/robot_fsm.cpp index 7772524..3ce03a0 100644 --- a/HiveCoreR0/src/robot_control/src/robot_fsm.cpp +++ b/HiveCoreR0/src/robot_control/src/robot_fsm.cpp @@ -6,13 +6,16 @@ using namespace robot_control; namespace fs = std::filesystem; // 机器人控制器构造函数 -RobotFsm::RobotFsm() : Node("robot_Fsm_node"){ - +RobotFsm::RobotFsm() : Node("robot_Fsm_node") +{ + std::cout << "RobotFsm Node is created!" << std::endl; InitTransitionTable(); - currentState_ = RobotState::STOPPED; - previousState_ = RobotState::STOPPED; + currentState_ = RobotState::INIT; + previousState_ = RobotState::INIT; isRobotEnabled_ = false; + enableCommandExecuted_ = false; + jogDirection_ = 0; // 初始化数据文件（设置路径，确保目录存在） data_file_path_ = "/home/demo/ros2_joint_data.txt"; // 数据保存路径 @@ -26,9 +29,6 @@ RobotFsm::RobotFsm() : Node("robot_Fsm_node"){ } else { // 写入表头（仅在文件为空时） data_file_.seekp(0, std::ios::end); // 移动到文件末尾 - if (data_file_.tellp() == 0) { // 文件为空 - data_file_ << "timestamp(sec),joint_1_position" << std::endl; - } } // 添加输入设备 @@ -58,6 +58,8 @@ RobotFsm::RobotFsm() : Node("robot_Fsm_node"){ // 创建定时器，每10ms执行一次控制逻辑（频率100Hz） controlTimer_ = this->create_wall_timer(std::chrono::milliseconds(10),std::bind(&RobotFsm::ControlLoop, this)); // 绑定到新的控制函数); + + std::cout << "RobotFsm Node is created finished!" << std::endl; } RobotFsm::~RobotFsm() @@ -68,9 +70,10 @@ RobotFsm::~RobotFsm() std::cout << "Robot controller stopped." << std::endl; } -void RobotFsm::JointStatesCallback(const sensor_msgs::msg::JointState::SharedPtr msg) { +void RobotFsm::JointStatesCallback(const sensor_msgs::msg::JointState::SharedPtr msg) +{ if (!msg) { // 检查消息是否有效 - RCLCPP_WARN(this->get_logger(), "收到空的joint_states消息,忽略"); + std::cout << "get null joint states!" << std::endl; return; } @@ -134,20 +137,25 @@ void RobotFsm::JoyCommandCallback(const std_msgs::msg::String::SharedPtr msg) { inputDevices_[InputType::JOY]->SetCommand(ControlCommand::RIGHT); } else if (command == "上升") { + jogDirection_ = 1; inputDevices_[InputType::JOY]->SetCommand(ControlCommand::UP); } else if (command == "下降") { + jogDirection_ = -1; inputDevices_[InputType::JOY]->SetCommand(ControlCommand::DOWN); } else if (command == "停止") { + jogDirection_ = 0; inputDevices_[InputType::JOY]->SetCommand(ControlCommand::STOP); } else if (command == "A") { - inputDevices_[InputType::JOY]->SetCommand(ControlCommand::GO_HOME); + // TODO: need to be checked. + // inputDevices_[InputType::JOY]->SetCommand(ControlCommand::GO_HOME); } else if (command == "B") { - if (!isRobotEnabled_) + if (!enableCommandExecuted_) { + enableCommandExecuted_ = true; EnableRobot(); } } @@ -158,10 +166,10 @@ void RobotFsm::JoyCommandCallback(const std_msgs::msg::String::SharedPtr msg) { void RobotFsm::EnableRobot() { - control_msgs::msg::DynamicInterfaceGroupValues enableMsg; - for (size_t i = 0; i < robotControlManager_.GetMotionParameters().total_joints_; i++) { + control_msgs::msg::DynamicInterfaceGroupValues enableMsg; + std::string tempInterfaceGroup = "joint_" + std::to_string(i+1); enableMsg.interface_groups.push_back(tempInterfaceGroup); @@ -169,12 +177,15 @@ void RobotFsm::EnableRobot() tempValue.interface_names = {"enable"}; tempValue.values = {1}; enableMsg.interface_values.push_back(tempValue); - } - - std::cout << "Enable robot" << std::endl; - // 发布消息 - gpioPub_->publish(enableMsg); + std::cout << "Enable robot " << i + 1 << std::endl; + + gpioPub_->publish(enableMsg); + + sleep(1); + } + + sleep(2); isRobotEnabled_ = true; } @@ -215,17 +226,15 @@ void RobotFsm::VoiceCommandCallback(const std_msgs::msg::String::SharedPtr msg) // 初始化状态转换表 void RobotFsm::InitTransitionTable() { - // 从任意状态都可以转换到停止状态 transitionTable_[{RobotState::STOPPED, ControlCommand::STOP}] = RobotState::STOPPED; - transitionTable_[{RobotState::MOVING_FORWARD, ControlCommand::STOP}] = RobotState::STOPPED; - transitionTable_[{RobotState::MOVING_BACKWARD, ControlCommand::STOP}] = RobotState::STOPPED; - transitionTable_[{RobotState::TURNING_LEFT, ControlCommand::STOP}] = RobotState::STOPPED; - transitionTable_[{RobotState::TURNING_RIGHT, ControlCommand::STOP}] = RobotState::STOPPED; - transitionTable_[{RobotState::MOVING_UP, ControlCommand::STOP}] = RobotState::STOPPED; - transitionTable_[{RobotState::MOVING_DOWN, ControlCommand::STOP}] = RobotState::STOPPED; - transitionTable_[{RobotState::HOME_MOVING, ControlCommand::STOP}] = RobotState::STOPPED; + transitionTable_[{RobotState::MOVING_FORWARD, ControlCommand::STOP}] = RobotState::STOPPING; + transitionTable_[{RobotState::MOVING_BACKWARD, ControlCommand::STOP}] = RobotState::STOPPING; + transitionTable_[{RobotState::TURNING_LEFT, ControlCommand::STOP}] = RobotState::STOPPING; + transitionTable_[{RobotState::TURNING_RIGHT, ControlCommand::STOP}] = RobotState::STOPPING; + transitionTable_[{RobotState::MOVING_UP, ControlCommand::STOP}] = RobotState::STOPPING; + transitionTable_[{RobotState::MOVING_DOWN, ControlCommand::STOP}] = RobotState::STOPPING; + transitionTable_[{RobotState::HOME_MOVING, ControlCommand::STOP}] = RobotState::STOPPING; - // 从停止状态转换到其他状态 transitionTable_[{RobotState::STOPPED, ControlCommand::FORWARD}] = RobotState::MOVING_FORWARD; transitionTable_[{RobotState::STOPPED, ControlCommand::BACKWARD}] = RobotState::MOVING_BACKWARD; transitionTable_[{RobotState::STOPPED, ControlCommand::LEFT}] = RobotState::TURNING_LEFT; @@ -233,6 +242,14 @@ void RobotFsm::InitTransitionTable() { transitionTable_[{RobotState::STOPPED, ControlCommand::UP}] = RobotState::MOVING_UP; transitionTable_[{RobotState::STOPPED, ControlCommand::DOWN}] = RobotState::MOVING_DOWN; transitionTable_[{RobotState::STOPPED, ControlCommand::GO_HOME}] = RobotState::HOME_MOVING; + + transitionTable_[{RobotState::INIT, ControlCommand::FORWARD}] = RobotState::MOVING_FORWARD; + transitionTable_[{RobotState::INIT, ControlCommand::BACKWARD}] = RobotState::MOVING_BACKWARD; + transitionTable_[{RobotState::INIT, ControlCommand::LEFT}] = RobotState::TURNING_LEFT; + transitionTable_[{RobotState::INIT, ControlCommand::RIGHT}] = RobotState::TURNING_RIGHT; + transitionTable_[{RobotState::INIT, ControlCommand::UP}] = RobotState::MOVING_UP; + transitionTable_[{RobotState::INIT, ControlCommand::DOWN}] = RobotState::MOVING_DOWN; + transitionTable_[{RobotState::INIT, ControlCommand::GO_HOME}] = RobotState::HOME_MOVING; } // 添加输入设备 @@ -241,32 +258,34 @@ void RobotFsm::AddInputDevice(InputType type, std::unique_ptr devic std::cout << "Added input device: " << inputDevices_[type]->GetDeviceName() << std::endl; } + // 状态机主循环 void RobotFsm::ControlLoop() { - // 检查所有输入设备 - ControlCommand cmd = ControlCommand::NONE; - for (const auto& [type, device] : inputDevices_) { - ControlCommand deviceCmd = device->GetCommand(); - if (deviceCmd != ControlCommand::NONE) { - cmd = deviceCmd; - break; - } + // 检查所有输入设备 + ControlCommand cmd = ControlCommand::NONE; + for (const auto& [type, device] : inputDevices_) { + ControlCommand deviceCmd = device->GetCommand(); + if (deviceCmd != ControlCommand::NONE) { + cmd = deviceCmd; + break; } - - ProcessCommand(cmd); + } + + ProcessCommand(cmd); - // 计算时间差 - rclcpp::Time current_time = this->now(); - rclcpp::Duration dt = current_time - lastTime_; - double dt_sec = dt.seconds(); - lastTime_ = current_time; - - ExecuteStateAction(dt_sec); + // 计算时间差 + rclcpp::Time current_time = this->now(); + rclcpp::Duration dt = current_time - lastTime_; + double dt_sec = dt.seconds(); + lastTime_ = current_time; + + ExecuteStateAction(dt_sec); - if (isRobotEnabled_) { - Gpio_publish_joint_trajectory(); - } + if (isRobotEnabled_) + { + Gpio_publish_joint_trajectory(); + } } @@ -304,38 +323,72 @@ void RobotFsm::Publish_joint_trajectory() void RobotFsm::Gpio_publish_joint_trajectory() { - auto cmd_msg = robotControlManager_.GetJointCommands(); + static size_t jointIndex = 5; + static auto jointValue = robotControlManager_.GetJointCommands().data; + static double jogValue = jointValue[jointIndex -1]; - // 1. 获取当前时间戳 - rclcpp::Time current_time = this->now(); - double timestamp = current_time.seconds(); + std::cout << jogValue << std::endl; - // 2. 保存整个数组数据到txt文件 - if (data_file_.is_open()) { - data_file_ << timestamp; + if (true) + { + control_msgs::msg::DynamicInterfaceGroupValues positionMsg; + for (size_t i = 0; i < robotControlManager_.GetMotionParameters().total_joints_; i++) + { + std::string tempInterfaceGroup = "joint_" + std::to_string(i + 1); + positionMsg.interface_groups.push_back(tempInterfaceGroup); - for (const auto& val : cmd_msg.data) { - data_file_ << "," << val; // 用逗号分隔每个元素 + double tempJointValue; + if (i + 1 == jointIndex) + { + jogValue = jogValue + jogDirection_ * 10; + tempJointValue = jogValue; + } + else + { + tempJointValue = jointValue[i]; + } + + control_msgs::msg::InterfaceValue tempValue; + tempValue.interface_names = {"position"}; + tempValue.values = {tempJointValue}; + positionMsg.interface_values.push_back(tempValue); + } + + + // 发布消息 + gpioPub_->publish(positionMsg); + } + else + { + auto cmd_msg = robotControlManager_.GetJointCommands(); + + data_file_ << 0; + + // 2. 保存整个数组数据到txt文件 + if (data_file_.is_open()) { + for (const auto& val : cmd_msg.data) { + data_file_ << "," << val; // 用逗号分隔每个元素 + } + + data_file_ << std::endl; // 换行 + data_file_.flush(); // 强制刷新 + } + + control_msgs::msg::DynamicInterfaceGroupValues positionMsg; + for (size_t i = 0; i < robotControlManager_.GetMotionParameters().total_joints_; i++) + { + std::string tempInterfaceGroup = "joint_" + std::to_string(i+1); + positionMsg.interface_groups.push_back(tempInterfaceGroup); + + control_msgs::msg::InterfaceValue tempValue; + tempValue.interface_names = {"position"}; + tempValue.values = {cmd_msg.data[i]}; + positionMsg.interface_values.push_back(tempValue); } - data_file_ << std::endl; // 换行 - data_file_.flush(); // 强制刷新 + // 发布消息 + //gpioPub_->publish(positionMsg); } - - control_msgs::msg::DynamicInterfaceGroupValues positionMsg; - for (size_t i = 0; i < robotControlManager_.GetMotionParameters().total_joints_; i++) - { - std::string tempInterfaceGroup = "joint_" + std::to_string(i+1); - positionMsg.interface_groups.push_back(tempInterfaceGroup); - - control_msgs::msg::InterfaceValue tempValue; - tempValue.interface_names = {"position"}; - tempValue.values = {cmd_msg.data[i]}; - positionMsg.interface_values.push_back(tempValue); - } - - // 发布消息 - // gpioPub_->publish(positionMsg); } // 处理命令并转换状态 @@ -358,8 +411,15 @@ void RobotFsm::ProcessCommand(ControlCommand cmd) { void RobotFsm::ExecuteStateAction(double dt) { switch (currentState_) { + case RobotState::INIT: + break; + case RobotState::STOPPING: + if(robotControlManager_.Stop(dt)) + { + currentState_ = RobotState::STOPPED; + } + break; case RobotState::STOPPED: - robotControlManager_.Stop(dt); break; case RobotState::MOVING_FORWARD: robotControlManager_.MoveForward(); @@ -383,7 +443,7 @@ void RobotFsm::ExecuteStateAction(double dt) { case RobotState::HOME_MOVING: if(robotControlManager_.GoHome(dt)) { - inputDevices_[InputType::JOY]->SetCommand(ControlCommand::STOP); + currentState_ = RobotState::STOPPED; } break; } @@ -394,6 +454,8 @@ void RobotFsm::ExecuteStateAction(double dt) { // 状态转换为字符串 std::string RobotFsm::StateToString(RobotState state) const { switch (state) { + case RobotState::INIT: return "INIT"; + case RobotState::STOPPING: return "STOPPING"; case RobotState::STOPPED: return "STOPPED"; case RobotState::MOVING_FORWARD: return "MOVING_FORWARD"; case RobotState::MOVING_BACKWARD: return "MOVING_BACKWARD"; diff --git a/HiveCoreR0/src/robot_control/src/trapezoidal_trajectory.cpp b/HiveCoreR0/src/robot_control/src/trapezoidal_trajectory.cpp index f953d02..7211f11 100644 --- a/HiveCoreR0/src/robot_control/src/trapezoidal_trajectory.cpp +++ b/HiveCoreR0/src/robot_control/src/trapezoidal_trajectory.cpp @@ -32,8 +32,11 @@ void TrapezoidalTrajectory::init(const std::vector& start_pos, const std start_pos_ = start_pos; target_pos_ = target_pos; current_pos_.resize(start_pos.size()); + stop_pos_.resize(start_pos.size()); + stop_vel_.resize(start_pos.size()); current_velocity_.resize(start_pos.size()); current_acceleration_.resize(start_pos.size()); + actual_deceleration_.resize(start_pos.size()); is_stopping_ = false; calculateTrajectoryParams(); @@ -259,6 +262,9 @@ void TrapezoidalTrajectory::StopCalculate() { size_t dof = current_pos_.size(); // 自由度数量 std::vector decel_times(dof); // 各轴理论减速时间 + + stop_pos_ = current_pos_; + stop_vel_ = current_velocity_; for (size_t i = 0; i < dof; ++i) { @@ -274,10 +280,10 @@ void TrapezoidalTrajectory::StopCalculate() return; } - // 第二步：计算各轴实际减速度（确保在总时间内减速到0） for (size_t i = 0; i < dof; ++i) { if (std::fabs(current_velocity_[i]) < 1e-9) { actual_deceleration_[i] = 0.0; // 静止轴减速度为0 + } else { // 实际减速度 = 初始速度 / 总时间（方向与速度相反） actual_deceleration_[i] = -std::copysign(max_acceleration_, current_velocity_[i]) @@ -288,15 +294,19 @@ void TrapezoidalTrajectory::StopCalculate() std::vector TrapezoidalTrajectory::StopUpdate(double dt) { + if(dt > stop_time_) + { + dt = stop_time_; + } + for (size_t i = 0; i < current_pos_.size(); ++i) { - // 计算当前速度：v(t) = v0 + a*t - current_velocity_[i] = current_velocity_[i] + actual_deceleration_[i] * dt; - - // 计算当前位置：s(t) = s0 + v0*t + 0.5*a*t² - current_pos_[i] = current_pos_[i] - + current_velocity_[i] * dt + + current_pos_[i] = stop_pos_[i] + + stop_vel_[i] * dt + 0.5 * actual_deceleration_[i] * dt * dt; + current_velocity_[i] = stop_vel_[i] + actual_deceleration_[i] * dt; + // 数值稳定性处理（避免因浮点误差出现微小负速度） if (std::fabs(current_velocity_[i]) < 1e-9) { current_velocity_[i] = 0.0;