Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #include <iomanip>
00036
00037 #include <math.h>
00038 #include <stddef.h>
00039
00040 #include <ethercat_hardware/wg05.h>
00041
00042 #include <dll/ethercat_dll.h>
00043 #include <al/ethercat_AL.h>
00044 #include <dll/ethercat_device_addressed_telegram.h>
00045 #include <dll/ethercat_frame.h>
00046
00047 #include <boost/crc.hpp>
00048 #include <boost/static_assert.hpp>
00049
00050 PLUGINLIB_DECLARE_CLASS(ethercat_hardware, 6805005, WG05, EthercatDevice);
00051
00052
00053 void WG05::construct(EtherCAT_SlaveHandler *sh, int &start_address)
00054 {
00055 WG0X::construct(sh, start_address);
00056
00057 unsigned int base_status = sizeof(WG0XStatus);
00058
00059
00060 BOOST_STATIC_ASSERT(sizeof(WG0XStatus) == WG0XStatus::SIZE);
00061
00062 command_size_ = sizeof(WG0XCommand);
00063 status_size_ = sizeof(WG0XStatus);
00064
00065
00066 EtherCAT_FMMU_Config *fmmu = new EtherCAT_FMMU_Config(2);
00067
00068 (*fmmu)[0] = EC_FMMU(start_address,
00069 command_size_,
00070 0x00,
00071 0x07,
00072 COMMAND_PHY_ADDR,
00073 0x00,
00074 false,
00075 true,
00076 true);
00077
00078 start_address += command_size_;
00079
00080
00081 (*fmmu)[1] = EC_FMMU(start_address,
00082 base_status,
00083 0x00,
00084 0x07,
00085 STATUS_PHY_ADDR,
00086 0x00,
00087 true,
00088 false,
00089 true);
00090
00091 start_address += base_status;
00092
00093 sh->set_fmmu_config(fmmu);
00094
00095 EtherCAT_PD_Config *pd = new EtherCAT_PD_Config(4);
00096
00097
00098 (*pd)[0] = EC_SyncMan(COMMAND_PHY_ADDR, command_size_, EC_BUFFERED, EC_WRITTEN_FROM_MASTER);
00099 (*pd)[0].ChannelEnable = true;
00100 (*pd)[0].ALEventEnable = true;
00101
00102 (*pd)[1] = EC_SyncMan(STATUS_PHY_ADDR, base_status);
00103 (*pd)[1].ChannelEnable = true;
00104
00105 (*pd)[2] = EC_SyncMan(WGMailbox::MBX_COMMAND_PHY_ADDR, WGMailbox::MBX_COMMAND_SIZE, EC_QUEUED, EC_WRITTEN_FROM_MASTER);
00106 (*pd)[2].ChannelEnable = true;
00107 (*pd)[2].ALEventEnable = true;
00108
00109 (*pd)[3] = EC_SyncMan(WGMailbox::MBX_STATUS_PHY_ADDR, WGMailbox::MBX_STATUS_SIZE, EC_QUEUED);
00110 (*pd)[3].ChannelEnable = true;
00111
00112 sh->set_pd_config(pd);
00113 }
00114
00115
00116 int WG05::initialize(pr2_hardware_interface::HardwareInterface *hw, bool allow_unprogrammed)
00117 {
00118 if ((fw_major_ == 1) && (fw_minor_ >= 21))
00119 {
00120 app_ram_status_ = APP_RAM_PRESENT;
00121 }
00122
00123 int retval = WG0X::initialize(hw, allow_unprogrammed);
00124
00125 EthercatDirectCom com(EtherCAT_DataLinkLayer::instance());
00126
00127
00128 if (!retval)
00129 {
00130 if (use_ros_)
00131 {
00132
00133 bool poor_measured_motor_voltage = (board_major_ <= 2);
00134 double max_pwm_ratio = double(0x3C00) / double(PWM_MAX);
00135 double board_resistance = 0.8;
00136 if (!WG0X::initializeMotorModel(hw, "WG005", max_pwm_ratio, board_resistance, poor_measured_motor_voltage))
00137 {
00138 ROS_FATAL("Initializing motor trace failed");
00139 sleep(1);
00140 return -1;
00141 }
00142 }
00143
00144 }
00145 return retval;
00146 }
00147
00148 void WG05::packCommand(unsigned char *buffer, bool halt, bool reset)
00149 {
00150 WG0X::packCommand(buffer, halt, reset);
00151 }
00152
00153 bool WG05::unpackState(unsigned char *this_buffer, unsigned char *prev_buffer)
00154 {
00155 bool rv = true;
00156
00157 unsigned char* this_status = this_buffer + command_size_;
00158 if (!verifyChecksum(this_status, status_size_))
00159 {
00160 status_checksum_error_ = true;
00161 rv = false;
00162 goto end;
00163 }
00164
00165 if (!WG0X::unpackState(this_buffer, prev_buffer))
00166 {
00167 rv = false;
00168 }
00169
00170 end:
00171 return rv;
00172 }
00173
00174