wg021.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 #include <iomanip>
00036 
00037 #include <math.h>
00038 #include <stddef.h>
00039 
00040 #include <ethercat_hardware/wg021.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 #include "ethercat_hardware/wg_util.h"
00051 
00052 PLUGINLIB_EXPORT_CLASS(WG021, EthercatDevice);
00053 
00054 void WG021::construct(EtherCAT_SlaveHandler *sh, int &start_address)
00055 {
00056   WG0X::construct(sh, start_address);
00057 
00058   unsigned int base_status = sizeof(WG0XStatus);
00059 
00060   // As good a place as any for making sure that compiler actually packed these structures correctly
00061   BOOST_STATIC_ASSERT(sizeof(WG021Status) == WG021Status::SIZE);
00062 
00063   status_size_ = base_status = sizeof(WG021Status);
00064   command_size_ = sizeof(WG021Command);
00065 
00066   EtherCAT_FMMU_Config *fmmu = new EtherCAT_FMMU_Config(2);
00067   //ROS_DEBUG("device %d, command  0x%X = 0x10000+%d", (int)sh->get_ring_position(), start_address, start_address-0x10000);
00068   (*fmmu)[0] = EC_FMMU(start_address, // Logical start address
00069                        command_size_,// Logical length
00070                        0x00, // Logical StartBit
00071                        0x07, // Logical EndBit
00072                        COMMAND_PHY_ADDR, // Physical Start address
00073                        0x00, // Physical StartBit
00074                        false, // Read Enable
00075                        true, // Write Enable
00076                        true); // Enable
00077 
00078   start_address += command_size_;
00079 
00080   //ROS_DEBUG("device %d, status   0x%X = 0x10000+%d", (int)sh->get_ring_position(), start_address, start_address-0x10000);
00081   (*fmmu)[1] = EC_FMMU(start_address, // Logical start address
00082                        base_status, // Logical length
00083                        0x00, // Logical StartBit
00084                        0x07, // Logical EndBit
00085                        STATUS_PHY_ADDR, // Physical Start address
00086                        0x00, // Physical StartBit
00087                        true, // Read Enable
00088                        false, // Write Enable
00089                        true); // Enable
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   // Sync managers
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 int WG021::initialize(pr2_hardware_interface::HardwareInterface *hw, bool allow_unprogrammed)
00116 {
00117   // WG021 has no use for application ram
00118   app_ram_status_ = APP_RAM_NOT_APPLICABLE;
00119 
00120   int retval = WG0X::initialize(hw, allow_unprogrammed);
00121 
00122   // Register digital outs with pr2_hardware_interface::HardwareInterface
00123   struct {
00124     pr2_hardware_interface::DigitalOut *d;
00125     string name;
00126   } digital_outs[] = {
00127     {&digital_out_A_, "_digital_out_A"},
00128     {&digital_out_B_, "_digital_out_B"},
00129     {&digital_out_I_, "_digital_out_I"},
00130     {&digital_out_M_, "_digital_out_M"},
00131     {&digital_out_L0_, "_digital_out_L0"},
00132     {&digital_out_L1_, "_digital_out_L1"},
00133   };
00134 
00135   for (size_t i = 0; i < sizeof(digital_outs)/sizeof(digital_outs[0]); ++i)
00136   {
00137     digital_outs[i].d->name_ = string(actuator_info_.name_) + digital_outs[i].name;
00138     if (hw && !hw->addDigitalOut(digital_outs[i].d))
00139     {
00140         ROS_FATAL("A digital out of the name '%s' already exists.  Device #%02d has a duplicate name", digital_outs[i].d->name_.c_str(), sh_->get_ring_position());
00141         return -1;
00142     }
00143   }
00144 
00145   // Register projector with pr2_hardware_interface::HardwareInterface
00146   {
00147     projector_.name_ = actuator_info_.name_;
00148     if (hw && !hw->addProjector(&projector_))
00149     {
00150         ROS_FATAL("A projector of the name '%s' already exists.  Device #%02d has a duplicate name", projector_.name_.c_str(), sh_->get_ring_position());
00151         return -1;
00152     }
00153     projector_.command_.enable_ = true;
00154     projector_.command_.current_ = 0;
00155   }
00156 
00157   return retval;
00158 }
00159 
00160 void WG021::packCommand(unsigned char *buffer, bool halt, bool reset)
00161 {
00162   pr2_hardware_interface::ProjectorCommand &cmd = projector_.command_;
00163 
00164   // Override enable if motors are halted  
00165   if (reset) 
00166   {
00167     clearErrorFlags();
00168   }
00169   resetting_ = reset;
00170 
00171   // Truncate the current to limit (do not allow negative current)
00172   projector_.state_.last_commanded_current_ = cmd.current_;
00173   cmd.current_ = max(min(cmd.current_, max_current_), 0.0);
00174 
00175   // Pack command structures into EtherCAT buffer
00176   WG021Command *c = (WG021Command *)buffer;
00177   memset(c, 0, command_size_);
00178   c->digital_out_ = digital_out_.command_.data_;
00179   c->programmed_current_ = int(cmd.current_ / config_info_.nominal_current_scale_);
00180   c->mode_ = (cmd.enable_ && !halt && !has_error_) ? (MODE_ENABLE | MODE_CURRENT) : MODE_OFF;
00181   c->mode_ |= reset ? MODE_SAFETY_RESET : 0;
00182   c->config0_ = ((cmd.A_ & 0xf) << 4) | ((cmd.B_ & 0xf) << 0);
00183   c->config1_ = ((cmd.I_ & 0xf) << 4) | ((cmd.M_ & 0xf) << 0);
00184   c->config2_ = ((cmd.L1_ & 0xf) << 4) | ((cmd.L0_ & 0xf) << 0);
00185   c->general_config_ = cmd.pulse_replicator_ == true;
00186   c->checksum_ = wg_util::rotateRight8(wg_util::computeChecksum(c, command_size_ - 1));
00187 }
00188 
00189 bool WG021::unpackState(unsigned char *this_buffer, unsigned char *prev_buffer)
00190 {
00191   bool rv = true;
00192 
00193   pr2_hardware_interface::ProjectorState &state = projector_.state_;
00194   WG021Status *this_status;
00195   //WG021Status *prev_status;
00196   this_status = (WG021Status *)(this_buffer + command_size_);
00197   //prev_status = (WG021Status *)(prev_buffer + command_size_);
00198   
00199   if (!verifyChecksum(this_status, status_size_))
00200   {
00201     status_checksum_error_ = true;
00202     rv = false;
00203     goto end;
00204   }
00205 
00206   digital_out_.state_.data_ = this_status->digital_out_;
00207 
00208   state.timestamp_us_ = this_status->timestamp_;
00209   state.falling_timestamp_us_ = this_status->output_stop_timestamp_;
00210   state.rising_timestamp_us_ = this_status->output_start_timestamp_;
00211 
00212   state.output_ = (this_status->output_status_ & 0x1) == 0x1;
00213   state.falling_timestamp_valid_ = (this_status->output_status_ & 0x8) == 0x8;
00214   state.rising_timestamp_valid_ = (this_status->output_status_ & 0x4) == 0x4;
00215 
00216   state.A_ = ((this_status->config0_ >> 4) & 0xf);
00217   state.B_ = ((this_status->config0_ >> 0) & 0xf);
00218   state.I_ = ((this_status->config1_ >> 4) & 0xf);
00219   state.M_ = ((this_status->config1_ >> 0) & 0xf);
00220   state.L1_ = ((this_status->config2_ >> 4) & 0xf);
00221   state.L0_ = ((this_status->config2_ >> 0) & 0xf);
00222   state.pulse_replicator_ = (this_status->general_config_ & 0x1) == 0x1;
00223 
00224   state.last_executed_current_ = this_status->programmed_current_ * config_info_.nominal_current_scale_;
00225   state.last_measured_current_ = this_status->measured_current_ * config_info_.nominal_current_scale_;
00226 
00227   state.max_current_ = max_current_;
00228 
00229   max_board_temperature_ = max(max_board_temperature_, this_status->board_temperature_);
00230   max_bridge_temperature_ = max(max_bridge_temperature_, this_status->bridge_temperature_);
00231 
00232   if (!verifyState((WG0XStatus *)(this_buffer + command_size_), (WG0XStatus *)(prev_buffer + command_size_)))
00233   {
00234     rv = false;
00235   }
00236 
00237 end:
00238   return rv;
00239 }
00240 
00241 
00242 
00243 void WG021::diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *buffer)
00244 {
00245   WG021Status *status = (WG021Status *)(buffer + command_size_);
00246 
00247   stringstream str;
00248   str << "EtherCAT Device (" << actuator_info_.name_ << ")";
00249   d.name = str.str();
00250   char serial[32];
00251   snprintf(serial, sizeof(serial), "%d-%05d-%05d", config_info_.product_id_ / 100000 , config_info_.product_id_ % 100000, config_info_.device_serial_number_);
00252   d.hardware_id = serial;
00253 
00254   d.summary(d.OK, "OK");
00255 
00256   d.clear();
00257   d.add("Configuration", config_info_.configuration_status_ ? "good" : "error loading configuration");
00258   d.add("Name", actuator_info_.name_);
00259   d.addf("Position", "%02d", sh_->get_ring_position());
00260   d.addf("Product code",
00261         "WG021 (%d) Firmware Revision %d.%02d, PCB Revision %c.%02d",
00262         sh_->get_product_code(), fw_major_, fw_minor_,
00263         'A' + board_major_, board_minor_);
00264 
00265   d.add("Robot", actuator_info_.robot_name_);
00266   d.add("Serial Number", serial);
00267   d.addf("Nominal Current Scale", "%f",  config_info_.nominal_current_scale_);
00268   d.addf("Nominal Voltage Scale",  "%f", config_info_.nominal_voltage_scale_);
00269   d.addf("HW Max Current", "%f", config_info_.absolute_current_limit_ * config_info_.nominal_current_scale_);
00270   d.addf("SW Max Current", "%f", actuator_info_.max_current_);
00271 
00272   publishGeneralDiagnostics(d);
00273   mailbox_.publishMailboxDiagnostics(d);
00274 
00275   d.add("Mode", modeString(status->mode_));
00276   d.addf("Digital out", "%d", status->digital_out_);
00277   d.addf("Programmed current", "%f", status->programmed_current_ * config_info_.nominal_current_scale_);
00278   d.addf("Measured current", "%f", status->measured_current_ * config_info_.nominal_current_scale_);
00279   d.addf("Timestamp", "%u", status->timestamp_);
00280   d.addf("Config 0", "%#02x", status->config0_);
00281   d.addf("Config 1", "%#02x", status->config1_);
00282   d.addf("Config 2", "%#02x", status->config2_);
00283   d.addf("Output Status", "%#02x", status->output_status_);
00284   d.addf("Output Start Timestamp", "%u", status->output_start_timestamp_);
00285   d.addf("Output Stop Timestamp", "%u", status->output_stop_timestamp_);
00286   d.addf("Board temperature", "%f", 0.0078125 * status->board_temperature_);
00287   d.addf("Max board temperature", "%f", 0.0078125 * max_board_temperature_);
00288   d.addf("Bridge temperature", "%f", 0.0078125 * status->bridge_temperature_);
00289   d.addf("Max bridge temperature", "%f", 0.0078125 * max_bridge_temperature_);
00290   d.addf("Supply voltage", "%f", status->supply_voltage_ * config_info_.nominal_voltage_scale_);
00291   d.addf("LED voltage", "%f", status->led_voltage_ * config_info_.nominal_voltage_scale_);
00292   d.addf("Packet count", "%d", status->packet_count_);
00293 
00294   EthercatDevice::ethercatDiagnostics(d, 2); 
00295 }


ethercat_hardware
Author(s): Rob Wheeler , Derek King
autogenerated on Thu Jun 6 2019 19:46:32