6.2.1 Example of EtherCAT PLC axis
#include "plc.h"
#include "smachine.h"
#include "GMPU3.h"
static char* msgVersion = "PLC axis demo";
static unsigned short* para;
static long* lPara;
static int HandwheelCounter_diff;
static int old_HandwheelCounter;
static int HandwheelCounter_encoderAccumulator;
static int get_plc_axis_pos_flag;
static int old_plc_axis_pos;
static int plc_handwheel_mode;
#define PLC_MH_ATTR_0 0x1
void PLCAPI plcOpen(Status& sts, PlcBlock& plc,unsigned short* PlcData, long* LongPlcData)
{
plc.versionText = msgVersion;
para = PlcData;
lPara = LongPlcData;
}
void PLCAPI plcRun(Status& sts, PlcBlock& plc)
{
plc.errorMessage = (char*)0;
plc.vto0.bit.emgsp = 0;
HandleOperationPanel(sts, plc);// scanning second panel GMPU3
//**start accumulating handwheels counter, a 16-bit unsigned short. Handwheel is connected to the second panel CMPU3
HandwheelCounter_diff = HandwheelCounter - old_HandwheelCounter;
if(HandwheelCounter_diff < -32767){
HandwheelCounter_diff = HandwheelCounter + 65536 - old_HandwheelCounter;
HandwheelCounter_encoderAccumulator = HandwheelCounter_encoderAccumulator + HandwheelCounter_diff;
}else if(HandwheelCounter_diff > 32767){
HandwheelCounter_diff = old_HandwheelCounter + 65536 - HandwheelCounter;
HandwheelCounter_encoderAccumulator = HandwheelCounter_encoderAccumulator - HandwheelCounter_diff;
}else{
HandwheelCounter_encoderAccumulator = HandwheelCounter_encoderAccumulator + HandwheelCounter_diff;
}
old_HandwheelCounter = HandwheelCounter;
//**end
if(plc_handwheel_mode){
if(!get_plc_axis_pos_flag){//store current axial position when switching to handwheel mode
old_plc_axis_pos = plc.ecatPlcAxis[0].sts.actualPos;
get_plc_axis_pos_flag = 1;
}
if(plc.ecatPlcAxis[0].cmd.flag == ECAT_AXIS_SET_PP_POS && plc.ecatPlcAxis[0].cmd.finish){
plc.ecatPlcAxis[0].cmd.flag = 0;
}else{//target position is sum of stored axial position and accumulated handwheel counter
plc.ecatPlcAxis[0].cmd.flag = ECAT_AXIS_SET_PP_POS;
plc.ecatPlcAxis[0].cmd.data[0] = old_plc_axis_pos + HandwheelCounter_encoderAccumulator*(16777216/4/100); //data0, assume that the screw has 10mm pitch, it will move 0.1mm on every handwheel click
plc.ecatPlcAxis[0].cmd.data[1] = 0; //data1, no confirm ending & absolute position //16777216, 24bit encoder
} //4 Handwheel generates 4 count on every click
}else{ //100 10mm pitch screw, move 0.1mm on every handwheel click
HandwheelCounter_encoderAccumulator = 0;
}
// test the index detecting
plc.gdo[0].bit.bit00 = plc.ecatPlcAxis[0].sts.index;
plc.gdo[0].bit.bit01 = plc.ecatPlcAxis[1].sts.index;
plc.gdo[0].bit.bit02 = plc.ecatPlcAxis[2].sts.index;
plc.gdo[0].bit.bit03 = plc.ecatPlcAxis[3].sts.index;
plc.gdo[0].bit.bit04 = plc.ecatPlcAxis[4].sts.index;
plc.gdo[0].bit.bit05 = plc.ecatPlcAxis[5].sts.index;
plc.gdo[0].bit.bit06 = plc.ecatPlcAxis[6].sts.index;
plc.gdo[0].bit.bit07 = plc.ecatPlcAxis[7].sts.index;
if(plc.mCode.flag){
switch(plc.mCode.data){
//Plc Axis Setting Command
case 10://Set PLC axis to PP mode 1:PP 3:PV Usage: M10,H0~7 M10,H0 access the 0th EtherCAT PLC axis
if(plc.mhAttr & PLC_MH_ATTR_0){
if(plc.ecatPlcAxis[plc.mhCode0].cmd.flag == ECAT_AXIS_SET_MODE && plc.ecatPlcAxis[plc.mhCode0].cmd.finish){
plc.ecatPlcAxis[plc.mhCode0].cmd.flag = 0;
plc.mCode.finish = 1;
}else{
plc.ecatPlcAxis[plc.mhCode0].cmd.flag = ECAT_AXIS_SET_MODE;
plc.ecatPlcAxis[plc.mhCode0].cmd.data[0] = 1; //data0
}
}else{
plc.mCode.finish = 1;
}
break;
case 11://Set PLC axis to PV mode 1:PP 3:PV
if(plc.mhAttr & PLC_MH_ATTR_0){
if(plc.ecatPlcAxis[plc.mhCode0].cmd.flag == ECAT_AXIS_SET_MODE && plc.ecatPlcAxis[plc.mhCode0].cmd.finish){
plc.ecatPlcAxis[plc.mhCode0].cmd.flag = 0;
plc.mCode.finish = 1;
}else{
plc.ecatPlcAxis[plc.mhCode0].cmd.flag = ECAT_AXIS_SET_MODE;
plc.ecatPlcAxis[plc.mhCode0].cmd.data[0] = 3; //data0
}
}else{
plc.mCode.finish = 1;
}
break;
case 12://Set acceleration of PLC axis, unit: pulse/sec^2
if(plc.mhAttr & PLC_MH_ATTR_0){
if(plc.ecatPlcAxis[plc.mhCode0].cmd.flag == ECAT_AXIS_SET_ACCEL && plc.ecatPlcAxis[plc.mhCode0].cmd.finish){
plc.ecatPlcAxis[plc.mhCode0].cmd.flag = 0;
plc.mCode.finish = 1;
}else{
plc.ecatPlcAxis[plc.mhCode0].cmd.flag = ECAT_AXIS_SET_ACCEL;
plc.ecatPlcAxis[plc.mhCode0].cmd.data[0] = 999999999; //data0
}
}else{
plc.mCode.finish = 1;
}
break;
case 13://Set velocity of PP mode, unit: pulse/sec
if(plc.mhAttr & PLC_MH_ATTR_0){
if(plc.ecatPlcAxis[plc.mhCode0].cmd.flag == ECAT_AXIS_SET_PP_VEL && plc.ecatPlcAxis[plc.mhCode0].cmd.finish){
plc.ecatPlcAxis[plc.mhCode0].cmd.flag = 0;
plc.mCode.finish = 1;
}else{
plc.ecatPlcAxis[plc.mhCode0].cmd.flag = ECAT_AXIS_SET_PP_VEL;
plc.ecatPlcAxis[plc.mhCode0].cmd.data[0] = 300000000; //data0
}
}else{
plc.mCode.finish = 1;
}
break;
case 14://Set velocity of PV mode, unit: pulse/sec
if(plc.mhAttr & PLC_MH_ATTR_0){
if(plc.ecatPlcAxis[plc.mhCode0].cmd.flag == ECAT_AXIS_SET_PV_VEL && plc.ecatPlcAxis[plc.mhCode0].cmd.finish){
plc.ecatPlcAxis[plc.mhCode0].cmd.flag = 0;
plc.mCode.finish = 1;
}else{
plc.ecatPlcAxis[plc.mhCode0].cmd.flag = ECAT_AXIS_SET_PV_VEL;
plc.ecatPlcAxis[plc.mhCode0].cmd.data[0] = 300000000; //data0
}
}else{
plc.mCode.finish = 1;
}
break;
case 15://enable servo
if(plc.mhAttr & PLC_MH_ATTR_0){
if(plc.ecatPlcAxis[plc.mhCode0].cmd.flag == ECAT_AXIS_SET_SRVON && plc.ecatPlcAxis[plc.mhCode0].cmd.finish){
plc.ecatPlcAxis[plc.mhCode0].cmd.flag = 0;
plc.mCode.finish = 1;
}else{
plc.ecatPlcAxis[plc.mhCode0].cmd.flag = ECAT_AXIS_SET_SRVON;
plc.ecatPlcAxis[plc.mhCode0].cmd.data[0] = 1; //data0
}
}else{
plc.mCode.finish = 1;
}
break;
case 16://disable servo
if(plc.mhAttr & PLC_MH_ATTR_0){
if(plc.ecatPlcAxis[plc.mhCode0].cmd.flag == ECAT_AXIS_SET_SRVON && plc.ecatPlcAxis[plc.mhCode0].cmd.finish){
plc.ecatPlcAxis[plc.mhCode0].cmd.flag = 0;
plc.mCode.finish = 1;
}else{
plc.ecatPlcAxis[plc.mhCode0].cmd.flag = ECAT_AXIS_SET_SRVON;
plc.ecatPlcAxis[plc.mhCode0].cmd.data[0] = 0; //data0
}
}else{
plc.mCode.finish = 1;
}
break;
case 17://enter handwheel mode, this example could only be used on 0th EtherCAT PLC axis
plc_handwheel_mode = 1;
get_plc_axis_pos_flag = 0;
plc.mCode.finish = 1;
break;
case 18://quit handwheel mode, this example could only be used on 0th EtherCAT PLC axis
if(plc.ecatPlcAxis[0].cmd.flag == 0){
plc_handwheel_mode = 0;
plc.mCode.finish = 1;
}
break;
case 19://Use absolute mode to move PLC axis to the position determined by PLC long data
if(plc.mhAttr & PLC_MH_ATTR_0){
if(plc.ecatPlcAxis[plc.mhCode0].cmd.flag == ECAT_AXIS_SET_PP_POS && plc.ecatPlcAxis[plc.mhCode0].cmd.finish){
plc.ecatPlcAxis[plc.mhCode0].cmd.flag = 0;
plc.mCode.finish = 1;
}else{
plc.ecatPlcAxis[plc.mhCode0].cmd.flag = ECAT_AXIS_SET_PP_POS;
plc.ecatPlcAxis[plc.mhCode0].cmd.data[0] = lPara[0]; //data0
plc.ecatPlcAxis[plc.mhCode0].cmd.data[1] = 0; //data1
}
}else{
plc.mCode.finish = 1;
}
break;
case 20://reset axial index signal
if(plc.mhAttr & PLC_MH_ATTR_0){
if(plc.ecatPlcAxis[plc.mhCode0].cmd.flag == ECAT_AXIS_CLR_IDX && plc.ecatPlcAxis[plc.mhCode0].cmd.finish){
plc.ecatPlcAxis[plc.mhCode0].cmd.flag = 0;
plc.mCode.finish = 1;
}else{
plc.ecatPlcAxis[plc.mhCode0].cmd.flag = ECAT_AXIS_CLR_IDX;
}
}else{
plc.mCode.finish = 1;
}
break;
case 21://reset axial absolute position
if(plc.mhAttr & PLC_MH_ATTR_0){
if(plc.ecatPlcAxis[plc.mhCode0].cmd.flag == ECAT_AXIS_CLR_ABS && plc.ecatPlcAxis[plc.mhCode0].cmd.finish){
plc.ecatPlcAxis[plc.mhCode0].cmd.flag = 0;
plc.mCode.finish = 1;
}else{
plc.ecatPlcAxis[plc.mhCode0].cmd.flag = ECAT_AXIS_CLR_ABS;
}
}else{
plc.mCode.finish = 1;
}
break;
default:
plc.mCode.finish = 1;
break;
}
}
if(plc.emgStopFlag){
plc.emgStopFlag = 0;
if(plc.mCode.flag){
plc.mCode.finish = 1;
}
if(plc.sCode.flag){
plc.sCode.finish = 1;
}
if(plc.tCode.flag){
plc.tCode.finish = 1;
}
}
}
void PLCAPI plcClose(Status& sts, PlcBlock& plc)
{
}