ESL_cia402Demo.c

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:

1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
2. 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.
3. 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; 
}
EC_SLV_API_CiA402_SAxis_t;
 
/*-----------------------------------------------------------------------------------------
------
------    local variables and constants
------
-----------------------------------------------------------------------------------------*/
static EC_SLV_API_CiA402_SAxis_t       localAxes_s[AXES_NUMBER];
 
//Supported drive modes: ETG6010 6.8.1
#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 EC_SLV_APP_getCiA402ObjectValue(EC_API_SLV_SHandle_t* pEcApiSlv_p, uint16_t index_p, uint16_t length_p, uint16_t* pValue_p)
{
    EC_API_SLV_SCoE_Object_t* pObject;
    uint32_t err;
    err = EC_API_SLV_CoE_getObject(pEcApiSlv_p,index_p, &pObject);
    if (err == EC_API_eERR_NONE)
    {
        err = EC_API_SLV_CoE_getObjectData(pEcApiSlv_p, pObject, length_p, pValue_p);
    }
    return  err;
}
 
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)
{
    EC_API_SLV_SCoE_Object_t* pObject;
    uint32_t err;
 
    err = EC_API_SLV_CoE_getObject(pEcApiSlv_p,index_p, &pObject);
    if (err == EC_API_eERR_NONE)
    {
        err = EC_API_SLV_CoE_setObjectData(pEcApiSlv_p, pObject, length_p, pValue_p);
    }
    return  err;
}
 
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)
{
    EC_API_SLV_SCoE_ObjEntry_t* pObjEntry;
    uint32_t err;
    err = EC_API_SLV_CoE_getObjectEntry(pEcApiSlv_p,index_p, subIndex_p, &pObjEntry);
    if (err == EC_API_eERR_NONE)
    {
        err = EC_API_SLV_CoE_getObjectEntryData(pEcApiSlv_p, pObjEntry, length_p, pValue_p);
    }
    return  err;
}
 
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)
{
    EC_API_SLV_SCoE_ObjEntry_t* pObjEntry;
    uint32_t err;
    err = EC_API_SLV_CoE_getObjectEntry(pEcApiSlv_p,index_p, subIndex_p, &pObjEntry);
    if (err == EC_API_eERR_NONE)
    {
        err = EC_API_SLV_CoE_setObjectEntryData(pEcApiSlv_p, pObjEntry, length_p, pValue_p);
    }
    return  err;
}
 
uint32_t EC_SLV_APP_setSupportedDriveModes(EC_API_SLV_SHandle_t* pEcApiSlv_p)
{
    uint32_t err;
    uint32_t driveMode = DRIVE_MODE_CSP | DRIVE_MODE_CSV | DRIVE_MODE_CST;
 
    for(uint8_t axisNo = 0; axisNo < AXES_NUMBER; axisNo++)
    {
        err = EC_SLV_APP_setCiA402ObjectValue(pEcApiSlv_p, OBD_SUPPORTED_DRIVE_MODES_INDEX(axisNo), sizeof (driveMode), (uint16_t*) &driveMode);
    }
    return err;
}
 
 
 
void EC_SLV_APP_setObdValues(void* pCtxt_p)
{
    EC_API_SLV_SHandle_t* pEcApiSlv = (EC_API_SLV_SHandle_t*) pCtxt_p;
    int32_t posMaxLimit = POSITION_MAX_LIMIT;
 
    EC_SLV_APP_setSupportedDriveModes(pEcApiSlv);
 
    for(uint8_t axisNo = 0; axisNo < AXES_NUMBER; axisNo++)
    {
        EC_SLV_APP_setCiA402ObjectEntryValue(pEcApiSlv, OBD_SW_POSITION_LIMIT_INDEX(axisNo), 2, sizeof(posMaxLimit), (uint16_t*) &posMaxLimit);
    }
}
 
EC_API_SLV_EUserRetCodes_t EC_SLV_APP_startInputHandler(void* pCtxt_p, uint16_t* pIntMask_p)
{
    OSALUNREF_PARM(pIntMask_p);
    EC_API_SLV_SHandle_t* pEcApiSlv = (EC_API_SLV_SHandle_t*) pCtxt_p;
    uint32_t sync0CycleTime = EC_API_SLV_readDoubleWordEscRegister(pEcApiSlv, ESC_DC_SYNC0_CYCLETIME_REG);
 
    sync0CycleTime = sync0CycleTime / NSEC_TO_USEC; //get cycle time in us
 
    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;
        }
    }
    return EC_USR_eRET_OK;
}
 
bool EC_SLV_APP_transitionAction(int16_t characteristic_p)
{
   switch(characteristic_p)
   {
   case SLOW_DOWN_RAMP:
        //do stuff
        break;
   case QUICKSTOP_RAMP:
        //do stuff
        break;
   case STOP_ON_CURRENT_LIMIT:
        //do stuff
        break;
   case STOP_ON_VOLTAGE_LIMIT:
        //do stuff
        break;
   default:
      break;
    }
 
    return true;
}
 
void EC_SLV_APP_CST(EC_API_SLV_SHandle_t* pEcApiSlv_p, EC_SLV_API_CiA402_SAxis_t* pCiA402Axis_p)
{
    int32_t targetTorque;
    //Read target position, velocity and torque values
    EC_SLV_APP_getCiA402ObjectValue(pEcApiSlv_p, OBD_TARGET_TORQUE_INDEX(pCiA402Axis_p->id), sizeof(targetTorque), (uint16_t*) &targetTorque);
    //Update velocity value
    EC_SLV_APP_setCiA402ObjectValue(pEcApiSlv_p, OBD_TORQUE_ACTUAL_VALUE_INDEX(pCiA402Axis_p->id), sizeof(targetTorque), (uint16_t*) &targetTorque);
}
 
void EC_SLV_APP_CSV(EC_API_SLV_SHandle_t* pEcApiSlv_p, EC_SLV_API_CiA402_SAxis_t* pCiA402Axis_p)
{
    int32_t targetVelocity;
    //Read target position, velocity and torque values
    EC_SLV_APP_getCiA402ObjectValue(pEcApiSlv_p, OBD_TARGET_VELOCITY_INDEX(pCiA402Axis_p->id), sizeof(targetVelocity), (uint16_t*) &targetVelocity);
    //Update velocity value
    EC_SLV_APP_setCiA402ObjectValue(pEcApiSlv_p, OBD_VELOCITY_ACTUAL_VALUE_INDEX(pCiA402Axis_p->id), sizeof(targetVelocity), (uint16_t*) &targetVelocity);
}
 
void EC_SLV_APP_CSP(EC_API_SLV_SHandle_t* pEcApiSlv_p, EC_SLV_API_CiA402_SAxis_t* pCiA402Axis_p)
{
    uint32_t actualPosition, actualVelocity;
    uint32_t targetPosition;
    int32_t targetVelocity;
    int16_t actualTorque, targetTorque;
 
    float incFactor = (float) (DRIVE_GEAR_RELATION * pCiA402Axis_p->cycleTime);
 
    //Read target position, velocity and torque values
    EC_SLV_APP_getCiA402ObjectValue(pEcApiSlv_p, OBD_TARGET_POSITION_INDEX(pCiA402Axis_p->id), sizeof(targetPosition), (uint16_t*) &targetPosition);
    EC_SLV_APP_getCiA402ObjectValue(pEcApiSlv_p, OBD_TARGET_VELOCITY_INDEX(pCiA402Axis_p->id), sizeof(targetVelocity), (uint16_t*) &targetVelocity);
    EC_SLV_APP_getCiA402ObjectValue(pEcApiSlv_p, OBD_TARGET_TORQUE_INDEX(pCiA402Axis_p->id), sizeof(targetTorque), (uint16_t*) &targetTorque);
 
    //Read actual position, velocity and torque values
    EC_SLV_APP_getCiA402ObjectValue(pEcApiSlv_p, OBD_POSITION_ACTUAL_VALUE_INDEX(pCiA402Axis_p->id), sizeof(actualPosition), (uint16_t*) &actualPosition);
    EC_SLV_APP_getCiA402ObjectValue(pEcApiSlv_p, OBD_VELOCITY_ACTUAL_VALUE_INDEX(pCiA402Axis_p->id), sizeof(actualVelocity), (uint16_t*) &actualVelocity);
    EC_SLV_APP_getCiA402ObjectValue(pEcApiSlv_p, OBD_TORQUE_ACTUAL_VALUE_INDEX(pCiA402Axis_p->id), sizeof(actualTorque), (uint16_t*) &actualTorque);
 
    if (incFactor != 0)
    {
        actualVelocity = (targetPosition - actualPosition) / incFactor;
        pCiA402Axis_p->positionActualValue = actualPosition + actualVelocity;
    }
    //Update position and velocity value
    EC_SLV_APP_setCiA402ObjectValue(pEcApiSlv_p, OBD_VELOCITY_ACTUAL_VALUE_INDEX(pCiA402Axis_p->id), sizeof(actualVelocity), (uint16_t*) &actualVelocity);
    EC_SLV_APP_setCiA402ObjectValue(pEcApiSlv_p, OBD_POSITION_ACTUAL_VALUE_INDEX(pCiA402Axis_p->id), sizeof(pCiA402Axis_p->positionActualValue), (uint16_t*) &pCiA402Axis_p->positionActualValue);
}
 
void EC_SLV_APP_motionControl(EC_API_SLV_SHandle_t* pEcApiSlv_p, EC_SLV_API_CiA402_SAxis_t* pCiA402Axis_p)
{
    uint32_t statusWord, controlWord, targetPosition;
    uint32_t posMaxLimit, posMinLimit;
    uint16_t operationModeDisplay;
 
    //Read control, status and drive operation mode objects
    EC_SLV_APP_getCiA402ObjectValue(pEcApiSlv_p, OBD_CONTROLWORD_INDEX(pCiA402Axis_p->id), sizeof(controlWord), (uint16_t*) &controlWord);
    EC_SLV_APP_getCiA402ObjectValue(pEcApiSlv_p, OBD_STATUSWORD_INDEX(pCiA402Axis_p->id), sizeof(operationModeDisplay), (uint16_t*) &statusWord);
    EC_SLV_APP_getCiA402ObjectValue(pEcApiSlv_p, OBD_MODES_OF_OPERATION_DISPLAY_INDEX(pCiA402Axis_p->id), sizeof(operationModeDisplay), (uint16_t*) &operationModeDisplay);
 
    //Read target position
    EC_SLV_APP_getCiA402ObjectValue(pEcApiSlv_p, OBD_TARGET_POSITION_INDEX(pCiA402Axis_p->id), sizeof(targetPosition), (uint16_t*) &targetPosition);
 
    //Get software position limits
    EC_SLV_APP_getCiA402ObjectEntryValue(pEcApiSlv_p, OBD_SW_POSITION_LIMIT_INDEX(pCiA402Axis_p->id), 1, sizeof(posMinLimit), (uint16_t *) &posMinLimit);
    EC_SLV_APP_getCiA402ObjectEntryValue(pEcApiSlv_p, OBD_SW_POSITION_LIMIT_INDEX(pCiA402Axis_p->id), 2, sizeof(posMaxLimit), (uint16_t *) &posMaxLimit);
 
    //Calculate new targets for CSP, CSV or CST modes
    if(controlWord == CONTROLWORD_COMMAND_ENABLEOPERATION)
    {
        if((posMaxLimit> pCiA402Axis_p->positionActualValue
            || pCiA402Axis_p->positionActualValue > targetPosition) &&
            (posMinLimit < pCiA402Axis_p->positionActualValue
            || pCiA402Axis_p->positionActualValue < targetPosition))
        {
            statusWord &= ~STATUSWORD_INTERNAL_LIMIT;
 
            switch(operationModeDisplay)
            {
            case CYCLIC_SYNC_POSITION_MODE:
                EC_SLV_APP_CSP(pEcApiSlv_p, pCiA402Axis_p);
                break;
            case CYCLIC_SYNC_VELOCITY_MODE:
                //nothing
                EC_SLV_APP_CSV(pEcApiSlv_p, pCiA402Axis_p);
                break;
            case CYCLIC_SYNC_TORQUE_MODE:
                //nothing
                EC_SLV_APP_CST(pEcApiSlv_p, pCiA402Axis_p);
                break;
            default:
                break;
            }
        }
        else
        {
            statusWord |= STATUSWORD_INTERNAL_LIMIT;
        }
    }
    //Update drive status
    EC_SLV_APP_setCiA402ObjectValue(pEcApiSlv_p, OBD_STATUSWORD_INDEX(pCiA402Axis_p->id), sizeof(statusWord), (uint16_t*) &statusWord);
 
    //Accept new mode of operation
    EC_SLV_APP_getCiA402ObjectValue(pEcApiSlv_p, OBD_MODES_OF_OPERATION_INDEX(pCiA402Axis_p->id), sizeof(operationModeDisplay), &operationModeDisplay);
    EC_SLV_APP_setCiA402ObjectValue(pEcApiSlv_p, OBD_MODES_OF_OPERATION_DISPLAY_INDEX(pCiA402Axis_p->id), sizeof(operationModeDisplay), &operationModeDisplay);
}
 
void EC_SLV_APP_cia402Application(void* pCtxt_p)
{
    EC_API_SLV_SHandle_t* pEcApiSlv = (EC_API_SLV_SHandle_t*) pCtxt_p;
 
    uint16_t controlWord, statusWord, errorCode;
    int16_t operationDisplayCode, quickStopOptionCode, shutdownOptionCode, disableOperationCode, faultReactionCode;
    uint16_t driveRamp = DISABLE_DRIVE;
 
    for(uint8_t axisNo = 0; axisNo < AXES_NUMBER; axisNo++)
    {
        //Read drive control and status objects
        EC_SLV_APP_getCiA402ObjectValue(pEcApiSlv, OBD_CONTROLWORD_INDEX(axisNo), sizeof(controlWord), &controlWord);
        EC_SLV_APP_getCiA402ObjectValue(pEcApiSlv, OBD_STATUSWORD_INDEX(axisNo), sizeof(statusWord), &statusWord);
 
        //Enable high level power, no torque on the motor.
        if(controlWord == CONTROLWORD_COMMAND_SWITCHON)
        {
            OSAL_printf("Axis %d Activated\n\r", axisNo);
            EC_API_SLV_CiA402_activateAxis(pEcApiSlv, axisNo, true);
        }
 
        //Read drive's operation mode
        EC_SLV_APP_getCiA402ObjectValue(pEcApiSlv, OBD_MODES_OF_OPERATION_DISPLAY_INDEX(axisNo), sizeof(operationDisplayCode), (uint16_t*) &operationDisplayCode);
 
        //Read supported error option codes (ETG6010 Chapter 4).
        errorCode = EC_API_SLV_CiA402_SM_getErrorCode(pEcApiSlv, axisNo);
 
        if(errorCode &&
                (operationDisplayCode == CYCLIC_SYNC_POSITION_MODE ||
                 operationDisplayCode == CYCLIC_SYNC_VELOCITY_MODE ||
                 operationDisplayCode == CYCLIC_SYNC_TORQUE_MODE))
        {
            statusWord &= ~ STATUSWORD_DRIVE_FOLLOWS_COMMAND;
        }
        else
        {
            statusWord |= STATUSWORD_DRIVE_FOLLOWS_COMMAND;
        }
 
        //Analyse error codes
        switch(errorCode)
        {
        case OBD_QUICKSTOP_INDEX(0):
            /*State transition 11 is pending analyse shutdown option code (0x605A)*/
            {
                EC_SLV_APP_getCiA402ObjectValue(pEcApiSlv, OBD_QUICKSTOP_INDEX(axisNo), sizeof(quickStopOptionCode), (uint16_t*) &quickStopOptionCode);
 
                /*Masked and execute specified quick stop ramp characteristic */
                if(quickStopOptionCode >= SLOWDOWN_RAMP_NO_TRANSIT && quickStopOptionCode <= VOLTAGE_LIMIT_NO_TRANSIT)
                {
                    if (quickStopOptionCode == SLOWDOWN_RAMP_NO_TRANSIT)
                    {
                        driveRamp = SLOW_DOWN_RAMP;
                    }
                    if (quickStopOptionCode == QUICKSTOP_RAMP_NO_TRANSIT)
                    {
                        driveRamp = QUICKSTOP_RAMP;
                    }
                    if (quickStopOptionCode == CURRENT_LIMIT_NO_TRANSIT)
                    {
                        driveRamp = STOP_ON_CURRENT_LIMIT;
                    }
                    if (quickStopOptionCode == VOLTAGE_LIMIT_NO_TRANSIT)
                    {
                        driveRamp = STOP_ON_VOLTAGE_LIMIT;
                    }
                }
 
                if(EC_SLV_APP_transitionAction(driveRamp))
                {
                    /*Quick stop ramp is finished complete state transition*/
                    EC_API_SLV_CiA402_SM_clearErrorCode(pEcApiSlv, axisNo);
                    if (quickStopOptionCode >= SLOWDOWN_RAMP_NO_TRANSIT && quickStopOptionCode <= VOLTAGE_LIMIT_NO_TRANSIT)
                    {
                        statusWord |= STATUSWORD_TARGET_REACHED;
                    }
                }
            }
            break;
        case OBD_SHUTDOWN_INDEX(0):
            /*State transition 8 is pending analyse shutdown option code (0x605B)*/
            {
                EC_SLV_APP_getCiA402ObjectValue(pEcApiSlv, OBD_SHUTDOWN_INDEX(axisNo), sizeof(shutdownOptionCode), (uint16_t*) &shutdownOptionCode);
                if(EC_SLV_APP_transitionAction(shutdownOptionCode))
                {
                    /*shutdown ramp is finished complete state transition*/
                    EC_API_SLV_CiA402_SM_clearErrorCode(pEcApiSlv, axisNo);
                }
            }
            break;
        case OBD_DISABLE_OPERATION_INDEX(0):
            /*State transition 5 is pending analyse Disable operation option code (0x605C)*/
            {
                EC_SLV_APP_getCiA402ObjectValue(pEcApiSlv, OBD_DISABLE_OPERATION_INDEX(axisNo), sizeof(disableOperationCode), (uint16_t*) &disableOperationCode);
                if(EC_SLV_APP_transitionAction(disableOperationCode))
                {
                    /*disable operation ramp is finished complete state transition*/
                    EC_API_SLV_CiA402_SM_clearErrorCode(pEcApiSlv, axisNo);
                }
            }
            break;
 
        case OBD_FAULT_REACTION_INDEX(0):
            /*State transition 14 is pending analyse Fault reaction option code (0x605E)*/
            {
                EC_SLV_APP_getCiA402ObjectValue(pEcApiSlv, OBD_FAULT_REACTION_INDEX(axisNo), sizeof(faultReactionCode) , (uint16_t*) &faultReactionCode);
                if(EC_SLV_APP_transitionAction(faultReactionCode))
                {
                    /*fault reaction ramp is finished complete state transition*/
                    EC_API_SLV_CiA402_SM_clearErrorCode(pEcApiSlv, axisNo);
                }
            }
            break;
        default:
            //Pending transition code is invalid => values from the master are used
            statusWord |= STATUSWORD_DRIVE_FOLLOWS_COMMAND;
            break;
        }
 
        EC_SLV_APP_setCiA402ObjectValue(pEcApiSlv, OBD_STATUSWORD_INDEX(axisNo), sizeof(statusWord), &statusWord);
 
        EC_SLV_APP_motionControl(pEcApiSlv, &localAxes_s[axisNo]);
    }
}
 
void EC_SLV_APP_cia402LocalError(void* pCtxt_p, uint16_t errorCode_p)
{
    OSALUNREF_PARM(pCtxt_p);
    OSAL_printf("Local error triggered: %d\r\n", errorCode_p);
}
 
//*************************************************************************************************