From ec59924d0e50f42ebb0229b141f07058fdffc369 Mon Sep 17 00:00:00 2001 From: Anders Sandstrom Date: Wed, 2 Feb 2022 15:29:40 +0100 Subject: [PATCH] WIP --- ecmc_plugin_grbl/ecmcGrbl.cpp | 41 ++++++++++++---------- ecmc_plugin_grbl/ecmcGrbl.h | 1 + ecmc_plugin_grbl/ecmcGrblWrap.cpp | 13 +++++++ ecmc_plugin_grbl/ecmcGrblWrap.h | 8 +++++ ecmc_plugin_grbl/ecmcPluginGrbl.c | 56 ++++++++++++++++++++++++++++++- iocsh/cfg/el7201_mod.ax | 4 +-- iocsh/plc/grbl.plc | 50 +++++++++++++++++++++------ iocsh/test.script | 2 +- 8 files changed, 142 insertions(+), 33 deletions(-) diff --git a/ecmc_plugin_grbl/ecmcGrbl.cpp b/ecmc_plugin_grbl/ecmcGrbl.cpp index 871ca10..4ef6be7 100644 --- a/ecmc_plugin_grbl/ecmcGrbl.cpp +++ b/ecmc_plugin_grbl/ecmcGrbl.cpp @@ -431,23 +431,27 @@ void ecmcGrbl::doMainWorker() { } } -int ecmcGrbl::setAllAxesEnable(int enable) { +int ecmcGrbl::setAllAxesEnable(int enable) { - if(cfgXAxisId_ >= 0) { - setAxisEnable(cfgXAxisId_, enable); + if(ecmcData_.xAxis.axisId >= 0) { + setAxisEnable(ecmcData_.xAxis.axisId, enable); } - if(cfgYAxisId_ >= 0 ) { - setAxisEnable(cfgYAxisId_, enable); + if(ecmcData_.yAxis.axisId >= 0 ) { + setAxisEnable(ecmcData_.yAxis.axisId, enable); } - if(cfgZAxisId_ >=0 ) { - setAxisEnable(cfgZAxisId_, enable); + if(ecmcData_.zAxis.axisId >= 0 ) { + setAxisEnable(ecmcData_.zAxis.axisId, enable); } - if(cfgSpindleAxisId_ >= 0) { - setAxisEnable(cfgSpindleAxisId_, enable); + if(ecmcData_.spindleAxis.axisId >= 0) { + setAxisEnable(ecmcData_.spindleAxis.axisId, enable); } return 0; } +int ecmcGrbl::getAllAxesEnabled() { + return ecmcData_.allEnabled; +} + void ecmcGrbl::autoEnableAxis(ecmcAxisStatusData ecmcAxisData) { if(!cfgAutoEnable_ || getEcmcEpicsIOCState()!=16 || errorCode_ || ecmcAxisData.axisId < 0) { @@ -676,7 +680,7 @@ void ecmcGrbl::readEcmcStatus(int ecmcError) { // grb realtime thread!!! int ecmcGrbl::grblRTexecute(int ecmcError) { - + if(getEcmcEpicsIOCState()!=16 || !grblInitDone_) { return 0; } @@ -690,12 +694,13 @@ int ecmcGrbl::grblRTexecute(int ecmcError) { setHalt(0); setHalt(1); - if(ecmcData_.error != errorCode_) { // ecmc error then reset - if(ecmcData_.error > 0 && ecmcData_.errorOld == 0) { - setReset(0); - setReset(1); - } - } + //if(ecmcData_.error != errorCode_) { // ecmc error then reset + // if(ecmcData_.error > 0 && ecmcData_.errorOld == 0) { + // setReset(0); + // setReset(1); + // } + //} + // Stop spindle if(ecmcData_.spindleAxis.axisId >= 0) { setAxisTargetVel(ecmcData_.spindleAxis.axisId, 0); @@ -753,8 +758,8 @@ void ecmcGrbl::postExeAxes() { postExeAxis(ecmcData_.zAxis,Z_AXIS); - if(cfgSpindleAxisId_ >= 0) { - setAxisTargetVel(cfgSpindleAxisId_,(double)sys.spindle_speed); + if(ecmcData_.spindleAxis.axisId >= 0) { + setAxisTargetVel(ecmcData_.spindleAxis.axisId,(double)sys.spindle_speed); if(sys.spindle_speed!=0) { moveVelocity(cfgSpindleAxisId_, (double)sys.spindle_speed, diff --git a/ecmc_plugin_grbl/ecmcGrbl.h b/ecmc_plugin_grbl/ecmcGrbl.h index 6f5aaa1..9eb1bee 100644 --- a/ecmc_plugin_grbl/ecmcGrbl.h +++ b/ecmc_plugin_grbl/ecmcGrbl.h @@ -79,6 +79,7 @@ class ecmcGrbl : public asynPortDriver { int setAllAxesEnable(int enable); int getError(); void resetError(); + int getAllAxesEnabled(); private: void parseConfigStr(char *configStr); diff --git a/ecmc_plugin_grbl/ecmcGrblWrap.cpp b/ecmc_plugin_grbl/ecmcGrblWrap.cpp index 793c410..a1aa43d 100644 --- a/ecmc_plugin_grbl/ecmcGrblWrap.cpp +++ b/ecmc_plugin_grbl/ecmcGrblWrap.cpp @@ -134,6 +134,19 @@ int resetError() { return 0; } +int setAllAxesEnable(int enable) { + if(grbl){ + grbl->setAllAxesEnable(enable); + } + return 0; +} + +int getAllAxesEnabled() { + if(grbl){ + return grbl->getAllAxesEnabled(); + } + return 0; +} void deleteGrbl() { if(grbl) { diff --git a/ecmc_plugin_grbl/ecmcGrblWrap.h b/ecmc_plugin_grbl/ecmcGrblWrap.h index 73403d4..77a3c5d 100644 --- a/ecmc_plugin_grbl/ecmcGrblWrap.h +++ b/ecmc_plugin_grbl/ecmcGrblWrap.h @@ -74,6 +74,14 @@ int getError(); */ int resetError(); +/** \brief get all configured axes enabled\n + */ +int getAllAxesEnabled(); + +/** \brief set all enable of all configured axes\n + */ +int setAllAxesEnable(int enable); + // Delete object void deleteGrbl(); diff --git a/ecmc_plugin_grbl/ecmcPluginGrbl.c b/ecmc_plugin_grbl/ecmcPluginGrbl.c index 0291c69..2401a00 100644 --- a/ecmc_plugin_grbl/ecmcPluginGrbl.c +++ b/ecmc_plugin_grbl/ecmcPluginGrbl.c @@ -142,6 +142,14 @@ double grbl_get_error() { return getError(); } +double grbl_get_all_enabled() { + return getAllAxesEnabled(); +} + +double grbl_set_all_enable(double enable) { + return setAllAxesEnable(enable); +} + // Register data for plugin so ecmc know what to use struct ecmcPluginData pluginDataDef = { // Allways use ECMC_PLUG_VERSION_MAGIC @@ -379,8 +387,54 @@ struct ecmcPluginData pluginDataDef = { .funcArg10 = NULL, .funcGenericObj = NULL, }, + .funcs[9] = + { /*----grbl_get_all_enabled----*/ + // Function name (this is the name you use in ecmc plc-code) + .funcName = "grbl_get_all_enabled", + // Function description + .funcDesc = "double grbl_get_all_enabled() : Get all configured axes enabled.", + /** + * 7 different prototypes allowed (only doubles since reg in plc). + * Only funcArg${argCount} func shall be assigned the rest set to NULL. + **/ + .funcArg0 = grbl_get_all_enabled, + .funcArg1 = NULL, + .funcArg2 = NULL, + .funcArg3 = NULL, + .funcArg4 = NULL, + .funcArg5 = NULL, + .funcArg6 = NULL, + .funcArg7 = NULL, + .funcArg8 = NULL, + .funcArg9 = NULL, + .funcArg10 = NULL, + .funcGenericObj = NULL, + }, + .funcs[10] = + { /*----grbl_set_all_enable----*/ + // Function name (this is the name you use in ecmc plc-code) + .funcName = "grbl_set_all_enable", + // Function description + .funcDesc = "double grbl_set_all_enable(enable) : Set enable on all configured axes.", + /** + * 7 different prototypes allowed (only doubles since reg in plc). + * Only funcArg${argCount} func shall be assigned the rest set to NULL. + **/ + .funcArg0 = NULL, + .funcArg1 = grbl_set_all_enable, + .funcArg2 = NULL, + .funcArg3 = NULL, + .funcArg4 = NULL, + .funcArg5 = NULL, + .funcArg6 = NULL, + .funcArg7 = NULL, + .funcArg8 = NULL, + .funcArg9 = NULL, + .funcArg10 = NULL, + .funcGenericObj = NULL, + }, - .funcs[9] = {0}, // last element set all to zero.. + .funcs[11] = {0}, // last element set all to zero.. // PLC consts .consts[0] = {0}, // last element set all to zero.. }; diff --git a/iocsh/cfg/el7201_mod.ax b/iocsh/cfg/el7201_mod.ax index a58e1a2..7b4c11e 100644 --- a/iocsh/cfg/el7201_mod.ax +++ b/iocsh/cfg/el7201_mod.ax @@ -6,7 +6,7 @@ epicsEnvSet("ECMC_EGU", "mm") # Motor Re epicsEnvSet("ECMC_PREC", "3") # Motor Record Precision epicsEnvSet("ECMC_AXISCONFIG", "") # Extra parameters to driver epicsEnvSet("ECMC_EC_AXIS_HEALTH", "") # Entry for axis health output (example: ec0.s1.binaryOutput01.0) -epicsEnvSet("ECMC_MOD_RANGE" , "0") # Modulo range (traj setpoints and encoder values will be in range 0..ECMC_MOD_RANGE) +epicsEnvSet("ECMC_MOD_RANGE" , "360") # Modulo range (traj setpoints and encoder values will be in range 0..ECMC_MOD_RANGE) epicsEnvSet("ECMC_MOD_TYPE", "0") # For positioning and MOD_RANGE!=: 0 = Normal, 1 = Always Fwd, 2 = Always Bwd, 3 = Closest Distance #Encoder @@ -47,7 +47,7 @@ epicsEnvSet("ECMC_VELO", "1000.0") epicsEnvSet("ECMC_JOG_VEL", "3600") epicsEnvSet("ECMC_JAR", "0.0") # JAR defaults to VELO/ACCL epicsEnvSet("ECMC_ACCS_EGU_PER_S2", "100") -epicsEnvSet("ECMC_EMERG_DECEL", "1000") # Emergency deceleration +epicsEnvSet("ECMC_EMERG_DECEL", "100") # Emergency deceleration epicsEnvSet("ECMC_JERK", "1000") # Only valid for ECMC_TRAJ_TYPE==1 #Homing diff --git a/iocsh/plc/grbl.plc b/iocsh/plc/grbl.plc index d23ded3..d321462 100644 --- a/iocsh/plc/grbl.plc +++ b/iocsh/plc/grbl.plc @@ -7,17 +7,45 @@ println('Parser busy :', grbl_get_parser_busy()); println('Error :', grbl_get_error()); println('Row :', grbl_get_code_row_num()); println('Ecmc error :', ax1.error, ax2.error, ax3.error); +println('All Enabled :', grbl_get_all_enabled()); +ax1.reset:=0; +ax2.reset:=0; +ax3.reset:=0; -if(grbl_get_error()) { - println('Try reset'); - grbl_reset_error(); - #ax1.reset:=1; - #ax2.reset:=1; - #ax3.reset:=1; -} else if(not(grbl_get_parser_busy())) { - static.counter+=1; - println('Retrigger g-code. Counter: ', static.counter); - grbl_set_execute(0); - grbl_set_execute(1); +# Enable axis in state 0 +if(static.state==0) { + if(not(grbl_get_busy()) and not(grbl_get_error())) { + grbl_set_all_enable(1); + }; + if(grbl_get_all_enabled()) { + static.state:=1; + }; }; + +# Trigg motion +if(static.state == 1) { + if(not(grbl_get_parser_busy())) { + static.counter+=1; + println('Retrigger g-code. Counter: ', static.counter); + grbl_set_execute(0); + grbl_set_execute(1); + }; +}; + + +#if(grbl_get_error()) { +# println('Try reset'); +# grbl_reset_error(); +# grbl_mc_resume(0); +# grbl_mc_resume(1); +# ax1.reset:=1; +# ax2.reset:=1; +# ax3.reset:=1; +# +#} else if(not(grbl_get_parser_busy())) { +# #static.counter+=1; +# #println('Retrigger g-code. Counter: ', static.counter); +# #grbl_set_execute(0); +# #grbl_set_execute(1); +#}; diff --git a/iocsh/test.script b/iocsh/test.script index ae63f98..d8ed902 100644 --- a/iocsh/test.script +++ b/iocsh/test.script @@ -82,7 +82,7 @@ epicsEnvSet("PLUGIN_VER" ,"develop") require ecmc_plugin_grbl $(PLUGIN_VER) epicsEnvSet(ECMC_PLUGIN_FILNAME,"/home/pi/epics/base-7.0.5/require/${E3_REQUIRE_VERSION}/siteMods/ecmc_plugin_grbl/$(PLUGIN_VER)/lib/${EPICS_HOST_ARCH=linux-x86_64}/libecmc_plugin_grbl.so") -epicsEnvSet(ECMC_PLUGIN_CONFIG,"DBG_PRINT=1;X_AXIS=1;Y_AXIS=2;SPINDLE_AXIS=3;AUTO_ENABLE=1;AUTO_START=0;") # Only one option implemented in this plugin +epicsEnvSet(ECMC_PLUGIN_CONFIG,"DBG_PRINT=1;X_AXIS=1;Y_AXIS=2;SPINDLE_AXIS=3;AUTO_ENABLE=0;AUTO_START=0;") # Only one option implemented in this plugin ${SCRIPTEXEC} ${ecmccfg_DIR}loadPlugin.cmd, "PLUGIN_ID=0,FILE=${ECMC_PLUGIN_FILNAME},CONFIG='${ECMC_PLUGIN_CONFIG}', REPORT=1" epicsEnvUnset(ECMC_PLUGIN_FILNAME) epicsEnvUnset(ECMC_PLUGIN_CONFIG)