#include <wg06.h>

Inheritance diagram for WG06:

Public Types
enum	{ PRODUCT_CODE = 6805006 }
Public Member Functions
void	construct (EtherCAT_SlaveHandler *sh, int &start_address)
	< Construct EtherCAT device
int	initialize (pr2_hardware_interface::HardwareInterface *, bool allow_unprogrammed=true)
virtual void	multiDiagnostics (vector< diagnostic_msgs::DiagnosticStatus > &vec, unsigned char *buffer)
	For EtherCAT devices that publish more than one EtherCAT Status message. If sub-class implements multiDiagnostics() then diagnostics() is not used.
void	packCommand (unsigned char *buffer, bool halt, bool reset)
bool	unpackState (unsigned char this_buffer, unsigned char prev_buffer)
	WG06 ()
	~WG06 ()
Private Member Functions
void	convertFTDataSampleToWrench (const FTDataSample &sample, geometry_msgs::Wrench &wrench)
	Convert FTDataSample to Wrench using gain, offset, and coefficient matrix.
void	diagnosticsAccel (diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *buffer)
void	diagnosticsFT (diagnostic_updater::DiagnosticStatusWrapper &d, WG06StatusWithAccelAndFT *status)
void	diagnosticsPressure (diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *buffer)
void	diagnosticsWG06 (diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *)
bool	initializeAccel (pr2_hardware_interface::HardwareInterface *hw)
bool	initializeFT (pr2_hardware_interface::HardwareInterface *hw)
bool	initializePressure (pr2_hardware_interface::HardwareInterface *hw)
bool	initializeSoftProcessor ()
bool	unpackAccel (WG06StatusWithAccel status, WG06StatusWithAccel last_status)
	Unpack 3-axis accelerometer samples from realtime data.
bool	unpackFT (WG06StatusWithAccelAndFT status, WG06StatusWithAccelAndFT last_status)
	Unpack force/torque ADC samples from realtime data.
bool	unpackPressure (unsigned char *pressure_buf)
	Unpack pressure sensor samples from realtime data.
Private Attributes
realtime_tools::RealtimePublisher < pr2_msgs::AccelerometerState > *	accel_publisher_
pr2_hardware_interface::Accelerometer	accelerometer_
unsigned	accelerometer_missed_samples_
	Total of accelerometer samples that were missed.
unsigned	accelerometer_samples_
	Number of accelerometer samples since last publish cycle.
uint64_t	diag_last_ft_sample_count_
	F/T Sample count last time diagnostics was published.
bool	enable_ft_sensor_
bool	enable_pressure_sensor_
bool	enable_soft_processor_access_
bool	first_publish_
pr2_hardware_interface::ForceTorque	force_torque_
	Provides F/T data to controllers.
pr2_hardware_interface::AnalogIn	ft_analog_in_
	Provides F/T data to controllers (deprecated, use pr2_hardware_interface::ForceTorque instead)
bool	ft_disconnected_
	f/t sensor may be disconnected
uint64_t	ft_missed_samples_
	Counts number of ft sensor samples that were missed.
uint8_t	ft_overload_flags_
	Bits 0-5 set to true if raw FT input goes beyond limit.
int	ft_overload_limit_
	Limit on raw range of F/T input.
FTParamsInternal	ft_params_
realtime_tools::RealtimePublisher < geometry_msgs::WrenchStamped > *	ft_publisher_
pr2_hardware_interface::AnalogIn	ft_raw_analog_in_
uint64_t	ft_sample_count_
	Counts number of ft sensor samples.
bool	ft_sampling_rate_error_
	True if FT sampling rate was incorrect.
bool	ft_vhalf_error_
	error with Vhalf reference voltage
bool	has_accel_and_ft_
	True if device has accelerometer and force/torque sensor.
uint32_t	last_pressure_time_
ros::Time	last_publish_time_
	Time diagnostics was last published.
bool	pressure_checksum_error_
	Set true where checksum error on pressure data is detected, cleared on reset.
unsigned	pressure_checksum_error_count_
	debugging
realtime_tools::RealtimePublisher < pr2_msgs::PressureState > *	pressure_publisher_
pr2_hardware_interface::PressureSensor	pressure_sensors_ [2]
unsigned	pressure_size_
	Size in bytes of pressure data region.
realtime_tools::RealtimePublisher < ethercat_hardware::RawFTData > *	raw_ft_publisher_
	Realtime Publisher of RAW F/T data.
WGSoftProcessor	soft_processor_
Static Private Attributes
static const unsigned	BIG_PRESSURE_PHY_ADDR = 0x2600
static const int	FT_VHALF_IDEAL = 32768
	Vhalf ADC measurement is ideally about (1<<16)/2.
static const int	FT_VHALF_RANGE = 300
	allow vhalf to range +/- 300 from ideal
static const unsigned	MAX_FT_SAMPLES = 4
static const unsigned	NUM_FT_CHANNELS = 6
static const unsigned	NUM_PRESSURE_REGIONS = 22
static const unsigned	PRESSURE_PHY_ADDR = 0x2200

Detailed Description

Definition at line 163 of file wg06.h.

Member Enumeration Documentation

anonymous enum

Enumerator:

PRODUCT_CODE

Definition at line 174 of file wg06.h.

Constructor & Destructor Documentation

WG06::WG06 ( )

Definition at line 54 of file wg06.cpp.

WG06::~WG06 ( )

Definition at line 82 of file wg06.cpp.

Member Function Documentation

void WG06::construct	(	EtherCAT_SlaveHandler *	sh,
		int &	start_address
	)		`[virtual]`

< Construct EtherCAT device

Construct non-EtherCAT device

Reimplemented from WG0X.

Definition at line 88 of file wg06.cpp.

void WG06::convertFTDataSampleToWrench	(	const FTDataSample &	sample,
		geometry_msgs::Wrench &	wrench
	)		`[private]`

Convert FTDataSample to Wrench using gain, offset, and coefficient matrix.

perform offset and gains multiplication on raw data and then multiply by calibration matrix to get force and torque values. The calibration matrix is based on "raw" deltaR/R values from strain gauges

Force/Torque = Coeff * ADCVoltage

Coeff = RawCoeff / ( ExcitationVoltage * AmplifierGain ) = RawCoeff / ( 2.5V * AmplifierGain )

ADCVoltage = Vref / 2^16 = 2.5 / 2^16

Force/Torque = RawCalibrationCoeff / ( ExcitationVoltage * AmplifierGain ) * (ADCValues * 2.5V/2^16) = (RawCalibration * ADCValues) / (AmplifierGain * 2^16)

Note on hardware circuit and Vref and excitation voltage should have save value. Thus, with Force/Torque calculation they cancel out.

Definition at line 635 of file wg06.cpp.

void WG06::diagnosticsAccel	(	diagnostic_updater::DiagnosticStatusWrapper &	d,
		unsigned char *	buffer
	)		`[private]`

Definition at line 816 of file wg06.cpp.

void WG06::diagnosticsFT	(	diagnostic_updater::DiagnosticStatusWrapper &	d,
		WG06StatusWithAccelAndFT *	status
	)		`[private]`

Definition at line 987 of file wg06.cpp.

void WG06::diagnosticsPressure	(	diagnostic_updater::DiagnosticStatusWrapper &	d,
		unsigned char *	buffer
	)		`[private]`

Definition at line 876 of file wg06.cpp.

void WG06::diagnosticsWG06	(	diagnostic_updater::DiagnosticStatusWrapper &	d,
		unsigned char *	buffer
	)		`[private]`

Definition at line 870 of file wg06.cpp.

int WG06::initialize	(	pr2_hardware_interface::HardwareInterface *	hw,
		bool	allow_unprogrammed = `true`
	)		`[virtual]`

Reimplemented from WG0X.

Definition at line 201 of file wg06.cpp.

bool WG06::initializeAccel ( pr2_hardware_interface::HardwareInterface * hw ) [private]

Definition at line 322 of file wg06.cpp.

bool WG06::initializeFT ( pr2_hardware_interface::HardwareInterface * hw ) [private]

Definition at line 342 of file wg06.cpp.

bool WG06::initializePressure ( pr2_hardware_interface::HardwareInterface * hw ) [private]

Definition at line 298 of file wg06.cpp.

bool WG06::initializeSoftProcessor ( ) [private]

Definition at line 399 of file wg06.cpp.

void WG06::multiDiagnostics	(	vector< diagnostic_msgs::DiagnosticStatus > &	vec,
		unsigned char *	buffer
	)		`[virtual]`

For EtherCAT devices that publish more than one EtherCAT Status message. If sub-class implements multiDiagnostics() then diagnostics() is not used.

Parameters:

vec	Vector of diagnostics status messages. Slave appends one or more new diagnostic status'.
buffer	Pointer to slave process data.

Reimplemented from EthercatDevice.

Definition at line 793 of file wg06.cpp.

void WG06::packCommand	(	unsigned char *	buffer,
		bool	halt,
		bool	reset
	)		`[virtual]`

Parameters:

reset	when asserted this will clear diagnostic error conditions device safety disable
halt	while asserted will disable actuator, usually by disabling H-bridge

Reimplemented from WG0X.

Definition at line 420 of file wg06.cpp.

bool WG06::unpackAccel	(	WG06StatusWithAccel *	status,
		WG06StatusWithAccel *	last_status
	)		`[private]`

Unpack 3-axis accelerometer samples from realtime data.

Returns:: True, if there are no problems.

Definition at line 576 of file wg06.cpp.

bool WG06::unpackFT	(	WG06StatusWithAccelAndFT *	status,
		WG06StatusWithAccelAndFT *	last_status
	)		`[private]`

Unpack force/torque ADC samples from realtime data.

Returns:: True, if there are no problems, false if there is something wrong with the data.

Definition at line 696 of file wg06.cpp.

bool WG06::unpackPressure ( unsigned char * pressure_buf ) [private]

Unpack pressure sensor samples from realtime data.

Returns:: True, if there are no problems, false if there is something wrong with the data.

Definition at line 511 of file wg06.cpp.

bool WG06::unpackState	(	unsigned char *	this_buffer,
		unsigned char *	prev_buffer
	)		`[virtual]`

Reimplemented from WG0X.

Definition at line 450 of file wg06.cpp.

Member Data Documentation

realtime_tools::RealtimePublisher<pr2_msgs::AccelerometerState>* WG06::accel_publisher_ [private]

Definition at line 215 of file wg06.h.

pr2_hardware_interface::Accelerometer WG06::accelerometer_ [private]

Definition at line 184 of file wg06.h.

unsigned WG06::accelerometer_missed_samples_ [private]

Total of accelerometer samples that were missed.

Definition at line 208 of file wg06.h.

unsigned WG06::accelerometer_samples_ [private]

Number of accelerometer samples since last publish cycle.

Definition at line 207 of file wg06.h.

const unsigned WG06::BIG_PRESSURE_PHY_ADDR = 0x2600 [static, private]

Definition at line 181 of file wg06.h.

uint64_t WG06::diag_last_ft_sample_count_ [private]

F/T Sample count last time diagnostics was published.

Definition at line 229 of file wg06.h.

bool WG06::enable_ft_sensor_ [private]

Definition at line 243 of file wg06.h.

bool WG06::enable_pressure_sensor_ [private]

Definition at line 242 of file wg06.h.

bool WG06::enable_soft_processor_access_ [private]

Certain version of WG06 firmware (2.xx and 3.xx) use soft-processors to communicate with certain peripherals (pressure sensor, F/T sensor, accelerometer...) For purposes of supporting new types of devices, the soft-processor FW can be be modified through use of service calls.

Definition at line 250 of file wg06.h.

bool WG06::first_publish_ [private]

Definition at line 210 of file wg06.h.

pr2_hardware_interface::ForceTorque WG06::force_torque_ [private]

Provides F/T data to controllers.

Definition at line 234 of file wg06.h.

pr2_hardware_interface::AnalogIn WG06::ft_analog_in_ [private]

Provides F/T data to controllers (deprecated, use pr2_hardware_interface::ForceTorque instead)

Definition at line 232 of file wg06.h.

bool WG06::ft_disconnected_ [private]

f/t sensor may be disconnected

Definition at line 224 of file wg06.h.