wg05.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 #include <iomanip>
36 
37 #include <math.h>
38 #include <stddef.h>
39 
40 #include <ethercat_hardware/wg05.h>
41 
42 #include <dll/ethercat_dll.h>
43 #include <al/ethercat_AL.h>
44 #include <dll/ethercat_device_addressed_telegram.h>
45 #include <dll/ethercat_frame.h>
46 
47 #include <boost/static_assert.hpp>
48 
50 
51 
52 void WG05::construct(EtherCAT_SlaveHandler *sh, int &start_address)
53 {
54  WG0X::construct(sh, start_address);
55 
56  unsigned int base_status = sizeof(WG0XStatus);
57 
58  // As good a place as any for making sure that compiler actually packed these structures correctly
59  BOOST_STATIC_ASSERT(sizeof(WG0XStatus) == WG0XStatus::SIZE);
60 
61  command_size_ = sizeof(WG0XCommand);
62  status_size_ = sizeof(WG0XStatus);
63 
64 
65  EtherCAT_FMMU_Config *fmmu = new EtherCAT_FMMU_Config(2);
66  //ROS_DEBUG("device %d, command 0x%X = 0x10000+%d", (int)sh->get_ring_position(), start_address, start_address-0x10000);
67  (*fmmu)[0] = EC_FMMU(start_address, // Logical start address
68  command_size_,// Logical length
69  0x00, // Logical StartBit
70  0x07, // Logical EndBit
71  COMMAND_PHY_ADDR, // Physical Start address
72  0x00, // Physical StartBit
73  false, // Read Enable
74  true, // Write Enable
75  true); // Enable
76 
77  start_address += command_size_;
78 
79  //ROS_DEBUG("device %d, status 0x%X = 0x10000+%d", (int)sh->get_ring_position(), start_address, start_address-0x10000);
80  (*fmmu)[1] = EC_FMMU(start_address, // Logical start address
81  base_status, // Logical length
82  0x00, // Logical StartBit
83  0x07, // Logical EndBit
84  STATUS_PHY_ADDR, // Physical Start address
85  0x00, // Physical StartBit
86  true, // Read Enable
87  false, // Write Enable
88  true); // Enable
89 
90  start_address += base_status;
91 
92  sh->set_fmmu_config(fmmu);
93 
94  EtherCAT_PD_Config *pd = new EtherCAT_PD_Config(4);
95 
96  // Sync managers
97  (*pd)[0] = EC_SyncMan(COMMAND_PHY_ADDR, command_size_, EC_BUFFERED, EC_WRITTEN_FROM_MASTER);
98  (*pd)[0].ChannelEnable = true;
99  (*pd)[0].ALEventEnable = true;
100 
101  (*pd)[1] = EC_SyncMan(STATUS_PHY_ADDR, base_status);
102  (*pd)[1].ChannelEnable = true;
103 
104  (*pd)[2] = EC_SyncMan(WGMailbox::MBX_COMMAND_PHY_ADDR, WGMailbox::MBX_COMMAND_SIZE, EC_QUEUED, EC_WRITTEN_FROM_MASTER);
105  (*pd)[2].ChannelEnable = true;
106  (*pd)[2].ALEventEnable = true;
107 
108  (*pd)[3] = EC_SyncMan(WGMailbox::MBX_STATUS_PHY_ADDR, WGMailbox::MBX_STATUS_SIZE, EC_QUEUED);
109  (*pd)[3].ChannelEnable = true;
110 
111  sh->set_pd_config(pd);
112 }
113 
114 
116 {
117  if ((fw_major_ == 1) && (fw_minor_ >= 21))
118  {
120  }
121 
122  int retval = WG0X::initialize(hw, allow_unprogrammed);
123 
124  EthercatDirectCom com(EtherCAT_DataLinkLayer::instance());
125 
126  // Determine if device supports application RAM
127  if (!retval)
128  {
129  if (use_ros_)
130  {
131  // WG005B has very poor motor voltage measurement, don't use meaurement for dectecting problems.
132  bool poor_measured_motor_voltage = (board_major_ <= 2);
133  double max_pwm_ratio = double(0x3C00) / double(PWM_MAX);
134  double board_resistance = 0.8;
135  if (!WG0X::initializeMotorModel(hw, "WG005", max_pwm_ratio, board_resistance, poor_measured_motor_voltage))
136  {
137  ROS_FATAL("Initializing motor trace failed");
138  sleep(1); // wait for ros to flush rosconsole output
139  return -1;
140  }
141  }
142 
143  }// end if !retval
144  return retval;
145 }
146 
147 void WG05::packCommand(unsigned char *buffer, bool halt, bool reset)
148 {
149  WG0X::packCommand(buffer, halt, reset);
150 }
151 
152 bool WG05::unpackState(unsigned char *this_buffer, unsigned char *prev_buffer)
153 {
154  bool rv = true;
155 
156  unsigned char* this_status = this_buffer + command_size_;
157  if (!verifyChecksum(this_status, status_size_))
158  {
159  status_checksum_error_ = true;
160  rv = false;
161  goto end;
162  }
163 
164  if (!WG0X::unpackState(this_buffer, prev_buffer))
165  {
166  rv = false;
167  }
168 
169  end:
170  return rv;
171 }
172 
173 
WG0X::verifyChecksum
bool verifyChecksum(const void *buffer, unsigned size)
Definition: wg0x.cpp:665
WG05::initialize
int initialize(pr2_hardware_interface::HardwareInterface *, bool allow_unprogrammed=true)
Definition: wg05.cpp:115
WG0X::unpackState
bool unpackState(unsigned char *this_buffer, unsigned char *prev_buffer)
Definition: wg0x.cpp:614
WG0XStatus
Definition: wg0x.h:160
WG05::construct
void construct(EtherCAT_SlaveHandler *sh, int &start_address)
< Construct EtherCAT device
Definition: wg05.cpp:52
EthercatDevice::use_ros_
bool use_ros_
Definition: ethercat_device.h:215
WG0X::STATUS_PHY_ADDR
static const unsigned STATUS_PHY_ADDR
Definition: wg0x.h:357
EthercatDirectCom
Definition: ethercat_com.h:57
wg05.h
WG05::unpackState
bool unpackState(unsigned char *this_buffer, unsigned char *prev_buffer)
Definition: wg05.cpp:152
pr2_hardware_interface::HardwareInterface
WG0X::status_checksum_error_
bool status_checksum_error_
Definition: wg0x.h:298
WG0XCommand
Definition: wg0x.h:187
PLUGINLIB_EXPORT_CLASS
PLUGINLIB_EXPORT_CLASS(WG05, EthercatDevice)
WG0X::app_ram_status_
AppRamStatus app_ram_status_
Definition: wg0x.h:324
WG0X::PWM_MAX
static const int PWM_MAX
Definition: wg0x.h:347
EthercatDevice
Definition: ethercat_device.h:133
WG0X::fw_major_
uint8_t fw_major_
Definition: wg0x.h:258
ROS_FATAL
#define ROS_FATAL(...)
WG05
EtherCAT driver for WG005 motor controller.
Definition: wg05.h:43
EthercatDevice::status_size_
unsigned int status_size_
Definition: ethercat_device.h:219
WG0X::construct
void construct(EtherCAT_SlaveHandler *sh, int &start_address)
< Construct EtherCAT device
Definition: wg0x.cpp:198
WG0X::board_major_
uint8_t board_major_
Printed circuit board revision (for this value 0=='A', 1=='B')
Definition: wg0x.h:260
WG0X::APP_RAM_PRESENT
@ APP_RAM_PRESENT
Definition: wg0x.h:323
WG0X::initialize
virtual int initialize(pr2_hardware_interface::HardwareInterface *, bool allow_unprogrammed=true)
Definition: wg0x.cpp:395
EthercatDevice::command_size_
unsigned int command_size_
Definition: ethercat_device.h:218
WG0X::initializeMotorModel
bool initializeMotorModel(pr2_hardware_interface::HardwareInterface *hw, const string &device_description, double max_pwm_ratio, double board_resistance, bool poor_measured_motor_voltage)
Allocates and initialized motor trace for WG0X devices than use it (WG006, WG005)
Definition: wg0x.cpp:238
WG0X::packCommand
void packCommand(unsigned char *buffer, bool halt, bool reset)
Definition: wg0x.cpp:563
WG0X::COMMAND_PHY_ADDR
static const unsigned COMMAND_PHY_ADDR
Definition: wg0x.h:356
WG0X::fw_minor_
uint8_t fw_minor_
Definition: wg0x.h:259
WG05::packCommand
void packCommand(unsigned char *buffer, bool halt, bool reset)
Definition: wg05.cpp:147
WG0XStatus::SIZE
static const unsigned SIZE
Definition: wg0x.h:183


ethercat_hardware
Author(s): Rob Wheeler , Derek King
autogenerated on Thu Sep 26 2024 02:44:04