wg05.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/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   // As good a place as any for making sure that compiler actually packed these structures correctly
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   //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 
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   // Determine if device supports application RAM
00128   if (!retval)
00129   {
00130     if (use_ros_)
00131     {
00132       // WG005B has very poor motor voltage measurement, don't use meaurement for dectecting problems. 
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); // wait for ros to flush rosconsole output
00140         return -1;
00141       }
00142     }
00143 
00144   }// end if !retval
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 


ethercat_hardware
Author(s): Rob Wheeler (email: wheeler@willowgarage.com), Maintained by Derek King (email: dking@willowgarage.com)
autogenerated on Thu Apr 24 2014 15:43:45