EtherCAT 从站CIA402实验

本文目标实现上位机TwinCAT3和下位机STM32F407 LAN9252 之间通过EtherCAT CIA402进行通讯。1.通过SSC生成从站设备描述文件SSC版本5.11方法跟我上一篇中的IO实验很像主要区别在于Excel的配置有一些不同这里我没有用CIA402的模板因为里面有一些用法比如多轴、多槽还有模块的用法我不是很了解。而我在项目中只是为了使用CIA402的状态机协议栈因此我是自己手动移植的望各位理解哈哈~以下是我为了移植CIA402专门确定的一些配置大家可以根据自己需要进行删减我没有把5001改成402没有也不影响402协议的使用只是主站不知道这个是402从站罢了。然后生成相应的对象字典以后就正式开始移植操作。2.CIA402协议栈的移植首先比较重要的地方在CIA402_Init()这个函数中对比直接通过模板生成的文件我们可以知道这部分主要用于初始化“轴”对象的一些属性和字典索引这里主要就两个部分/*Reset Axis buffer*/ HMEMSET(LocalAxes[0],0,SIZEOF(TCiA402Axis)); LocalAxes[0].bAxisIsActive FALSE; LocalAxes[0].bBrakeApplied TRUE; LocalAxes[0].bLowLevelPowerApplied TRUE; LocalAxes[0].bHighLevelPowerApplied FALSE; LocalAxes[0].bAxisFunctionEnabled FALSE; LocalAxes[0].bConfigurationAllowed TRUE; LocalAxes[0].i16State STATE_NOT_READY_TO_SWITCH_ON; LocalAxes[0].u16PendingOptionCode 0x00; LocalAxes[0].fCurPosition 0; LocalAxes[0].u32CycleTime 0;/*********************************** init objects *************************************/ /*set default values*/ HMEMCPY(LocalAxes[0].Objects,DefCiA402ObjectValues,CIA402_OBJECTS_SIZE); /*********************************** init objects dictionary entries *************************************/ LocalAxes[0].ObjDic (TOBJECT *) ALLOCMEM(SIZEOF(DefCiA402AxisObjDic)); HMEMCPY(LocalAxes[0].ObjDic,DefCiA402AxisObjDic,SIZEOF(DefCiA402AxisObjDic)); { TOBJECT OBJMEM *pDiCEntry LocalAxes[0].ObjDic; /*adapt Object index and assign Var pointer*/ while(pDiCEntry-Index ! 0xFFFF) { BOOL bObjectFound TRUE; switch(pDiCEntry-Index) { case 0x1600: pDiCEntry-pVarPtr LocalAxes[0].Objects.sRxPDOMap0; break; case 0x1A00: pDiCEntry-pVarPtr LocalAxes[0].Objects.sTxPDOMap0; break; case 0x2000: pDiCEntry-pVarPtr LocalAxes[0].Objects.Kp; break; case 0x2001: pDiCEntry-pVarPtr LocalAxes[0].Objects.Kd; break; case 0x603F: pDiCEntry-pVarPtr LocalAxes[0].Objects.objErrorCode; break; case 0x6040: pDiCEntry-pVarPtr LocalAxes[0].Objects.objControlWord; break; case 0x6041: pDiCEntry-pVarPtr LocalAxes[0].Objects.objStatusWord; break; case 0x6502: pDiCEntry-pVarPtr LocalAxes[0].Objects.objSupportedDriveModes; break; case 0x605A: pDiCEntry-pVarPtr LocalAxes[0].Objects.objQuickStopOptionCode; break; case 0x605B: pDiCEntry-pVarPtr LocalAxes[0].Objects.objShutdownOptionCode; break; case 0x605C: pDiCEntry-pVarPtr LocalAxes[0].Objects.objDisableOperationOptionCode; break; case 0x605E: pDiCEntry-pVarPtr LocalAxes[0].Objects.objFaultReactionCode; break; case 0x6060: pDiCEntry-pVarPtr LocalAxes[0].Objects.objModesOfOperation; break; case 0x6061: pDiCEntry-pVarPtr LocalAxes[0].Objects.objModesOfOperationDisplay; break; case 0x6064: pDiCEntry-pVarPtr LocalAxes[0].Objects.objPositionActualValue; break; case 0x606C: pDiCEntry-pVarPtr LocalAxes[0].Objects.objVelocityActualValue; break; case 0x6071: pDiCEntry-pVarPtr LocalAxes[0].Objects.objTargetTorque; break; case 0x6077: pDiCEntry-pVarPtr LocalAxes[0].Objects.objTorqueActualValue; break; case 0x607A: pDiCEntry-pVarPtr LocalAxes[0].Objects.objTargetPosition; break; case 0x607D: pDiCEntry-pVarPtr LocalAxes[0].Objects.objSoftwarePositionLimit; break; case 0x6085: pDiCEntry-pVarPtr LocalAxes[0].Objects.objQuickStopDeclaration; break; case 0x60C2: pDiCEntry-pVarPtr LocalAxes[0].Objects.objInterpolationTimePeriod; break; case 0x60FF: pDiCEntry-pVarPtr LocalAxes[0].Objects.objTargetVelocity; break; default : bObjectFound FALSE; break; } pDiCEntry; } }这里我删去了复杂的多槽功能使得代码看上去简单易懂很多。然后CIA402的状态机是不变的我们不需要动它主要关注应用层的实现首先是APPL_GenerateMapping这个函数主要是用来确保PDO映射的准确性以及模式支持的。这里我直接把OutputSize和InputSize给写死然后objSupportedDriveModes定为0x380这样就可以支持三种不同模式CSP,CSV,CST。UINT16 APPL_GenerateMapping(UINT16* pInputSize,UINT16* pOutputSize) { UINT16 result ALSTATUSCODE_NOERROR; UINT16 InputSize 0; UINT16 OutputSize 0; OBJCONST TOBJECT OBJMEM *pDiCEntry NULL; /*Axis is mapped to process data check if axis objects need to be added to the object dictionary*/ if(!LocalAxes[0].bAxisIsActive) { /*add objects to dictionary*/ pDiCEntry LocalAxes[0].ObjDic; while(pDiCEntry-Index ! 0xFFFF) { result COE_AddObjectToDic(pDiCEntry); if(result ! 0) { return result; } pDiCEntry; //get next entry } LocalAxes[0].bAxisIsActive TRUE; LocalAxes[0].Objects.objSupportedDriveModes 0x380; } // OutputSize OutputSize 3; OutputSize 18; if(result 0) { // InputSize InputSize 3; InputSize 16; } *pInputSize InputSize; *pOutputSize OutputSize; return result; }void APPL_InputMapping(UINT16* pData) { UINT16 j 0; UINT16 *pTmpData (UINT16 *)pData; UINT8 AxisIndex 0; *pTmpData LocalAxes[AxisIndex].Objects.objPositionActualValue 0xFFFF; *pTmpData LocalAxes[AxisIndex].Objects.objPositionActualValue16; *pTmpData LocalAxes[AxisIndex].Objects.objVelocityActualValue 0xFFFF; *pTmpData LocalAxes[AxisIndex].Objects.objVelocityActualValue16; *pTmpData LocalAxes[AxisIndex].Objects.objTorqueActualValue; *pTmpData LocalAxes[AxisIndex].Objects.objStatusWord; *pTmpData LocalAxes[AxisIndex].Objects.objErrorCode; *pTmpData LocalAxes[AxisIndex].Objects.objModesOfOperationDisplay 0xFF; } void APPL_OutputMapping(UINT16* pData) { UINT16 j 0; UINT16 *pTmpData (UINT16 *)pData; UINT8 AxisIndex 0; LocalAxes[AxisIndex].Objects.objControlWord SWAPWORD(*pTmpData); LocalAxes[AxisIndex].Objects.objModesOfOperation SWAPWORD(*pTmpData)0x00ff; LocalAxes[AxisIndex].Objects.objTargetPosition SWAPWORD(*pTmpData); LocalAxes[AxisIndex].Objects.objTargetPosition (SWAPWORD(*pTmpData)16); LocalAxes[AxisIndex].Objects.objTargetVelocity SWAPWORD(*pTmpData); LocalAxes[AxisIndex].Objects.objTargetVelocity (SWAPWORD(*pTmpData)16); LocalAxes[AxisIndex].Objects.objTargetTorque SWAPWORD(*pTmpData); LocalAxes[AxisIndex].Objects.objTargetTorque (SWAPWORD(*pTmpData)16); LocalAxes[AxisIndex].Objects.Kp SWAPWORD(*pTmpData); LocalAxes[AxisIndex].Objects.Kd SWAPWORD(*pTmpData); }这样改了之后主站发来的数据就会实时更新LocalAxes的值然后在状态机中进行一些逻辑操作通过修改控制字来让电机使能一般是发送0x06,0x07,0x0F,然后在CiA402_Application()中就会周期性地调用CiA402_DummyMotionControl()执行你的电机控制层逻辑比如修改目标电机位置速度力矩等信息。通过修改操作模式来改变电机运行状态我这里代码里支持三种。int testPcnt 0; int testVcnt 0; int testTcnt 0; void CiA402_DummyMotionControl(TCiA402Axis *pCiA402Axis) { // float IncFactor (float)0.0010922 * (float) pCiA402Axis-u32CycleTime; INT32 i32TargetVelocity 0; INT32 i32TargetTorque 0; /*Motion Controller shall only be triggered if application is trigger by DC Sync Signals, and a valid mode of operation is set*/ /*calculate actual position */ // pCiA402Axis-fCurPosition ((double)pCiA402Axis-Objects.objVelocityActualValue) * IncFactor; // pCiA402Axis-Objects.objPositionActualValue (INT32)(pCiA402Axis-fCurPosition); if(pCiA402Axis-bAxisFunctionEnabled pCiA402Axis-bLowLevelPowerApplied pCiA402Axis-bHighLevelPowerApplied !pCiA402Axis-bBrakeApplied) { if((pCiA402Axis-Objects.objSoftwarePositionLimit.MaxPositionLimit pCiA402Axis-Objects.objPositionActualValue || pCiA402Axis-Objects.objPositionActualValue pCiA402Axis-Objects.objTargetPosition) (pCiA402Axis-Objects.objSoftwarePositionLimit.MinPositionLimit pCiA402Axis-Objects.objPositionActualValue || pCiA402Axis-Objects.objPositionActualValue pCiA402Axis-Objects.objTargetPosition)) { pCiA402Axis-Objects.objStatusWord ~STATUSWORD_INTERNAL_LIMIT; switch(pCiA402Axis-Objects.objModesOfOperationDisplay) { case CYCLIC_SYNC_POSITION_MODE: if(pCiA402Axis-i16State STATE_OPERATION_ENABLED) { // to do ... } else { // to do ... } break; case CYCLIC_SYNC_VELOCITY_MODE: testVcnt; if(pCiA402Axis-i16State STATE_OPERATION_ENABLED) i32TargetVelocity pCiA402Axis-Objects.objTargetVelocity; else i32TargetVelocity 0; break; case CYCLIC_SYNC_TORQUE_MODE: testTcnt; if(pCiA402Axis-i16State STATE_OPERATION_ENABLED) i32TargetTorque pCiA402Axis-Objects.objTargetTorque; else i32TargetTorque 0; break; default: break; } } else { pCiA402Axis-Objects.objStatusWord | STATUSWORD_INTERNAL_LIMIT; } } // 根据当前电机状态进行更新 pCiA402Axis-Objects.objVelocityActualValue 0; pCiA402Axis-Objects.objPositionActualValue 0; pCiA402Axis-Objects.objTorqueActualValue 0; // 更新模式 pCiA402Axis-Objects.objModesOfOperationDisplay pCiA402Axis-Objects.objModesOfOperation; }内容到此结束了代码在下方有需要自取感谢各位看官老爷欢迎批评指正AtomGit | GitCode - 全球开发者的开源社区,开源代码托管平台