EtherCAT Slave
1.04.01
CiA 402 Directive Example
Author KUNBUS GmbH
Date 2020-06-19
Copyright Copyright (c) 2020, KUNBUS GmbH
All rights reserved.
Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "ESL_cia402Demo.h"
#include "ESL_cia402Obd.h"
typedef struct
{
uint8_t id;
bool axisIsActive;
bool brakeApplied;
bool lowLevelPowerApplied;
bool highLevelPowerApplied;
bool axisFunctionEnabled;
bool configurationAllowed;
double positionActualValue;
uint32_t cycleTime;
}
#define SUPPORTED_DRIVE_MODE_CSP_BIT 7
#define SUPPORTED_DRIVE_MODE_CSV_BIT 8
#define SUPPORTED_DRIVE_MODE_CST_BIT 9
#define DRIVE_MODE_CSP (1 << SUPPORTED_DRIVE_MODE_CSP_BIT)
#define DRIVE_MODE_CSV (1 << SUPPORTED_DRIVE_MODE_CSV_BIT)
#define DRIVE_MODE_CST (1 << SUPPORTED_DRIVE_MODE_CST_BIT)
#define DRIVE_GEAR_RELATION 0.0010922
#define POSITION_MAX_LIMIT 0xFFFFFFFF
#define NON_DC_DEFAULT_CYCLE_TIME_USEC 4000
#define NSEC_TO_USEC 1000
#define ESC_DC_SYNC0_CYCLETIME_REG 0x09A0
{
uint32_t err;
{
}
return err;
}
{
uint32_t err;
{
}
return err;
}
{
uint32_t err;
{
}
return err;
}
{
uint32_t err;
{
}
return err;
}
{
uint32_t err;
uint32_t driveMode = DRIVE_MODE_CSP | DRIVE_MODE_CSV | DRIVE_MODE_CST;
for (uint8_t axisNo = 0; axisNo < AXES_NUMBER; axisNo++)
{
}
return err;
}
{
int32_t posMaxLimit = POSITION_MAX_LIMIT;
for (uint8_t axisNo = 0; axisNo < AXES_NUMBER; axisNo++)
{
}
}
{
OSALUNREF_PARM(pIntMask_p);
sync0CycleTime = sync0CycleTime / NSEC_TO_USEC;
for (uint8_t axisNo = 0; axisNo < AXES_NUMBER; axisNo++)
{
localAxes_s[axisNo].
id = axisNo;
if (localAxes_s[axisNo].axisIsActive)
{
localAxes_s[axisNo].
cycleTime = sync0CycleTime;
}
if (!localAxes_s[axisNo].cycleTime)
{
localAxes_s[axisNo].
cycleTime = NON_DC_DEFAULT_CYCLE_TIME_USEC;
}
}
}
{
switch (characteristic_p)
{
break ;
break ;
break ;
break ;
default :
break ;
}
return true ;
}
{
int32_t targetTorque;
}
{
int32_t targetVelocity;
}
{
uint32_t actualPosition, actualVelocity;
uint32_t targetPosition;
int32_t targetVelocity;
int16_t actualTorque, targetTorque;
float incFactor = (float) (DRIVE_GEAR_RELATION * pCiA402Axis_p->
cycleTime );
if (incFactor != 0)
{
actualVelocity = (targetPosition - actualPosition) / incFactor;
}
}
{
uint32_t statusWord, controlWord, targetPosition;
uint32_t posMaxLimit, posMinLimit;
uint16_t operationModeDisplay;
{
(posMinLimit < pCiA402Axis_p->positionActualValue
{
switch (operationModeDisplay)
{
break ;
break ;
break ;
default :
break ;
}
}
else
{
}
}
}
{
uint16_t controlWord, statusWord, errorCode;
int16_t operationDisplayCode, quickStopOptionCode, shutdownOptionCode, disableOperationCode, faultReactionCode;
for (uint8_t axisNo = 0; axisNo < AXES_NUMBER; axisNo++)
{
{
OSAL_printf("Axis %d Activated\n\r" , axisNo);
}
if (errorCode &&
{
}
else
{
}
switch (errorCode)
{
{
{
{
}
{
}
{
}
{
}
}
{
{
}
}
}
break ;
{
{
}
}
break ;
{
{
}
}
break ;
{
{
}
}
break ;
default :
break ;
}
}
}
{
OSALUNREF_PARM(pCtxt_p);
OSAL_printf("Local error triggered: %d\r\n" , errorCode_p);
}
uint32_t EC_API_SLV_CoE_setObjectData(EC_API_SLV_SHandle_t *pEcSlaveApi_p, EC_API_SLV_SCoE_Object_t *pObject_p, uint32_t length_p, uint16_t *pData_p)
This function writes Data to the Object Dictionary.
Definition: ecSlvApi_CoEStub.c:188
uint32_t cycleTime
Motion controller cycletime in us.
Definition: ESL_cia402Demo.c:74
#define SLOW_DOWN_RAMP
Slow down ramp (options: 0x605B; 0x605C; 0x605E)
Definition: ecApiDef.h:515
uint8_t id
Axis Identification.
Definition: ESL_cia402Demo.c:47
#define DISABLE_DRIVE
Disable drive (options: 0x605B; 0x605C; 0x605E)
Definition: ecApiDef.h:514
#define CONTROLWORD_COMMAND_SWITCHON
Switch on command.
Definition: ecApiDef.h:269
#define OBD_MODES_OF_OPERATION_DISPLAY_INDEX(x)
Definition: ecApiDef.h:550
#define OBD_MODES_OF_OPERATION_INDEX(x)
Definition: ecApiDef.h:549
uint32_t EC_API_SLV_readDoubleWordEscRegister(EC_API_SLV_SHandle_t *pEcSlaveApi_p, uint16_t escAddress_p)
This is the function reads a byte from ESC Register.
Definition: ecSlvApiStub.c:1190
#define OBD_TARGET_POSITION_INDEX(x)
Definition: ecApiDef.h:573
uint32_t EC_SLV_APP_getCiA402ObjectEntryValue(EC_API_SLV_SHandle_t *pEcApiSlv_p, uint16_t index_p, uint8_t subIndex_p, uint16_t length_p, uint16_t *pValue_p)
Read CiA402 Object entry.
Definition: ESL_cia402Demo.c:219
uint32_t EC_SLV_APP_getCiA402ObjectValue(EC_API_SLV_SHandle_t *pEcApiSlv_p, uint16_t index_p, uint16_t length_p, uint16_t *pValue_p)
Read CiA402 Objects.
Definition: ESL_cia402Demo.c:118
#define OBD_SHUTDOWN_INDEX(x)
Definition: ecApiDef.h:544
#define OBD_STATUSWORD_INDEX(x)
Definition: ecApiDef.h:541
Data structure to handle an CiA 402 axis.
Definition: ESL_cia402Demo.c:65
#define CONTROLWORD_COMMAND_ENABLEOPERATION
Enable operation command.
Definition: ecApiDef.h:274
bool EC_SLV_APP_transitionAction(int16_t characteristic_p)
This function shall calculate the desired Axis input values to move on a predefined ramp.
Definition: ESL_cia402Demo.c:454
#define QUICKSTOP_RAMP_NO_TRANSIT
Slow down on quick stop ramp and stay in Quick Stop Active.
Definition: ecApiDef.h:527
enum EC_API_SLV_EUserRetCodeserRetCodes EC_API_SLV_EUserRetCodes_t
EC_RETCODE_T Error codes used during EtherCAT State Machine transitions.
#define VOLTAGE_LIMIT_NO_TRANSIT
Slow down on voltage limit and stay in Quick Stop Active.
Definition: ecApiDef.h:529
uint32_t EC_API_SLV_CoE_getObjectData(EC_API_SLV_SHandle_t *pEcSlaveApi_p, EC_API_SLV_SCoE_Object_t *pObject_p, uint32_t length_p, uint16_t *pData_p)
This function read the object data from the Object Dictionary.
Definition: ecSlvApi_CoEStub.c:138
uint32_t EC_SLV_APP_setSupportedDriveModes(EC_API_SLV_SHandle_t *pEcApiSlv_p)
Set supported drive modes.
Definition: ESL_cia402Demo.c:313
struct EC_API_SLV_SCoE_Object EC_API_SLV_SCoE_Object_t
TSdo describes an Object Dictionary Object.
Definition: ecSlvApi.h:131
#define STOP_ON_VOLTAGE_LIMIT
Stop on voltage limit (options: 0x605E)
Definition: ecApiDef.h:518
#define STATUSWORD_INTERNAL_LIMIT
Internal limit.
Definition: ecApiDef.h:285
struct EC_API_SLV_SHandle EC_API_SLV_SHandle_t
EC_API_SLV_SHandle_t describes the EtherCAT Slave API.
Definition: ecSlvApi.h:134
uint32_t EC_SLV_APP_setCiA402ObjectEntryValue(EC_API_SLV_SHandle_t *pEcApiSlv_p, uint16_t index_p, uint8_t subIndex_p, uint16_t length_p, uint16_t *pValue_p)
Write CiA402 Object entry.
Definition: ESL_cia402Demo.c:270
uint32_t EC_API_SLV_CoE_getObject(EC_API_SLV_SHandle_t *pEcSlaveApi_p, uint16_t index_p, EC_API_SLV_SCoE_Object_t **pObject_p)
This function returns an object of the Object Dictionary.
Definition: ecSlvApi_CoEStub.c:87
#define OBD_FAULT_REACTION_INDEX(x)
Definition: ecApiDef.h:547
uint32_t EC_API_SLV_CoE_getObjectEntry(EC_API_SLV_SHandle_t *pEcSlaveApi_p, uint16_t index_p, uint8_t subIndex_p, EC_API_SLV_SCoE_ObjEntry_t **pObjectEntry_p)
This function returns object entries from the Object Dictionary.
Definition: ecSlvApi_CoEStub.c:325
#define OBD_DISABLE_OPERATION_INDEX(x)
Definition: ecApiDef.h:545
void EC_SLV_APP_CSV(EC_API_SLV_SHandle_t *pEcApiSlv_p, EC_SLV_API_CiA402_SAxis_t *pCiA402Axis_p)
Cyclic synchronous velocity mode. ETG6010 6.3.
Definition: ESL_cia402Demo.c:546
uint32_t EC_API_SLV_CoE_getObjectEntryData(EC_API_SLV_SHandle_t *pEcSlaveApi_p, EC_API_SLV_SCoE_ObjEntry_t *pObjectEntry_p, uint32_t length_p, uint16_t *pData_p)
This function reads Data from the Object Dictionary.
Definition: ecSlvApi_CoEStub.c:376
#define CURRENT_LIMIT_NO_TRANSIT
Slow down on current limit and stay in Quick Stop Active.
Definition: ecApiDef.h:528
#define OBD_CONTROLWORD_INDEX(x)
Definition: ecApiDef.h:540
#define OBD_POSITION_ACTUAL_VALUE_INDEX(x)
Definition: ecApiDef.h:554
#define STATUSWORD_TARGET_REACHED
Target reached.
Definition: ecApiDef.h:287
uint16_t EC_API_SLV_CiA402_SM_getErrorCode(EC_API_SLV_SHandle_t *pEcSlaveApi_p, uint8_t axisNo_p)
Get Local Error value.
Definition: ecSlvApi_CiA402Stub.c:443
void EC_SLV_APP_CSP(EC_API_SLV_SHandle_t *pEcApiSlv_p, EC_SLV_API_CiA402_SAxis_t *pCiA402Axis_p)
Cyclic synchronous position mode. ETG6010 6.2.
Definition: ESL_cia402Demo.c:585
#define CYCLIC_SYNC_TORQUE_MODE
Cyclic Synchronous Torque mode.
Definition: ecApiDef.h:334
#define SLOWDOWN_RAMP_NO_TRANSIT
Slow down on slow down ramp and stay in Quick Stop Active.
Definition: ecApiDef.h:526
#define CYCLIC_SYNC_POSITION_MODE
Cyclic Synchronous Position mode.
Definition: ecApiDef.h:332
uint32_t EC_API_SLV_CoE_setObjectEntryData(EC_API_SLV_SHandle_t *pEcSlaveApi_p, EC_API_SLV_SCoE_ObjEntry_t *pObjectEntry_p, uint32_t length_p, uint16_t *pData_p)
This function writes Data to the Object Dictionary.
Definition: ecSlvApi_CoEStub.c:426
struct EC_API_SLV_SCoE_ObjEntry EC_API_SLV_SCoE_ObjEntry_t
TSdoEntry describes an OBD Object Entry.
Definition: ecSlvApi.h:128
#define OBD_TORQUE_ACTUAL_VALUE_INDEX(x)
Definition: ecApiDef.h:571
#define OBD_TARGET_TORQUE_INDEX(x)
Definition: ecApiDef.h:568
#define OBD_SUPPORTED_DRIVE_MODES_INDEX(x)
Definition: ecApiDef.h:624
#define STATUSWORD_DRIVE_FOLLOWS_COMMAND
Drive follows command (used in cyclic synchronous modes)
Definition: ecApiDef.h:289
double positionActualValue
Actual position within control loop.
Definition: ESL_cia402Demo.c:73
#define CYCLIC_SYNC_VELOCITY_MODE
Cyclic Synchronous Velocity mode.
Definition: ecApiDef.h:333
#define QUICKSTOP_RAMP
Quick stop ramp (options: 0x605E)
Definition: ecApiDef.h:516
#define STOP_ON_CURRENT_LIMIT
Stop on current limit (options: 0x605E)
Definition: ecApiDef.h:517
void EC_SLV_APP_setObdValues(void *pCtxt_p)
Set default values for CiA 402 object dictionary.
Definition: ESL_cia402Demo.c:356
#define OBD_SW_POSITION_LIMIT_INDEX(x)
Definition: ecApiDef.h:577
#define OBD_TARGET_VELOCITY_INDEX(x)
Definition: ecApiDef.h:622
#define OBD_QUICKSTOP_INDEX(x)
Definition: ecApiDef.h:543
void EC_SLV_APP_motionControl(EC_API_SLV_SHandle_t *pEcApiSlv_p, EC_SLV_API_CiA402_SAxis_t *pCiA402Axis_p)
This functions provides a simple feedback functionality.
Definition: ESL_cia402Demo.c:647
void EC_SLV_APP_CST(EC_API_SLV_SHandle_t *pEcApiSlv_p, EC_SLV_API_CiA402_SAxis_t *pCiA402Axis_p)
Cyclic synchronous torque mode. ETG6010 6.4.
Definition: ESL_cia402Demo.c:507
@ EC_USR_eRET_OK
no error occurred
Definition: ecSlvApi.h:78
#define OBD_VELOCITY_ACTUAL_VALUE_INDEX(x)
Definition: ecApiDef.h:562
void EC_SLV_APP_cia402Application(void *pCtxt_p)
CiA402 Application function.
Definition: ESL_cia402Demo.c:745
void EC_API_SLV_CiA402_SM_clearErrorCode(EC_API_SLV_SHandle_t *pEcSlaveApi_p, uint8_t axisNo_p)
Inform the CiA402 state machine whether a state change finished or not.
Definition: ecSlvApi_CiA402Stub.c:402
void EC_API_SLV_CiA402_activateAxis(EC_API_SLV_SHandle_t *pEcSlaveApi_p, uint8_t axisNo_p, bool active_p)
Activate Axis. This function is required to inform the CiA402 state machine that the axis is switched...
Definition: ecSlvApi_CiA402Stub.c:356
EC_API_SLV_EUserRetCodes_t EC_SLV_APP_startInputHandler(void *pCtxt_p, uint16_t *pIntMask_p)
Get cycle time information.
Definition: ESL_cia402Demo.c:401
@ EC_API_eERR_NONE
Definition: ecApiError.h:66
uint32_t EC_SLV_APP_setCiA402ObjectValue(EC_API_SLV_SHandle_t *pEcApiSlv_p, uint16_t index_p, uint16_t length_p, uint16_t *pValue_p)
Write CiA402 Object value.
Definition: ESL_cia402Demo.c:167
void EC_SLV_APP_cia402LocalError(void *pCtxt_p, uint16_t errorCode_p)
Local Error function handler.
Definition: ESL_cia402Demo.c:901