diff --git a/ecmc_plugin_grbl/.vscode/settings.json b/ecmc_plugin_grbl/.vscode/settings.json index 1e94aca..fefddb1 100644 --- a/ecmc_plugin_grbl/.vscode/settings.json +++ b/ecmc_plugin_grbl/.vscode/settings.json @@ -18,6 +18,59 @@ "bitset": "cpp", "memory": "cpp", "random": "cpp", - "future": "cpp" + "future": "cpp", + "iosfwd": "cpp", + "cctype": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "csetjmp": "cpp", + "csignal": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "atomic": "cpp", + "cfenv": "cpp", + "charconv": "cpp", + "chrono": "cpp", + "cinttypes": "cpp", + "codecvt": "cpp", + "complex": "cpp", + "condition_variable": "cpp", + "cstdint": "cpp", + "cuchar": "cpp", + "deque": "cpp", + "forward_list": "cpp", + "list": "cpp", + "unordered_map": "cpp", + "unordered_set": "cpp", + "vector": "cpp", + "exception": "cpp", + "algorithm": "cpp", + "iterator": "cpp", + "map": "cpp", + "memory_resource": "cpp", + "numeric": "cpp", + "set": "cpp", + "string": "cpp", + "string_view": "cpp", + "fstream": "cpp", + "initializer_list": "cpp", + "iomanip": "cpp", + "iostream": "cpp", + "limits": "cpp", + "mutex": "cpp", + "new": "cpp", + "scoped_allocator": "cpp", + "shared_mutex": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "streambuf": "cpp", + "thread": "cpp", + "valarray": "cpp" } } \ No newline at end of file diff --git a/ecmc_plugin_grbl/ecmcGrbl.cpp b/ecmc_plugin_grbl/ecmcGrbl.cpp index d14e19b..7d7c1a1 100644 --- a/ecmc_plugin_grbl/ecmcGrbl.cpp +++ b/ecmc_plugin_grbl/ecmcGrbl.cpp @@ -27,7 +27,10 @@ extern "C" { #include "grbl.h" } +// Global vars int enableDebugPrintouts = 0; +int stepperInterruptEnable = 0; + system_t sys; int32_t sys_position[N_AXIS]; // Real-time machine (aka home) position vector in steps. int32_t sys_probe_position[N_AXIS]; // Last probe position in machine coordinates and steps. @@ -115,7 +118,8 @@ ecmcGrbl::ecmcGrbl(char* configStr, grblInitDone_ = 0; cfgAutoEnableAtStart_ = 0; autoEnableExecuted_ = 0; - timeToNextExeMs_ = 0; + timeToNextExeMs_ = 0; + writerBusy_ = 0; grblCommandBufferIndex_ = 0; grblCommandBuffer_.clear(); @@ -132,7 +136,7 @@ ecmcGrbl::ecmcGrbl(char* configStr, // auto execute if(cfgAutoStart_) { - executeCmd_ = 1; + setExecute(1); } // global varaible in grbl @@ -265,7 +269,7 @@ void ecmcGrbl::doWriteWorker() { // basically flush buffer for(;;) { while(serial_get_tx_buffer_count()==0) { - delay_ms(1); + delay_ms(2); } char c = ecmc_get_char_from_grbl_tx_buffer(); reply += c; @@ -277,18 +281,21 @@ void ecmcGrbl::doWriteWorker() { break; } } - + + // wait for epics state && auto enable at start + while(getEcmcEpicsIOCState()!=16 || !autoEnableExecuted_) { + delay_ms(2); + } + // GRBL ready, now we can send comamnds for(;;) { - if( (grblCommandBuffer_.size() > grblCommandBufferIndex_) && - getEcmcEpicsIOCState()==16 && - autoEnableExecuted_ && - executeCmd_) { - - epicsMutexLock(grblCommandBufferMutex_); - std::string command = grblCommandBuffer_[grblCommandBufferIndex_]; + if( (grblCommandBuffer_.size() > grblCommandBufferIndex_) && + executeCmd_) { + epicsMutexLock(grblCommandBufferMutex_); + std::string command = grblCommandBuffer_[grblCommandBufferIndex_]; epicsMutexUnlock(grblCommandBufferMutex_); + // wait for grbl while(serial_get_rx_buffer_available() <= strlen(command.c_str())+1) { delay_ms(1); @@ -298,8 +305,10 @@ void ecmcGrbl::doWriteWorker() { grblCommandBufferIndex_, command.c_str()); } - ecmc_write_command_serial(strdup(command.c_str())); - reply = ""; + + ecmc_write_command_serial(strdup(command.c_str())); + reply = ""; + // Wait for reply! for(;;) { while(serial_get_tx_buffer_count()==0) { @@ -315,6 +324,7 @@ void ecmcGrbl::doWriteWorker() { command.c_str()); } break; + } else if(reply.find(ECMC_PLUGIN_GRBL_GRBL_ERR_STRING) != std::string::npos) { if(cfgDbgMode_){ printf("GRBL Reply: ERROR (command[%d] = %s)\n", @@ -322,19 +332,38 @@ void ecmcGrbl::doWriteWorker() { command.c_str()); } errorCode_ = ECMC_PLUGIN_GRBL_COMMAND_ERROR_CODE; - // Stop Motion here!! + // stop motion + setExecute(0); + setReset(1); + setReset(0); + break; + + } else if(reply.find(ECMC_PLUGIN_GRBL_GRBL_STARTUP_STRING) != std::string::npos ) { + if(cfgDbgMode_){ + printf("GRBL READY FOR COMMANDS: %s\n",reply.c_str()); + } + // system has reset + setExecute(0); break; + } else { - // keep waiting (no break) + // keep waiting (no break) if(cfgDbgMode_){ printf("GRBL Reply: Non protocol related: %s\n",reply.c_str()); + } } } } + // All rows written + //if(grblCommandBufferIndex_ == grblCommandBuffer_.size()) { + // writerBusy_ = 0; + //} grblCommandBufferIndex_++; } else { + writerBusy_ = 0; + // Wait for right condition to start delay_ms(5); } } @@ -511,15 +540,20 @@ int ecmcGrbl::grblRTexecute(int ecmcError) { return 0; } - if((ecmcError_ == 0 && ecmcError>0) || (errorCode_>0 && errorCodeOld_ == 0)) { + if((ecmcError_ == 0 && ecmcError>0) || (errorCode_>0 && errorCodeOld_ == 0)) { setHalt(0); setHalt(1); - setReset(0); - setReset(1); + if(ecmcError != errorCode_) { // ecmc error the reset + if(ecmcError>0 && ecmcError_ == 0) { + setReset(0); + setReset(1); + } + } setExecute(0); printf("Error encountered: ecmc 0x%x, plugin 0x%x\n",ecmcError,errorCode_); ecmcError_ = ecmcError; errorCodeOld_ = errorCode_; + return errorCode_; } @@ -566,8 +600,12 @@ void ecmcGrbl::postExeAxes() { // trigg start of g-code int ecmcGrbl::setExecute(int exe) { + if(!exe) { + writerBusy_ = 0; + } if(!executeCmd_ && exe) { - // + grblCommandBufferIndex_ = 0; + writerBusy_ = 1; } executeCmd_ = exe; return 0; @@ -575,7 +613,7 @@ int ecmcGrbl::setExecute(int exe) { int ecmcGrbl::setHalt(int halt) { if(!haltCmd_ && halt) { - //suspend with a "CMD_FEED_HOLD" command maybe? + system_set_exec_state_flag(EXEC_FEED_HOLD); } haltCmd_ = halt; return 0; @@ -583,7 +621,7 @@ int ecmcGrbl::setHalt(int halt) { int ecmcGrbl::setResume(int resume) { if(!resumeCmd_ && resume) { - //hepp + system_set_exec_state_flag(EXEC_CYCLE_START); } resumeCmd_ = resume; return 0; @@ -593,10 +631,14 @@ int ecmcGrbl::setReset(int reset) { if(!resetCmd_ && reset) { mc_reset(); } - resetCmd_ = reset; + resetCmd_ = reset; return 0; } +int ecmcGrbl::getBusy() { + return getEcmcEpicsIOCState()!=16 || writerBusy_ || stepperInterruptEnable; +} + // Avoid issues with std:to_string() std::string ecmcGrbl::to_string(int value) { std::ostringstream os; diff --git a/ecmc_plugin_grbl/ecmcGrbl.h b/ecmc_plugin_grbl/ecmcGrbl.h index 7fb71b9..668a751 100644 --- a/ecmc_plugin_grbl/ecmcGrbl.h +++ b/ecmc_plugin_grbl/ecmcGrbl.h @@ -50,6 +50,7 @@ class ecmcGrbl : public asynPortDriver { int setHalt(int halt); int setResume(int resume); int setReset(int reset); + int getBusy(); private: void parseConfigStr(char *configStr); void preExeAxes(); @@ -86,6 +87,7 @@ class ecmcGrbl : public asynPortDriver { int autoEnableExecuted_; int grblExeCycles_; double timeToNextExeMs_; + bool writerBusy_; }; #endif /* ECMC_GRBL_H_ */ diff --git a/ecmc_plugin_grbl/ecmcGrblWrap.cpp b/ecmc_plugin_grbl/ecmcGrblWrap.cpp index 2a88cda..1637132 100644 --- a/ecmc_plugin_grbl/ecmcGrblWrap.cpp +++ b/ecmc_plugin_grbl/ecmcGrblWrap.cpp @@ -92,6 +92,13 @@ int setResume(int resume) { return 0; } +int getBusy() { + if(grbl){ + return grbl->getBusy(); + } + return 0; +} + int setReset(int reset) { if(grbl){ return grbl->setReset(reset); diff --git a/ecmc_plugin_grbl/ecmcGrblWrap.h b/ecmc_plugin_grbl/ecmcGrblWrap.h index ad4b3e3..6e0bb5b 100644 --- a/ecmc_plugin_grbl/ecmcGrblWrap.h +++ b/ecmc_plugin_grbl/ecmcGrblWrap.h @@ -54,6 +54,10 @@ int setResume(int resume); */ int setReset(int reset); +/** \brief get grbl busy\n + */ +int getBusy(); + // Delete object void deleteGrbl(); diff --git a/ecmc_plugin_grbl/ecmcPluginGrbl.c b/ecmc_plugin_grbl/ecmcPluginGrbl.c index d63b691..4238649 100644 --- a/ecmc_plugin_grbl/ecmcPluginGrbl.c +++ b/ecmc_plugin_grbl/ecmcPluginGrbl.c @@ -119,6 +119,12 @@ double grbl_mc_reset(double halt) { return setReset((int)halt); } + +// Plc function for reset grbl +double grbl_get_busy() { + return getBusy(); +} + // Register data for plugin so ecmc know what to use struct ecmcPluginData pluginDataDef = { // Allways use ECMC_PLUG_VERSION_MAGIC @@ -173,7 +179,7 @@ struct ecmcPluginData pluginDataDef = { .funcGenericObj = NULL, }, .funcs[1] = - { /*----can_connect----*/ + { /*----grbl_mc_halt----*/ // Function name (this is the name you use in ecmc plc-code) .funcName = "grbl_mc_halt", // Function description @@ -219,7 +225,7 @@ struct ecmcPluginData pluginDataDef = { .funcGenericObj = NULL, }, .funcs[3] = - { /*----can_connect----*/ + { /*----grbl_mc_reset----*/ // Function name (this is the name you use in ecmc plc-code) .funcName = "grbl_mc_reset", // Function description @@ -241,7 +247,31 @@ struct ecmcPluginData pluginDataDef = { .funcArg10 = NULL, .funcGenericObj = NULL, }, - .funcs[4] = {0}, // last element set all to zero.. + .funcs[4] = + { /*----can_connect----*/ + // Function name (this is the name you use in ecmc plc-code) + .funcName = "grbl_get_busy", + // Function description + .funcDesc = "double grbl_get_busy() : Get grbl system busy", + /** + * 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_busy, + .funcArg1 = NULL, + .funcArg2 = NULL, + .funcArg3 = NULL, + .funcArg4 = NULL, + .funcArg5 = NULL, + .funcArg6 = NULL, + .funcArg7 = NULL, + .funcArg8 = NULL, + .funcArg9 = NULL, + .funcArg10 = NULL, + .funcGenericObj = NULL, + }, + + .funcs[5] = {0}, // last element set all to zero.. // PLC consts .consts[0] = {0}, // last element set all to zero.. }; diff --git a/grbl/grbl_stepper.c b/grbl/grbl_stepper.c index c579165..91a33e2 100644 --- a/grbl/grbl_stepper.c +++ b/grbl/grbl_stepper.c @@ -181,8 +181,6 @@ typedef struct { } st_prep_t; static st_prep_t prep; -static bool stepperInterruptEnable = 0; - /* BLOCK VELOCITY PROFILE DEFINITION __________________________ diff --git a/grbl/grbl_system.h b/grbl/grbl_system.h index 74a6094..4f7bd82 100644 --- a/grbl/grbl_system.h +++ b/grbl/grbl_system.h @@ -152,6 +152,7 @@ extern system_t sys; // NOTE: These position variables may need to be declared as volatiles, if problems arise. extern int enableDebugPrintouts; +extern int stepperInterruptEnable; extern int32_t sys_position[N_AXIS]; // Real-time machine (aka home) position vector in steps. extern int32_t sys_probe_position[N_AXIS]; // Last probe position in machine coordinates and steps. diff --git a/iocsh/plc/grbl.plc b/iocsh/plc/grbl.plc new file mode 100644 index 0000000..8b58e67 --- /dev/null +++ b/iocsh/plc/grbl.plc @@ -0,0 +1,2 @@ + +println('GRBL busy:', grbl_get_busy()); diff --git a/iocsh/test.script b/iocsh/test.script index 44035a8..ee00908 100644 --- a/iocsh/test.script +++ b/iocsh/test.script @@ -67,10 +67,10 @@ epicsEnvUnset(ECMC_PLUGIN_CONFIG) ecmcGrblAddCommand("G1X20Y20F180"); ecmcGrblAddCommand("G2X0Y0R20"); -ecmcGrblAddCommand("G0X10Y10"); -ecmcGrblAddCommand("G2X0adadsdY0R20"); -ecmcGrblAddCommand("G1X0Y0"); +#ecmcGrblAddCommand("G0X10Y10"); +#ecmcGrblAddCommand("G1X0Y0"); +#ecmcGrblAddCommand("G2X0adadsdY0R20"); #ecmcGrblAddCommand("G1X0Y20"); #ecmcGrblAddCommand("G1X10Y0F360"); #ecmcGrblAddCommand("G4P1"); @@ -79,7 +79,7 @@ ecmcGrblAddCommand("G1X0Y0"); ############################################################################## ## PLC 0 -# $(SCRIPTEXEC) $(ecmccfg_DIR)loadPLCFile.cmd, "PLC_ID=0, SAMPLE_RATE_MS=1000,FILE=./plc/can.plc") + $(SCRIPTEXEC) $(ecmccfg_DIR)loadPLCFile.cmd, "PLC_ID=0, SAMPLE_RATE_MS=100,FILE=./plc/grbl.plc") ############################################################################## ############# Configure diagnostics: