From ccd0c53ed3e7f5faf79be8bf1d628fdd304b9c19 Mon Sep 17 00:00:00 2001 From: Anders Sandstrom Date: Wed, 2 Feb 2022 13:54:25 +0100 Subject: [PATCH] WIP --- ecmc_plugin_grbl/ecmcGrbl.cpp | 62 +++++++++++++++-------------------- ecmc_plugin_grbl/ecmcGrbl.h | 1 - iocsh/cfg/el7201_mod.ax | 3 +- iocsh/plc/grbl.plc | 13 +++++--- iocsh/test.script | 2 +- 5 files changed, 38 insertions(+), 43 deletions(-) diff --git a/ecmc_plugin_grbl/ecmcGrbl.cpp b/ecmc_plugin_grbl/ecmcGrbl.cpp index c529925..871ca10 100644 --- a/ecmc_plugin_grbl/ecmcGrbl.cpp +++ b/ecmc_plugin_grbl/ecmcGrbl.cpp @@ -108,7 +108,6 @@ ecmcGrbl::ecmcGrbl(char* configStr, cfgAutoEnable_ = 0; grblInitDone_ = 0; autoStartDone_ = 0; - autoEnableExecuted_ = 0; timeToNextExeMs_ = 0; writerBusy_ = 0; spindleAcceleration_ = 0; @@ -125,11 +124,7 @@ ecmcGrbl::ecmcGrbl(char* configStr, parseConfigStr(configStr); // Assigns all configs - // simulate auto enable - if(!cfgAutoEnable_) { - autoEnableExecuted_ = 1; - } - + ecmcData_.xAxis.axisId = cfgXAxisId_; ecmcData_.yAxis.axisId = cfgYAxisId_; ecmcData_.zAxis.axisId = cfgZAxisId_; @@ -263,15 +258,14 @@ void ecmcGrbl::doWriteWorker() { } // wait for epics state && auto enable at start - while(getEcmcEpicsIOCState()!=16 || !autoEnableExecuted_) { + while(getEcmcEpicsIOCState()!=16) { delay_ms(2); } // GRBL ready, now we can send comamnds for(;;) { if( (grblCommandBuffer_.size() > grblCommandBufferIndex_) && - executeCmd_) { - + executeCmd_ && ecmcData_.allEnabled && grblInitDone_) { epicsMutexLock(grblCommandBufferMutex_); std::string commandRaw = grblCommandBuffer_[grblCommandBufferIndex_]; epicsMutexUnlock(grblCommandBufferMutex_); @@ -340,14 +334,14 @@ void ecmcGrbl::doWriteWorker() { } } } - // All rows written - //if(grblCommandBufferIndex_ == grblCommandBuffer_.size()) { - // writerBusy_ = 0; - //} grblCommandBufferIndex_++; } else { - writerBusy_ = 0; + if( (( grblCommandBufferIndex_ >= grblCommandBuffer_.size()) || !executeCmd_ ) && + grblInitDone_) { + writerBusy_ = 0; + } + // Wait for right condition to start delay_ms(5); } @@ -511,7 +505,6 @@ void ecmcGrbl::preExeAxes() { autoEnableAxis(ecmcData_.spindleAxis); if(ecmcData_.allEnabled) { - autoEnableExecuted_ = 1; autoEnableTimeOutCounter_ = 0; } else { if(cfgAutoEnable_ && !errorCode_) { @@ -541,7 +534,8 @@ void ecmcGrbl::preExeAxis(ecmcAxisStatusData ecmcAxisData, int grblAxisId) { void ecmcGrbl::giveControlToEcmcIfNeeded() { // Give total control to ecmc at negative edge of any limit switch - if(!ecmcData_.allLimitsOK && ecmcData_.allLimitsOKOld) { + if( (!ecmcData_.allLimitsOK && ecmcData_.allLimitsOKOld) || + (!ecmcData_.errorOld && ecmcData_.error) ) { if(ecmcData_.xAxis.axisId >= 0) { if(ecmcData_.xAxis.trajSource == ECMC_DATA_SOURCE_EXTERNAL) { @@ -578,8 +572,7 @@ void ecmcGrbl::giveControlToEcmcIfNeeded() { errorCode_ = ECMC_PLUGIN_LIMIT_SWITCH_VIOLATION_ERROR_CODE; // Also reset for safety (avoid autoenable) - autoEnableExecuted_ = 1; - cfgAutoStart_ = 0; + cfgAutoStart_ = 0; } } @@ -703,14 +696,18 @@ int ecmcGrbl::grblRTexecute(int ecmcError) { setReset(1); } } + // Stop spindle + if(ecmcData_.spindleAxis.axisId >= 0) { + setAxisTargetVel(ecmcData_.spindleAxis.axisId, 0); + stopMotion(ecmcData_.spindleAxis.axisId,0); + } + setExecute(0); printf("GRBL: ERROR: ecmc 0x%x, plugin 0x%x\n",ecmcError,errorCode_); - errorCodeOld_ = errorCode_; - return errorCode_; - } + errorCodeOld_ = errorCode_; - if(errorCodeOld_) { - writerBusy_ = false; + giveControlToEcmcIfNeeded(); + return errorCode_; } // auto start @@ -726,7 +723,7 @@ int ecmcGrbl::grblRTexecute(int ecmcError) { preExeAxes(); double sampleRateMs = 0.0; - if(grblInitDone_ && autoEnableExecuted_) { + if(grblInitDone_ && ecmcData_.allEnabled) { while(timeToNextExeMs_ < exeSampleTimeMs_ && sampleRateMs >= 0) { sampleRateMs = ecmc_grbl_main_rt_thread(); if(sampleRateMs > 0){ @@ -739,7 +736,6 @@ int ecmcGrbl::grblRTexecute(int ecmcError) { timeToNextExeMs_-= exeSampleTimeMs_; } } - //update setpoints postExeAxes(); return errorCode_; @@ -760,23 +756,18 @@ void ecmcGrbl::postExeAxes() { if(cfgSpindleAxisId_ >= 0) { setAxisTargetVel(cfgSpindleAxisId_,(double)sys.spindle_speed); if(sys.spindle_speed!=0) { - int errorCode = moveVelocity(cfgSpindleAxisId_, - (double)sys.spindle_speed, - ecmcData_.spindleAxis.acceleration, - ecmcData_.spindleAxis.acceleration); - if (errorCode) { - errorCode_ = errorCode; - } + moveVelocity(cfgSpindleAxisId_, + (double)sys.spindle_speed, + ecmcData_.spindleAxis.acceleration, + ecmcData_.spindleAxis.acceleration); } } } // trigg start of g-code int ecmcGrbl::setExecute(int exe) { - if(!exe) { - writerBusy_ = 0; + if(!exe) { autoEnableTimeOutCounter_ = 0; - autoEnableExecuted_ = 0; } if(!executeCmd_ && exe) { @@ -807,6 +798,7 @@ int ecmcGrbl::setReset(int reset) { if(!resetCmd_ && reset) { mc_reset(); } + grblInitDone_ = 0; resetCmd_ = reset; return 0; } diff --git a/ecmc_plugin_grbl/ecmcGrbl.h b/ecmc_plugin_grbl/ecmcGrbl.h index 90788a0..6f5aaa1 100644 --- a/ecmc_plugin_grbl/ecmcGrbl.h +++ b/ecmc_plugin_grbl/ecmcGrbl.h @@ -116,7 +116,6 @@ class ecmcGrbl : public asynPortDriver { unsigned int grblCommandBufferIndex_; epicsMutexId grblCommandBufferMutex_; bool autoStartDone_; - int autoEnableExecuted_; int grblExeCycles_; double timeToNextExeMs_; bool writerBusy_; diff --git a/iocsh/cfg/el7201_mod.ax b/iocsh/cfg/el7201_mod.ax index 885cc2f..a58e1a2 100644 --- a/iocsh/cfg/el7201_mod.ax +++ b/iocsh/cfg/el7201_mod.ax @@ -1,6 +1,5 @@ #General -epicsEnvSet("ECMC_MOTOR_NAME", "Axis1") -epicsEnvSet("ECMC_R", "Axis1-") +epicsEnvSet("ECMC_MOTOR_NAME", "Axis3") epicsEnvSet("ECMC_AXIS_NO", "3") epicsEnvSet("ECMC_DESC", "EL7201 Test") epicsEnvSet("ECMC_EGU", "mm") # Motor Record Unit diff --git a/iocsh/plc/grbl.plc b/iocsh/plc/grbl.plc index 175551e..d23ded3 100644 --- a/iocsh/plc/grbl.plc +++ b/iocsh/plc/grbl.plc @@ -2,14 +2,19 @@ # Test of GRBL plugin for g-code parsing # -println('GRBL busy :', grbl_get_busy()); -println('Parser busy:', grbl_get_parser_busy()); -println('Error:', grbl_get_error()); +println('GRBL busy :', grbl_get_busy()); +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); if(grbl_get_error()) { println('Try reset'); grbl_reset_error(); - ec_err_rst(); + #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); diff --git a/iocsh/test.script b/iocsh/test.script index 9c94562..ae63f98 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=0;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=1;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)