EtherCAT Control Framework v0.9
Implementation of EtherCAT protocol using IgH EtherCAT library for robot controller.
EthercatLifeCycleNode::EthercatLifeCycle Class Reference

Class for managing real-time EtherCAT communication. More...

#include <ecat_lifecycle.hpp>

Collaboration diagram for EthercatLifeCycleNode::EthercatLifeCycle:
[legend]

Public Member Functions

 EthercatLifeCycle ()
 
 ~EthercatLifeCycle ()
 
uint8_t on_configure ()
 Initialization function.Class initialized in unconfigured state. This function has to called to do inital configuration which will open EtherCAT master from shell, check connected slave, assign initial configuration parameter based on selected operation mode, map pdos, activate master and set thread priorities. More...
 
uint8_t on_activate ()
 Starts real-time Ethercat communication. More...
 
uint8_t on_deactivate ()
 Stops real-time communication. More...
 
uint8_t on_cleanup ()
 Cleans up all variables and datas assigned by Ethercat class. More...
 
uint8_t on_shutdown ()
 Shuts down EtherCAT lifecycle instance, releases Ethercat master. More...
 
uint8_t on_error ()
 There isn't any error recovery functionality for now, just resets nodes. Reconfiguration is needed for restarting communication. More...
 
int SetComThreadPriorities ()
 Sets Ethercat communication thread's properties After this function called user must call StartEthercatCommunication() function]. More...
 
int InitEthercatCommunication ()
 Encapsulates all configuration steps for the EtherCAT communication with default slaves. And waits for connected slaves to become operational. More...
 
int StartEthercatCommunication ()
 Starts EtherCAT communcation. More...
 
void StartPdoExchange (void *instance)
 Realtime cyclic Pdo exchange function which will constantly read/write values from/to slaves. More...
 
int GetComState ()
 Gets master's communication state. More...
 
void ReadFromSlaves ()
 Reads data from slaves and updates received data structure. More...
 
void WriteToSlavesVelocityMode ()
 Updates data that will be sent to slaves in velocity mode. More...
 
void WriteToSlavesInPositionMode ()
 Writes target position and control word to motor in profile position mode. More...
 
void UpdateVelocityModeParameters ()
 Acquired data from xbox controller will be assigned as motor speed parameter. More...
 
void UpdateCyclicVelocityModeParameters ()
 Acquired data from xbox controller will be assigned as motor speed parameter. More...
 
void UpdatePositionModeParameters ()
 Acquired data from xbox controller will be assigned as motor target position parameter. More...
 
void UpdateCyclicPositionModeParameters ()
 Acquired data from xbox controller will be assigned as motor cyclic target position parameter in configured interpolation time. More...
 
void UpdateMotorStateVelocityMode ()
 Updates motor control word and motor state in velocity mode based on CIA402. More...
 
void UpdateMotorStatePositionMode ()
 Updates motor control word and motor state in position mode based on CIA402 state machine,. More...
 
void UpdateCyclicTorqueModeParameters ()
 Updates cylic torque mode parameters based on controller inputs. More...
 
void WriteToSlavesInCyclicTorqueMode ()
 Writes target torque and control word in cyclic sync. torque mode. More...
 
int GetDriveState (const int &statusWord)
 CKim - This function checks status word and returns state of the motor driver. More...
 
int EnableDrivers ()
 CKim - This function checks status word, clears any faults and enables torque of the motor driver. More...
 
int SetConfigurationParameters ()
 Set the Configuration Parameters based on selected operation mode. More...
 

Static Public Member Functions

static void * PassCycylicExchange (void *arg)
 Helper function to enter pthread_create, since pthread's are C function it doesn't accept class member function, to pass class member function this helper function is created. More...
 

Public Attributes

DataReceived received_data_
 Structure to hold values received from connected slaves.
More...
 
DataSent sent_data_
 Structure to hold values that will be sent to the slaves. More...
 
std::unique_ptr< EthercatNodeecat_node_
 
pthread_t ethercat_thread_
 pthread create required parameters. More...
 
struct sched_param ethercat_sched_param_ = {}
 Scheduler parameter. More...
 
pthread_attr_t ethercat_thread_attr_
 Thread attribute parameter. More...
 
int32_t err_
 Error flag. More...
 
uint8_t al_state_ = 0
 Application layer of slaves seen by master.(INIT/PREOP/SAFEOP/OP) More...
 
uint32_t motor_state_ [g_kNumberOfServoDrivers]
 
uint32_t command_ = 0x004F
 
Controller controller_
 Structure for Xbox Controller values. More...
 
uint8_t emergency_status_ = 1
 
std::int32_t measurement_time = 0
 
Timing timer_info_
 Timing measurement information instance. More...
 

Detailed Description

Class for managing real-time EtherCAT communication.

Constructor & Destructor Documentation

◆ EthercatLifeCycle()

EthercatLifeCycle::EthercatLifeCycle ( )

◆ ~EthercatLifeCycle()

EthercatLifeCycle::~EthercatLifeCycle ( )

Member Function Documentation

◆ EnableDrivers()

int EthercatLifeCycle::EnableDrivers ( )

CKim - This function checks status word, clears any faults and enables torque of the motor driver.

◆ GetComState()

int EthercatLifeCycle::GetComState ( )

Gets master's communication state.

See also
ec_al_state_t
Returns
Application layer state for master.

◆ GetDriveState()

int EthercatLifeCycle::GetDriveState ( const int &  statusWord)

CKim - This function checks status word and returns state of the motor driver.

◆ InitEthercatCommunication()

int EthercatLifeCycle::InitEthercatCommunication ( )

Encapsulates all configuration steps for the EtherCAT communication with default slaves. And waits for connected slaves to become operational.

Returns
0 if succesful otherwise -1.

◆ on_activate()

uint8_t EthercatLifeCycle::on_activate ( )

Starts real-time Ethercat communication.

Returns
Success if activation succesfull,otherwise FAILURE

◆ on_cleanup()

uint8_t EthercatLifeCycle::on_cleanup ( )

Cleans up all variables and datas assigned by Ethercat class.

Returns
Success if cleanup succesfull,otherwise FAILURE

◆ on_configure()

uint8_t EthercatLifeCycle::on_configure ( )

Initialization function.Class initialized in unconfigured state. This function has to called to do inital configuration which will open EtherCAT master from shell, check connected slave, assign initial configuration parameter based on selected operation mode, map pdos, activate master and set thread priorities.

Note
This class implemented based on ROS2 lifecycle node state machine. For more information about Lifecycle node and it's interfaces check below link : https://design.ros2.org/articles/node_lifecycle.html
Returns
Success if configuration succesfull,otherwise FAILURE

◆ on_deactivate()

uint8_t EthercatLifeCycle::on_deactivate ( )

Stops real-time communication.

Returns
Success if deactivation succesfull,otherwise FAILURE

◆ on_error()

uint8_t EthercatLifeCycle::on_error ( )

There isn't any error recovery functionality for now, just resets nodes. Reconfiguration is needed for restarting communication.

Returns
Success

◆ on_shutdown()

uint8_t EthercatLifeCycle::on_shutdown ( )

Shuts down EtherCAT lifecycle instance, releases Ethercat master.

Returns
Success if shut down succesfull,otherwise FAILURE

◆ PassCycylicExchange()

void * EthercatLifeCycle::PassCycylicExchange ( void *  arg)
static

Helper function to enter pthread_create, since pthread's are C function it doesn't accept class member function, to pass class member function this helper function is created.

Parameters
argPointer to current class instance.
Returns
void*

◆ ReadFromSlaves()

void EthercatLifeCycle::ReadFromSlaves ( )

Reads data from slaves and updates received data structure.

◆ SetComThreadPriorities()

int EthercatLifeCycle::SetComThreadPriorities ( )

Sets Ethercat communication thread's properties After this function called user must call StartEthercatCommunication() function].

Returns
0 if succesfull, otherwise -1.

◆ SetConfigurationParameters()

int EthercatLifeCycle::SetConfigurationParameters ( )

Set the Configuration Parameters based on selected operation mode.

Returns
0 if succesfull oterwise -1.

◆ StartEthercatCommunication()

int EthercatLifeCycle::StartEthercatCommunication ( )

Starts EtherCAT communcation.

Returns
0 if succesfull, otherwise -1.

◆ StartPdoExchange()

void EthercatLifeCycle::StartPdoExchange ( void *  instance)

Realtime cyclic Pdo exchange function which will constantly read/write values from/to slaves.

Parameters
argUsed during pthread_create function to pass variables to realtime task.
Returns
NULL

◆ UpdateCyclicPositionModeParameters()

void EthercatLifeCycle::UpdateCyclicPositionModeParameters ( )

Acquired data from xbox controller will be assigned as motor cyclic target position parameter in configured interpolation time.

WRITE YOUR CUSTOM CONTROL ALGORITHM, VARIABLES DECLARATAION HERE, LIKE IN EXAMPLE BELOW.

◆ UpdateCyclicTorqueModeParameters()

void EthercatLifeCycle::UpdateCyclicTorqueModeParameters ( )

Updates cylic torque mode parameters based on controller inputs.

WRITE YOUR CUSTOM CONTROL ALGORITHM VARIABLES DECLARATAION HERE

WRITE YOUR CUSTOM CONTROL ALGORITHM HERE IF YOU WANT TO USE CYCLIC TORQUE MODE YOU CAN CHECK EXAMPLE CONTROL CODE BELOW.

◆ UpdateCyclicVelocityModeParameters()

void EthercatLifeCycle::UpdateCyclicVelocityModeParameters ( )

Acquired data from xbox controller will be assigned as motor speed parameter.

WRITE YOUR CUSTOM CONTROL ALGORITHM, VARIABLES DECLARATAION HERE, LIKE IN EXAMPLE BELOW.

◆ UpdateMotorStatePositionMode()

void EthercatLifeCycle::UpdateMotorStatePositionMode ( )

Updates motor control word and motor state in position mode based on CIA402 state machine,.

◆ UpdateMotorStateVelocityMode()

void EthercatLifeCycle::UpdateMotorStateVelocityMode ( )

Updates motor control word and motor state in velocity mode based on CIA402.

◆ UpdatePositionModeParameters()

void EthercatLifeCycle::UpdatePositionModeParameters ( )

Acquired data from xbox controller will be assigned as motor target position parameter.

WRITE YOUR CUSTOM CONTROL ALGORITHM, VARIABLES DECLARATAION HERE, LIKE IN EXAMPLE BELOW. KEEP IN MIND THAT YOU WILL HAVE TO WAIT FOR THE MOTION TO FINISH IN POSITION MODE, THEREFORE YOU HAVE TO CHECK 10th BIT OF STATUS WORD TO CHECK WHETHER TARGET IS REACHED OR NOT.

◆ UpdateVelocityModeParameters()

void EthercatLifeCycle::UpdateVelocityModeParameters ( )

Acquired data from xbox controller will be assigned as motor speed parameter.

WRITE YOUR CUSTOM CONTROL ALGORITHM VARIABLES DECLARATAION HERE

WRITE YOUR CUSTOM CONTROL ALGORITHM HERE IF YOU WANT TO USE VELOCITY MODE

YOU CAN CHECK EXAMPLE CONTROL CODE BELOW.

◆ WriteToSlavesInCyclicTorqueMode()

void EthercatLifeCycle::WriteToSlavesInCyclicTorqueMode ( )

Writes target torque and control word in cyclic sync. torque mode.

◆ WriteToSlavesInPositionMode()

void EthercatLifeCycle::WriteToSlavesInPositionMode ( )

Writes target position and control word to motor in profile position mode.

◆ WriteToSlavesVelocityMode()

void EthercatLifeCycle::WriteToSlavesVelocityMode ( )

Updates data that will be sent to slaves in velocity mode.

Member Data Documentation

◆ al_state_

uint8_t EthercatLifeCycleNode::EthercatLifeCycle::al_state_ = 0

Application layer of slaves seen by master.(INIT/PREOP/SAFEOP/OP)

◆ command_

uint32_t EthercatLifeCycleNode::EthercatLifeCycle::command_ = 0x004F

◆ controller_

Controller EthercatLifeCycleNode::EthercatLifeCycle::controller_

Structure for Xbox Controller values.

◆ ecat_node_

std::unique_ptr<EthercatNode> EthercatLifeCycleNode::EthercatLifeCycle::ecat_node_

◆ emergency_status_

uint8_t EthercatLifeCycleNode::EthercatLifeCycle::emergency_status_ = 1

◆ err_

int32_t EthercatLifeCycleNode::EthercatLifeCycle::err_

Error flag.

◆ ethercat_sched_param_

struct sched_param EthercatLifeCycleNode::EthercatLifeCycle::ethercat_sched_param_ = {}

Scheduler parameter.

◆ ethercat_thread_

pthread_t EthercatLifeCycleNode::EthercatLifeCycle::ethercat_thread_

pthread create required parameters.

◆ ethercat_thread_attr_

pthread_attr_t EthercatLifeCycleNode::EthercatLifeCycle::ethercat_thread_attr_

Thread attribute parameter.

◆ measurement_time

std::int32_t EthercatLifeCycleNode::EthercatLifeCycle::measurement_time = 0

◆ motor_state_

uint32_t EthercatLifeCycleNode::EthercatLifeCycle::motor_state_[g_kNumberOfServoDrivers]

◆ received_data_

DataReceived EthercatLifeCycleNode::EthercatLifeCycle::received_data_

Structure to hold values received from connected slaves.

◆ sent_data_

DataSent EthercatLifeCycleNode::EthercatLifeCycle::sent_data_

Structure to hold values that will be sent to the slaves.

◆ timer_info_

Timing EthercatLifeCycleNode::EthercatLifeCycle::timer_info_

Timing measurement information instance.


The documentation for this class was generated from the following files: