├── .gitignore ├── CMakeLists.txt ├── Common.h ├── EcatAdmin.cpp ├── EcatAdmin.h ├── README.md ├── main.cpp ├── motor.cpp ├── motor.h └── slave_config.h /.gitignore: -------------------------------------------------------------------------------- 1 | .idea 2 | cmake-build-debug 3 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(ros) 3 | 4 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 5 | 6 | #set(CMAKE_CXX_STANDARD 11) 7 | 8 | set(ETHERCAT_INCLUDE_DIR /usr/local/include) 9 | set(ETHERCAT_LIBRARY /usr/local/lib/libethercat.so.1.0.0) 10 | 11 | include_directories(${ETHERCAT_INCLUDE_DIR}) 12 | 13 | 14 | set(SOURCE_FILES main.cpp motor.cpp motor.h Common.h slave_config.h EcatAdmin.cpp EcatAdmin.h) 15 | 16 | add_executable(ros ${SOURCE_FILES}) 17 | 18 | target_link_libraries(ros ${ETHERCAT_LIBRARY}) 19 | -------------------------------------------------------------------------------- /Common.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by han on 16-12-27. 3 | // 4 | 5 | #ifndef ROS_COMMON_H 6 | #define ROS_COMMON_H 7 | 8 | #include 9 | 10 | // used 6 * 6 = 36 in fact 11 | #define ECAT_REG_TABLE_SIZE 64 12 | #define PI 3.14159265359 13 | 14 | #endif //ROS_COMMON_H 15 | -------------------------------------------------------------------------------- /EcatAdmin.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by han on 16-12-27. 3 | // 4 | 5 | #include "EcatAdmin.h" 6 | 7 | ec_master_t* EcatAdmin::master = nullptr; 8 | ec_master_state_t EcatAdmin::master_state = {}; 9 | 10 | ec_domain_t* EcatAdmin::domain1 = nullptr; 11 | ec_domain_state_t EcatAdmin::domain1_state = {}; 12 | 13 | uint8_t* EcatAdmin::domain1_pd = nullptr; 14 | 15 | EcatAdmin::slaves_map EcatAdmin::slaves_dict; 16 | 17 | int EcatAdmin::start_for_ros_control() 18 | { 19 | master = ecrt_request_master(0); 20 | 21 | if (!master) { 22 | printf("master init error\n"); 23 | goto out_return; 24 | } 25 | 26 | domain1 = ecrt_master_create_domain(master); 27 | if (!domain1) { 28 | printf("domain1 init error\n"); 29 | goto out_release_master; 30 | } 31 | 32 | for(auto & item : slaves_dict){ 33 | auto &drive = item.second; 34 | if (!(drive->sc_ = ecrt_master_slave_config(master, drive->get_alias(), drive->get_position(), drive->get_vendor_id(), drive->get_product_code()))) { 35 | printf("Could not get slave configuration for Drive at bus position = %u\n", (unsigned int)drive->get_position()); 36 | goto out_release_master; 37 | } 38 | /* Clear RxPdo */ 39 | ecrt_slave_config_sdo8( drive->sc_, 0x1C12, 0, 0 ); /* clear sm pdo 0x1c12 */ 40 | ecrt_slave_config_sdo8( drive->sc_, 0x1600, 0, 0 ); /* clear RxPdo 0x1600 */ 41 | ecrt_slave_config_sdo8( drive->sc_, 0x1601, 0, 0 ); /* clear RxPdo 0x1601 */ 42 | ecrt_slave_config_sdo8( drive->sc_, 0x1602, 0, 0 ); /* clear RxPdo 0x1602 */ 43 | ecrt_slave_config_sdo8( drive->sc_, 0x1603, 0, 0 ); /* clear RxPdo 0x1603 */ 44 | 45 | /* Define RxPdo */ 46 | ecrt_slave_config_sdo8( drive->sc_, 0x1600, 0, 1 ); /* set number of PDO entries for 0x1600 */ 47 | ecrt_slave_config_sdo32( drive->sc_, 0x1600, 1, 0x60400010 ); /* control word */ 48 | ecrt_slave_config_sdo8( drive->sc_, 0x1601, 0, 2 ); /* set number of PDO entries for 0x1601 */ 49 | ecrt_slave_config_sdo32( drive->sc_, 0x1601, 1, 0x607A0020 ); /* target position*/ 50 | ecrt_slave_config_sdo32( drive->sc_, 0x1601, 2, 0x60810020 ); /* profile_velocity */ 51 | 52 | /* clear TxPdo */ 53 | ecrt_slave_config_sdo8( drive->sc_, 0x1C13, 0, 0 ); /* clear sm pdo 0x1c13 */ 54 | ecrt_slave_config_sdo8( drive->sc_, 0x1A00, 0, 0 ); /* clear TxPdo 0x1A00 */ 55 | ecrt_slave_config_sdo8( drive->sc_, 0x1A01, 0, 0 ); /* clear TxPdo 0x1A01 */ 56 | ecrt_slave_config_sdo8( drive->sc_, 0x1A02, 0, 0 ); /* clear TxPdo 0x1A02 */ 57 | ecrt_slave_config_sdo8( drive->sc_, 0x1A03, 0, 0 ); /* clear TxPdo 0x1A03 */ 58 | /* Define TxPdo */ 59 | ecrt_slave_config_sdo32( drive->sc_, 0x1A00, 1, 0x60410010 ); /* 0x6041:0/16bits, status word */ 60 | ecrt_slave_config_sdo8( drive->sc_, 0x1A00, 0, 1 ); /* set number of PDO entries for 0x1A00 */ 61 | ecrt_slave_config_sdo32( drive->sc_, 0x1A01, 1, 0x606C0020 ); /* 0x606c:0/32bits, act velocity */ 62 | ecrt_slave_config_sdo32( drive->sc_, 0x1A01, 2, 0x60640020 ); /* 0x6063:0/32bits, act position */ 63 | ecrt_slave_config_sdo8( drive->sc_, 0x1A01, 0, 2 ); /* set number of PDO entries for 0x1A01 */ 64 | 65 | ecrt_slave_config_sdo8( drive->sc_, 0x6060, 0, 8 ); 66 | 67 | if (ecrt_slave_config_pdos(drive->sc_, EC_END, cdhd_syncs)) { 68 | printf("Could not configure PDOs for Drive at bus position = %u\n", (unsigned int)drive->get_position()); 69 | goto out_release_master; 70 | } 71 | } 72 | 73 | if (ecrt_domain_reg_pdo_entry_list(domain1, &(get_pdo_entry_regs_with_terminated()[0]))) { 74 | printf("Could not register PDO entries. \n"); 75 | goto out_release_master; 76 | } 77 | 78 | 79 | printf("Activating the master. \n"); 80 | 81 | if (ecrt_master_activate(master)) { 82 | printf("Could not activate the ethercat master. \n"); 83 | goto out_release_master; 84 | } 85 | 86 | domain1_pd = ecrt_domain_data(domain1); 87 | 88 | 89 | return 0; 90 | 91 | out_release_master: 92 | printf("releasing the master\n"); 93 | ecrt_release_master(master); 94 | out_return: 95 | printf("error occur!!!\n"); 96 | return 1; 97 | } 98 | 99 | std::vector EcatAdmin::get_pdo_entry_regs_with_terminated() 100 | { 101 | std::vector regs; 102 | 103 | for (auto & item : slaves_dict) { 104 | auto &drive = item.second; 105 | 106 | for (int i = 0; i < 6; ++i) { 107 | ec_pdo_entry_reg_t entry; 108 | entry.alias = drive->get_alias(); 109 | entry.position = drive->get_position(); 110 | entry.vendor_id = drive->get_vendor_id(); 111 | entry.product_code = drive->get_product_code(); 112 | 113 | entry.index = drive->pdo_index_[i]; 114 | entry.subindex = drive->pdo_subindex_[i]; 115 | entry.offset = &Motor::regs[drive->index_to_offset_list_[i]]; 116 | entry.bit_position = 0; 117 | regs.push_back(entry); 118 | } 119 | } 120 | 121 | ec_pdo_entry_reg_t terminator; 122 | regs.push_back(terminator); 123 | 124 | return (regs); 125 | } 126 | 127 | void EcatAdmin::add_motor(int joint_no, std::string joint_name, uint16_t alias, uint16_t position, uint32_t vendor_id, uint32_t product_code) 128 | { 129 | std::shared_ptr motor_ptr = std::make_shared(joint_no, joint_name, alias, position, vendor_id, product_code); 130 | 131 | slaves_dict[joint_no] = motor_ptr; 132 | } 133 | 134 | void EcatAdmin::shutdown() 135 | { 136 | ecrt_release_master(master); 137 | } 138 | 139 | void EcatAdmin::check_domain_state(void) 140 | { 141 | ec_domain_state_t ds = {}; 142 | 143 | ecrt_domain_state(domain1, &ds); 144 | 145 | if (ds.working_counter != domain1_state.working_counter) { 146 | printf("Domain1: WC %u.\n", ds.working_counter); 147 | } 148 | 149 | if (ds.wc_state != domain1_state.wc_state) { 150 | printf("Domain1: State %u.\n", ds.wc_state); 151 | } 152 | 153 | domain1_state = ds; 154 | } 155 | 156 | void EcatAdmin::check_master_state(void) 157 | { 158 | ec_master_state_t ms; 159 | 160 | ecrt_master_state(master, &ms); 161 | 162 | if (ms.slaves_responding != master_state.slaves_responding) { 163 | printf("%u slave(s).\n", ms.slaves_responding); 164 | } 165 | 166 | if (ms.al_states != master_state.al_states) { 167 | printf("AL states: 0x%02X.\n", ms.al_states); 168 | } 169 | 170 | if (ms.link_up != master_state.link_up) { 171 | printf("Link is %s.\n", ms.link_up ? "up" : "down"); 172 | } 173 | 174 | master_state = ms; 175 | } -------------------------------------------------------------------------------- /EcatAdmin.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by han on 16-12-27. 3 | // 4 | 5 | #ifndef ROS_ECATADMIN_H 6 | #define ROS_ECATADMIN_H 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include "Common.h" 13 | #include "motor.h" 14 | 15 | #include "slave_config.h" 16 | 17 | class EcatAdmin { 18 | public: 19 | using slaves_map = std::unordered_map>; 20 | 21 | 22 | static slaves_map slaves_dict; 23 | 24 | static ec_master_t *master; 25 | static ec_master_state_t master_state; 26 | 27 | static ec_domain_t *domain1; 28 | static ec_domain_state_t domain1_state; 29 | 30 | static uint8_t *domain1_pd; 31 | 32 | static int start_for_ros_control(); 33 | 34 | static void add_motor(int joint_no, std::string joint_name, uint16_t alias, uint16_t position, uint32_t vendor_id, uint32_t product_code); 35 | 36 | static void shutdown(); 37 | 38 | static void check_domain_state(void); 39 | 40 | static void check_master_state(void); 41 | 42 | private: 43 | static std::vector get_pdo_entry_regs_with_terminated(); 44 | 45 | }; 46 | 47 | 48 | #endif //ROS_ECATADMIN_H 49 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ros ethercat 2 | ubuntu 14.04 + ros indigo + igh 1.5.2 + r8169 3 | -------------------------------------------------------------------------------- /main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "signal.h" 3 | 4 | #include "EcatAdmin.h" 5 | 6 | static int run = 1; 7 | 8 | void signal_handler(int sig) 9 | { 10 | run = 0; 11 | } 12 | 13 | int main(void) { 14 | 15 | //加6个电机 16 | //使用EcatAdmin::add_motor() 17 | EcatAdmin::add_motor(1, "joint_1", 1, 0, 0x000002E1, 0x00000000); 18 | EcatAdmin::add_motor(2, "joint_2", 2, 0, 0x000002E1, 0x00000000); 19 | EcatAdmin::add_motor(3, "joint_3", 3, 0, 0x000002E1, 0x00000000); 20 | EcatAdmin::add_motor(4, "joint_4", 4, 0, 0x000002E1, 0x00000000); 21 | EcatAdmin::add_motor(5, "joint_5", 5, 0, 0x000002E1, 0x00000000); 22 | EcatAdmin::add_motor(6, "joint_6", 6, 0, 0x000002E1, 0x00000000); 23 | 24 | 25 | EcatAdmin::start_for_ros_control(); 26 | 27 | int cycle_counter = 0; 28 | 29 | while(run) 30 | { 31 | ecrt_master_receive(EcatAdmin::master); 32 | ecrt_domain_process(EcatAdmin::domain1); 33 | 34 | EcatAdmin::check_domain_state(); 35 | 36 | if (!(cycle_counter % 100)) { 37 | EcatAdmin::check_master_state(); 38 | } 39 | 40 | ecrt_domain_queue(EcatAdmin::domain1); 41 | ecrt_master_send(EcatAdmin::master); 42 | 43 | usleep(10000); 44 | } 45 | 46 | EcatAdmin::shutdown(); 47 | return 0; 48 | } -------------------------------------------------------------------------------- /motor.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by han on 16-12-26. 3 | // 4 | 5 | #include "motor.h" 6 | 7 | Motor::Motor(int joint_no, std::string joint_name, uint16_t alias, uint16_t position, uint32_t vendor_id, uint32_t product_code): 8 | joint_no_(joint_no), 9 | joint_name_(joint_name), 10 | alias_(alias), 11 | position_(position), 12 | vendor_id_(vendor_id), 13 | product_code_(product_code){ 14 | 15 | for(int i = 0; i != 6; ++i){ 16 | index_to_offset_list_.push_back(current_index_++); 17 | } 18 | 19 | pdo_index_ = {0x6040, 0x607A, 0x6081, 0x6041, 0x6064, 0x606C}; 20 | pdo_subindex_ = {0, 0, 0, 0, 0, 0}; 21 | 22 | } 23 | 24 | 25 | uint16_t Motor::get_alias() const noexcept{ 26 | return alias_; 27 | } 28 | 29 | uint16_t Motor::get_position() const noexcept{ 30 | return position_; 31 | } 32 | 33 | uint32_t Motor::get_vendor_id() const noexcept{ 34 | return vendor_id_; 35 | } 36 | 37 | uint32_t Motor::get_product_code() const noexcept{ 38 | return product_code_; 39 | } 40 | 41 | int Motor::current_index_ = 0; 42 | 43 | unsigned int Motor::regs[ECAT_REG_TABLE_SIZE] = {0}; 44 | 45 | int Motor::get_control_word() const noexcept{ 46 | return index_to_offset_list_[0]; 47 | } 48 | 49 | int Motor::get_r_position() const noexcept{ 50 | return index_to_offset_list_[1]; 51 | } 52 | 53 | int Motor::get_r_velocity() const noexcept{ 54 | return index_to_offset_list_[2]; 55 | } 56 | 57 | 58 | int Motor::get_status_word() const noexcept{ 59 | return index_to_offset_list_[3]; 60 | } 61 | 62 | int Motor::get_t_position() const noexcept{ 63 | return index_to_offset_list_[4]; 64 | } 65 | 66 | int Motor::get_t_velocity() const noexcept{ 67 | return index_to_offset_list_[5]; 68 | } 69 | 70 | void Motor::setting_trans(double offset, double param_1, double param_2, double param_3) 71 | { 72 | offset_ = offset; 73 | param_1_ = param_1; 74 | param_2_ = param_2; 75 | param_3_ = param_3; 76 | } 77 | 78 | int Motor::radian_to_pulse(double radian) 79 | { 80 | double tmp = (radian*param_1_*param_2_)/(2*PI*param_3_) + offset_; 81 | return static_cast(tmp); 82 | } 83 | 84 | double Motor::pulse_to_radian(int pulse) 85 | { 86 | return (pulse-offset_)*(2*PI*param_3_)/(param_1_*param_2_); 87 | } -------------------------------------------------------------------------------- /motor.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by han on 16-12-26. 3 | // 4 | 5 | #ifndef ROS_MOTOR_H 6 | #define ROS_MOTOR_H 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include "Common.h" 13 | 14 | 15 | // 16 | // radian * param_1 * param_2 17 | // pulse = ----------------------------- + offset 18 | // 2*PI*param_3 19 | // 20 | 21 | 22 | class Motor { 23 | public: 24 | Motor(int joint_no, std::string joint_name, uint16_t alias, uint16_t position, uint32_t vendor_id, uint32_t product_code); 25 | ~Motor() = default; 26 | Motor(const Motor&) = delete; 27 | 28 | uint16_t get_alias() const noexcept; 29 | uint16_t get_position() const noexcept; 30 | uint32_t get_vendor_id() const noexcept; 31 | uint32_t get_product_code() const noexcept; 32 | 33 | ec_slave_config_t *sc_; 34 | ec_slave_config_state_t slave_config_state_; 35 | ec_slave_config_state_t old_slave_config_state_; 36 | 37 | static unsigned int regs[ECAT_REG_TABLE_SIZE]; 38 | static int current_index_; 39 | 40 | std::vector index_to_offset_list_; 41 | std::vector pdo_index_; 42 | std::vector pdo_subindex_; 43 | 44 | int get_control_word() const noexcept; 45 | int get_r_position() const noexcept; 46 | int get_r_velocity() const noexcept; 47 | 48 | int get_status_word() const noexcept; 49 | int get_t_position() const noexcept; 50 | int get_t_velocity() const noexcept; 51 | 52 | void setting_trans(double offset, double param_1, double param_2, double param_3); 53 | 54 | int radian_to_pulse(double radian); 55 | 56 | double pulse_to_radian(int pulse); 57 | 58 | private: 59 | 60 | int joint_no_; 61 | std::string joint_name_; 62 | uint16_t alias_; 63 | uint16_t position_; 64 | uint32_t vendor_id_; 65 | uint32_t product_code_; 66 | 67 | double offset_; 68 | double param_1_; 69 | double param_2_; 70 | double param_3_; 71 | 72 | }; 73 | 74 | 75 | #endif //ROS_MOTOR_H 76 | -------------------------------------------------------------------------------- /slave_config.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by han on 16-12-27. 3 | // 4 | 5 | #ifndef ROS_SLAVE_CONFIG_H 6 | #define ROS_SLAVE_CONFIG_H 7 | 8 | #include 9 | 10 | static ec_pdo_entry_info_t cdhd_pdo_entries[] = { 11 | { 0x6040, 0x00, 16 }, 12 | { 0x607A, 0x00, 32 }, 13 | { 0x6081, 0x00, 32 }, 14 | { 0x6041, 0x00, 16 }, 15 | { 0x6064, 0x00, 32 }, 16 | { 0x606C, 0x00, 32 }, 17 | }; 18 | 19 | 20 | static ec_pdo_info_t cdhd_pdos[] = { 21 | { 0x1600, 1, cdhd_pdo_entries + 0 }, 22 | { 0x1601, 2, cdhd_pdo_entries + 1 }, 23 | { 0x1a00, 1, cdhd_pdo_entries + 3 }, 24 | { 0x1a01, 2, cdhd_pdo_entries + 4 }, 25 | }; 26 | 27 | static ec_sync_info_t cdhd_syncs[] = { 28 | { 0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE }, 29 | { 1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE }, 30 | { 2, EC_DIR_OUTPUT, 2, cdhd_pdos + 0, EC_WD_DISABLE }, 31 | { 3, EC_DIR_INPUT, 2, cdhd_pdos + 2, EC_WD_DISABLE }, 32 | { 0xFF } 33 | }; 34 | 35 | #endif //ROS_SLAVE_CONFIG_H 36 | --------------------------------------------------------------------------------