CiA 402 Directive Example.
#include <osal.h>
#include <ESL_os.h>
#include <ESL_BOARD_OS_config.h>
#include "ecSlvCiA402.h"
#include "ESL_cia402Demo.h"
#include "ESL_cia402Obd.h"
#include "ESL_gpioHelper.h"
#if !(defined BIT2BYTE)
#define BIT2BYTE(x) (((x)+7) >> 3)
#endif
#define ENABLE_DYNAMIC_POSITION_LIMITS 0
#define EC_SLV_APP_CIA_GETAXISVALUE(gav_type, gav_target, gav_axisDesc) \
{ if (gotInOffset && (NULL != (gav_axisDesc).pdoObject)) { \
(gav_target) = ((gav_type*)&(pApplication_p->pdRxBuffer[(gav_axisDesc).pdoOffset]))[0]; } else { \
(void)EC_SLV_APP_getCiA402ObjectValue(pApplication_p, (gav_axisDesc).pSdo, sizeof(gav_type), (uint16_t*)&(gav_target)); } }
#define EC_SLV_APP_CIA_SETAXISVALUE(sav_type, sav_axisDesc, sav_value) \
{ if (gotOutOffset && (NULL != (sav_axisDesc).pdoObject)) { \
((sav_type*)&(pApplication_p->pdTxBuffer[(sav_axisDesc).pdoOffset]))[0] = (sav_value); } else { \
(void)EC_SLV_APP_setCiA402ObjectValue(pApplication_p, &(sav_axisDesc), sizeof(sav_type), (uint16_t*)&(sav_value)); } }
{
#define SUPPORTED_DRIVE_MODE_CSP_BIT (7u)
#define SUPPORTED_DRIVE_MODE_CSV_BIT (8u)
#define SUPPORTED_DRIVE_MODE_CST_BIT (9u)
#define DRIVE_MODE_CSP ((uint32_t)(1u << SUPPORTED_DRIVE_MODE_CSP_BIT))
#define DRIVE_MODE_CSV ((uint32_t)(1u << SUPPORTED_DRIVE_MODE_CSV_BIT))
#define DRIVE_MODE_CST ((uint32_t)(1u << SUPPORTED_DRIVE_MODE_CST_BIT))
#define DRIVE_GEAR_RELATION 0.0010922
#define POSITION_MAX_LIMIT (0xFFFFFFFFu)
#define NON_DC_DEFAULT_CYCLE_TIME_USEC (4000u)
#define NSEC_TO_USEC (1000u)
#define ESC_DC_SYNC0_CYCLETIME_REG (0x09A0u)
{
if((NULL == pApplication_p) || (NULL == pObject_p))
{
goto Exit;
}
err = EC_API_SLV_CoE_getObjectData(pEcApiSlv, pObject_p, length_p, pValue_p);
Exit:
return err;
}
{
uint32_t err;
if (NULL != pCiaObject_p->
pSdo)
{
pObject = pCiaObject_p->
pSdo;
}
else
{
if (0u != err)
{
goto Exit;
}
}
err = EC_API_SLV_CoE_setObjectData(pApplication_p->
ptEcSlvApi, pObject, 0, length_p, pValue_p);
Exit:
return err;
}
{
if (!pApplication || !pObjectEntry_p)
{
goto Exit;
}
error = EC_API_SLV_CoE_getObjectEntryData(pEcSlvApi, pObjectEntry_p, length_p, pValue_p);
Exit:
return error;
}
static 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)
{
uint32_t err;
err = EC_API_SLV_CoE_getObjectEntry(pEcApiSlv_p,index_p, subIndex_p, &pObjEntry);
{
err = EC_API_SLV_CoE_setObjectEntryData(pEcApiSlv_p, pObjEntry, length_p, pValue_p);
}
return err;
}
{
uint32_t err;
uint32_t driveMode = (uint32_t)(DRIVE_MODE_CSP | DRIVE_MODE_CSV | DRIVE_MODE_CST);
uint8_t axisNo;
for(axisNo = 0u; axisNo < AXES_NUMBER; axisNo++)
{
err = EC_SLV_APP_setCiA402ObjectValue(pApplication_p,
sizeof (driveMode),
(uint16_t*) &driveMode);
}
return err;
}
{
int32_t posMaxLimit = POSITION_MAX_LIMIT;
uint8_t axisNo;
OSAL_printf("+%s\r\n", __func__);
(void)EC_SLV_APP_setSupportedDriveModes(pApplicationInstance);
for(axisNo = 0u; axisNo < AXES_NUMBER; axisNo++)
{
(void)EC_SLV_APP_setCiA402ObjectEntryValue(
2,
sizeof(posMaxLimit),
(uint16_t*) &posMaxLimit);
}
}
{
uint32_t sync0CycleTime = 0;
uint8_t axisNo;
OSALUNREF_PARM(intMask);
if (NULL == pApplicationInstance)
{
goto Exit;
}
EC_API_SLV_readDoubleWordEscRegister(pEcApiSlv, ESC_DC_SYNC0_CYCLETIME_REG, &sync0CycleTime);
sync0CycleTime = sync0CycleTime / NSEC_TO_USEC;
for(axisNo = 0u; 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;
}
OSALUNREF_PARM(localAxes_s[axisNo].brakeApplied);
OSALUNREF_PARM(localAxes_s[axisNo].lowLevelPowerApplied);
OSALUNREF_PARM(localAxes_s[axisNo].highLevelPowerApplied);
OSALUNREF_PARM(localAxes_s[axisNo].axisFunctionEnabled);
OSALUNREF_PARM(localAxes_s[axisNo].configurationAllowed);
}
Exit:
return retVal;
}
static bool EC_SLV_APP_transitionAction(int16_t characteristic_p)
{
switch(characteristic_p)
{
break;
break;
break;
break;
default:
break;
}
return true;
}
static void EC_SLV_APP_CST(
bool gotInOffset,
bool gotOutOffset)
{
int16_t targetTorque;
}
static void EC_SLV_APP_CSV(
bool gotInOffset,
bool gotOutOffset)
{
int32_t targetVelocity;
}
static void EC_SLV_APP_CSP(
bool gotInOffset,
bool gotOutOffset)
{
uint32_t targetPosition = 0;
uint32_t actualPosition = 0;
int32_t targetVelocity = 0;
int32_t actualVelocity = 0;
int16_t targetTorque = 0;
int16_t actualTorque = 0;
float incFactor = (float) (DRIVE_GEAR_RELATION * pCiA402Axis_p->
cycleTime);
OSALUNREF_PARM(targetVelocity);
OSALUNREF_PARM(targetTorque);
OSALUNREF_PARM(actualTorque);
if(0.0 != incFactor)
{
actualVelocity = (int32_t)(((targetPosition - actualPosition) * 1.0) / incFactor);
}
}
static void EC_SLV_APP_motionControl(
bool gotInOffset,
bool gotOutOffset)
{
uint16_t statusWord = 0;
uint16_t controlWord = 0;
uint32_t targetPosition = 0;
uint32_t posMaxLimit = 0;
uint32_t posMinLimit = 0;
uint8_t operationModeDisplay = 0;
if (!pApplication_p)
{
goto Exit;
}
#if (defined ENABLE_DYNAMIC_POSITION_LIMITS) && (1==ENABLE_DYNAMIC_POSITION_LIMITS)
#endif
{
&&((posMinLimit <= pCiA402Axis_p->positionActualValue) || (pCiA402Axis_p->
positionActualValue <= targetPosition)))
{
statusWord &= ~STATUSWORD_INTERNAL_LIMIT;
switch(operationModeDisplay)
{
EC_SLV_APP_CSP(pApplication_p, pCiA402Axis_p, gotInOffset, gotOutOffset);
break;
EC_SLV_APP_CSV(pApplication_p, pCiA402Axis_p, gotInOffset, gotOutOffset);
break;
EC_SLV_APP_CST(pApplication_p, pCiA402Axis_p, gotInOffset, gotOutOffset);
break;
default:
break;
}
}
else
{
}
}
Exit:
return;
}
{
static bool gotInOffset = false;
static bool gotOutOffset = false;
uint8_t axisNo;
uint16_t controlWord = 0;
uint16_t statusWord = 0;
uint16_t errorCode = 0;
int16_t quickStopOptionCode = 0;
int16_t shutdownOptionCode = 0;
int16_t disableOperationCode = 0;
int16_t faultReactionCode = 0;
uint8_t operationDisplayCode = 0;
uint32_t apiError;
if (!pApplication_p)
{
goto Exit;
}
#if (defined GPIO_TEST_PINS) && (1==GPIO_TEST_PINS)
#if (defined GPIO_TEST_PROFILE_SEL) && (defined GPIO_TEST_PROFILE_1) && (GPIO_TEST_PROFILE_1 == GPIO_TEST_PROFILE_SEL)
ESL_GPIO_testPins_set(ESL_TESTPIN_STATE_REG_BANK, ESL_TESTPIN_2_MASK);
#endif
#endif
EC_API_SLV_getState(pEcApiSlv, &curState, &alErrorCode);
curState &= ~EC_API_SLV_eESM_errState;
{
(void)EC_SLV_APP_CiA_dropPDOffsets(pApplication_p);
gotInOffset = gotOutOffset = false;
}
{
(void)EC_SLV_APP_CiA_fetchPDOffsets(pApplication_p);
OSAL_printf(
"PDO size In:0x%x/0x%x\r\n",
);
gotInOffset = true;
}
{
(void)EC_SLV_APP_CiA_fetchPDOffsets(pApplication_p);
OSAL_printf(
"PDO size Out:0x%x/0x%x, In:0x%x/0x%x\r\n",
);
gotOutOffset = true;
}
#if (defined GPIO_TEST_PINS) && (1==GPIO_TEST_PINS)
#if (defined GPIO_TEST_PROFILE_SEL) && (defined GPIO_TEST_PROFILE_1) && (GPIO_TEST_PROFILE_1 == GPIO_TEST_PROFILE_SEL)
ESL_GPIO_testPins_clear(ESL_TESTPIN_STATE_REG_BANK, ESL_TESTPIN_2_MASK);
#endif
#endif
#if (defined GPIO_TEST_PINS) && (1==GPIO_TEST_PINS)
#if (defined GPIO_TEST_PROFILE_SEL) && (defined GPIO_TEST_PROFILE_1) && (GPIO_TEST_PROFILE_1 == GPIO_TEST_PROFILE_SEL)
ESL_GPIO_testPins_set(ESL_TESTPIN_STATE_REG_BANK, ESL_TESTPIN_2_MASK);
#endif
#endif
{
apiError = EC_API_SLV_preSeqInputPDBuffer(
);
{
goto Exit;
}
}
{
apiError = EC_API_SLV_preSeqOutputPDBuffer(
);
{
goto Exit;
}
}
for(axisNo = 0; axisNo < AXES_NUMBER; axisNo++)
{
{
OSAL_printf("Axis %d Activated\n\r", axisNo);
EC_API_SLV_CiA402_activateAxis(pEcApiSlv, axisNo, true);
}
EC_API_SLV_CiA402_SM_getErrorCode(pEcApiSlv, axisNo, &errorCode);
if(errorCode &&
{
statusWord &= ~ STATUSWORD_DRIVE_FOLLOWS_COMMAND;
}
else
{
}
switch(errorCode)
{
{
{
{
}
{
}
{
}
{
}
}
if(EC_SLV_APP_transitionAction(driveRamp))
{
EC_API_SLV_CiA402_SM_clearErrorCode(pEcApiSlv, axisNo);
{
}
}
}
break;
{
if(EC_SLV_APP_transitionAction(shutdownOptionCode))
{
EC_API_SLV_CiA402_SM_clearErrorCode(pEcApiSlv, axisNo);
}
}
break;
{
if(EC_SLV_APP_transitionAction(disableOperationCode))
{
EC_API_SLV_CiA402_SM_clearErrorCode(pEcApiSlv, axisNo);
}
}
break;
{
if(EC_SLV_APP_transitionAction(faultReactionCode))
{
EC_API_SLV_CiA402_SM_clearErrorCode(pEcApiSlv, axisNo);
}
}
break;
default:
break;
}
#if (defined GPIO_TEST_PINS) && (1==GPIO_TEST_PINS)
#if (defined GPIO_TEST_PROFILE_SEL) && (defined GPIO_TEST_PROFILE_1) && (GPIO_TEST_PROFILE_1 == GPIO_TEST_PROFILE_SEL)
ESL_GPIO_testPins_set(ESL_TESTPIN_STATE_REG_BANK, ESL_TESTPIN_1_MASK);
#endif
#endif
EC_SLV_APP_motionControl(pApplication_p, &localAxes_s[axisNo], gotInOffset, gotOutOffset);
#if (defined GPIO_TEST_PINS) && (1==GPIO_TEST_PINS)
#if (defined GPIO_TEST_PROFILE_SEL) && (defined GPIO_TEST_PROFILE_1) && (GPIO_TEST_PROFILE_1 == GPIO_TEST_PROFILE_SEL)
ESL_GPIO_testPins_clear(ESL_TESTPIN_STATE_REG_BANK, ESL_TESTPIN_1_MASK);
#endif
#endif
}
Exit:
{
EC_API_SLV_postSeqOutputPDBuffer(
);
}
{
EC_API_SLV_postSeqInputPDBuffer(
);
}
#if (defined GPIO_TEST_PINS) && (1==GPIO_TEST_PINS)
#if (defined GPIO_TEST_PROFILE_SEL) && (defined GPIO_TEST_PROFILE_1) && (GPIO_TEST_PROFILE_1 == GPIO_TEST_PROFILE_SEL)
ESL_GPIO_testPins_clear(ESL_TESTPIN_STATE_REG_BANK, ESL_TESTPIN_2_MASK);
#endif
#endif
return;
}
{
OSALUNREF_PARM(ctxt);
OSAL_printf("Local error triggered: 0x%04x\r\n", errorCode);
}
#define ALSTATUSCODE_NOERROR
No error.
Definition ecSlvApiDef_AL_codes.h:43
#define CONTROLWORD_COMMAND_ENABLEOPERATION
Enable operation command.
Definition ecSlvApiDef_CiA402.h:68
#define CYCLIC_SYNC_TORQUE_MODE
Cyclic Synchronous Torque mode.
Definition ecSlvApiDef_CiA402.h:128
#define SLOW_DOWN_RAMP
Slow down ramp (options: 0x605B; 0x605C; 0x605E)
Definition ecSlvApiDef_CiA402.h:309
uint16_t EC_SLV_APP_CIA_startInputHandler(void *ctxt, uint16_t *intMask)
Get cycle time information.
Definition ESL_cia402Demo.c:520
#define EC_SLV_APP_CIA_GETAXISVALUE(gav_type, gav_target, gav_axisDesc)
Read CiA402 Axis value.
Definition ESL_cia402Demo.c:84
#define OBD_FAULT_REACTION_INDEX(x)
Definition ecSlvApiDef_CiA402.h:341
void EC_SLV_APP_cia402Application(void *ctxt)
CiA402 Application function.
Definition ESL_cia402Demo.c:983
#define VOLTAGE_LIMIT_NO_TRANSIT
Slow down on voltage limit and stay in Quick Stop Active.
Definition ecSlvApiDef_CiA402.h:323
#define QUICKSTOP_RAMP_NO_TRANSIT
Slow down on quick stop ramp and stay in Quick Stop Active.
Definition ecSlvApiDef_CiA402.h:321
#define CONTROLWORD_COMMAND_SWITCHON
Switch on command.
Definition ecSlvApiDef_CiA402.h:63
void EC_SLV_APP_cia402LocalError(void *ctxt, uint16_t errorCode)
Local Error function handler.
Definition ESL_cia402Demo.c:1316
uint32_t EC_SLV_APP_getCiA402ObjectEntryValue(void *pAppCtxt_p, EC_API_SLV_SCoE_ObjEntry_t *pObjectEntry_p, uint16_t length_p, uint16_t *pValue_p)
Read CiA402 Object entry.
Definition ESL_cia402Demo.c:305
#define EC_SLV_APP_CIA_SETAXISVALUE(sav_type, sav_axisDesc, sav_value)
Write CiA402 Axis value.
Definition ESL_cia402Demo.c:112
#define QUICKSTOP_RAMP
Quick stop ramp (options: 0x605E)
Definition ecSlvApiDef_CiA402.h:310
#define STATUSWORD_INTERNAL_LIMIT
Internal limit.
Definition ecSlvApiDef_CiA402.h:79
#define OBD_DISABLE_OPERATION_INDEX(x)
Definition ecSlvApiDef_CiA402.h:339
#define DISABLE_DRIVE
Disable drive (options: 0x605B; 0x605C; 0x605E)
Definition ecSlvApiDef_CiA402.h:308
#define STATUSWORD_DRIVE_FOLLOWS_COMMAND
Drive follows command (used in cyclic synchronous modes)
Definition ecSlvApiDef_CiA402.h:83
#define OBD_SW_POSITION_LIMIT_INDEX(x)
Definition ecSlvApiDef_CiA402.h:371
#define CYCLIC_SYNC_POSITION_MODE
Cyclic Synchronous Position mode.
Definition ecSlvApiDef_CiA402.h:126
#define CURRENT_LIMIT_NO_TRANSIT
Slow down on current limit and stay in Quick Stop Active.
Definition ecSlvApiDef_CiA402.h:322
#define STOP_ON_VOLTAGE_LIMIT
Stop on voltage limit (options: 0x605E)
Definition ecSlvApiDef_CiA402.h:312
#define CYCLIC_SYNC_VELOCITY_MODE
Cyclic Synchronous Velocity mode.
Definition ecSlvApiDef_CiA402.h:127
#define OBD_QUICKSTOP_INDEX(x)
Definition ecSlvApiDef_CiA402.h:337
#define OBD_SHUTDOWN_INDEX(x)
Definition ecSlvApiDef_CiA402.h:338
void EC_SLV_APP_setObdValues(void *ctxt)
Set default values for CiA 402 object dictionary.
Definition ESL_cia402Demo.c:463
#define STOP_ON_CURRENT_LIMIT
Stop on current limit (options: 0x605E)
Definition ecSlvApiDef_CiA402.h:311
#define SLOWDOWN_RAMP_NO_TRANSIT
Slow down on slow down ramp and stay in Quick Stop Active.
Definition ecSlvApiDef_CiA402.h:320
#define STATUSWORD_TARGET_REACHED
Target reached.
Definition ecSlvApiDef_CiA402.h:81
@ EC_API_eERR_INVALID
Definition ecSlvApiDef_error.h:52
@ EC_API_eERR_BUSY
Definition ecSlvApiDef_error.h:50
@ EC_API_eERR_NULLPTR
Definition ecSlvApiDef_error.h:58
@ EC_API_eERR_NONE
Definition ecSlvApiDef_error.h:45
enum EC_API_SLV_EEsmState EC_API_SLV_EEsmState_t
EC_STATE_T EtherCAT State Machine states.
@ EC_API_SLV_eESM_op
Operational State.
Definition ecSlvApi_types.h:75
@ EC_API_SLV_eESM_uninit
Uninitialized State.
Definition ecSlvApi_types.h:70
@ EC_API_SLV_eESM_safeop
SafeOP State.
Definition ecSlvApi_types.h:74
enum EC_API_SLV_EUserRetCodeserRetCodes EC_API_SLV_EUserRetCodes_t
EC_RETCODE_T Error codes used during EtherCAT State Machine transitions.
@ EC_USR_eRET_OK
no error occurred
Definition ecSlvApi_types.h:62
@ EC_USR_eRET_ERROR
Unspecified error occurred.
Definition ecSlvApi_types.h:63
Definition ecSlvApiInternal.h:130
Definition ecSlvApiInternal.h:144
Definition ecSlvApiInternal.h:329
uint16_t realPdoOutLen
Definition ecSlvCiA402.h:207
uint8_t * pdRxBuffer
Definition ecSlvCiA402.h:204
uint8_t * pdTxBuffer
Definition ecSlvCiA402.h:205
uint16_t realPdoInLen
Definition ecSlvCiA402.h:208
uint32_t pdoInLen
Definition ecSlvCiA402.h:203
EC_API_SLV_SHandle_t * ptEcSlvApi
Definition ecSlvCiA402.h:211
uint32_t pdoOutLen
Definition ecSlvCiA402.h:202
EC_SLV_APP_sCIA_axisData_t CiA402_axisData[AXES_NUMBER]
Definition ecSlvCiA402.h:209
Definition ecSlvCiA402.h:158
uint8_t id
Axis Identification.
Definition ESL_cia402Demo.c:120
bool lowLevelPowerApplied
Low-level power applied.
Definition ESL_cia402Demo.c:123
double positionActualValue
Actual position within control loop.
Definition ESL_cia402Demo.c:127
bool brakeApplied
Motor brake is applied.
Definition ESL_cia402Demo.c:122
uint32_t cycleTime
Motion controller cycletime in us.
Definition ESL_cia402Demo.c:128
bool axisIsActive
Axis is active.
Definition ESL_cia402Demo.c:121
bool highLevelPowerApplied
High-level power applied.
Definition ESL_cia402Demo.c:124
bool axisFunctionEnabled
Axis functions enabled.
Definition ESL_cia402Demo.c:125
bool configurationAllowed
Configuration allowed.
Definition ESL_cia402Demo.c:126
Data structure to handle an CiA 402 axis.
Definition ESL_cia402Demo.c:119
EC_SLV_APP_sCIA_object_t statusWordIndex
Definition ecSlvCiA402.h:82
uint32_t posLimitMin
Definition ecSlvCiA402.h:153
EC_SLV_APP_sCIA_object_t targetTorqueIndex
Definition ecSlvCiA402.h:104
EC_SLV_APP_sCIA_object_t targetVelocityIndex
Definition ecSlvCiA402.h:114
EC_SLV_APP_sCIA_object_t faultReactionIndex
Definition ecSlvCiA402.h:87
EC_SLV_APP_sCIA_object_t modesOfOperationDisplayIndex
Definition ecSlvCiA402.h:89
EC_SLV_APP_sCIA_object_t targetPositionIndex
Definition ecSlvCiA402.h:109
EC_SLV_APP_sCIA_object_t modesOfOperationIndex
Definition ecSlvCiA402.h:88
EC_SLV_APP_sCIA_object_t velocityActualValueIndex
Definition ecSlvCiA402.h:99
EC_SLV_APP_sCIA_object_t quickStopIndex
Definition ecSlvCiA402.h:83
uint32_t posLimitMax
Definition ecSlvCiA402.h:152
EC_SLV_APP_sCIA_objectEntry_t positionLimitMin
Definition ecSlvCiA402.h:150
EC_SLV_APP_sCIA_object_t disableOperationIndex
Definition ecSlvCiA402.h:85
EC_SLV_APP_sCIA_object_t controlWordIndex
Definition ecSlvCiA402.h:81
EC_SLV_APP_sCIA_object_t supportedDriveModesIndex
Definition ecSlvCiA402.h:147
EC_SLV_APP_sCIA_object_t torqueActualValueIndex
Definition ecSlvCiA402.h:107
EC_SLV_APP_sCIA_object_t positionActualValueIndex
Definition ecSlvCiA402.h:92
EC_SLV_APP_sCIA_objectEntry_t positionLimitMax
Definition ecSlvCiA402.h:149
EC_SLV_APP_sCIA_object_t shutdownIndex
Definition ecSlvCiA402.h:84
EC_API_SLV_SCoE_Object_t * pSdo
Definition ecSlvCiA402.h:62
uint16_t objectIndex
Definition ecSlvCiA402.h:61
EC_API_SLV_SCoE_ObjEntry_t * pObjetEntry
Definition ecSlvCiA402.h:75
Definition ecSlvCiA402.h:60