From a42b4b62f2435cc9c848f5676472e17f62360ff5 Mon Sep 17 00:00:00 2001 From: Anders Sandstrom Date: Wed, 26 Jan 2022 09:46:16 +0100 Subject: [PATCH] WIP --- ecmc_plugin_grbl/ecmcGrbl.cpp | 41 +++++++++++++++++++------------ ecmc_plugin_grbl/ecmcGrbl.h | 10 +++++--- ecmc_plugin_grbl/ecmcGrblDefs.h | 1 + ecmc_plugin_grbl/ecmcGrblWrap.cpp | 11 +++++++-- ecmc_plugin_grbl/ecmcGrblWrap.h | 6 ++++- ecmc_plugin_grbl/ecmcPluginGrbl.c | 5 ++-- iocsh/test.script | 2 +- 7 files changed, 49 insertions(+), 27 deletions(-) diff --git a/ecmc_plugin_grbl/ecmcGrbl.cpp b/ecmc_plugin_grbl/ecmcGrbl.cpp index b34ebee..5d1427b 100644 --- a/ecmc_plugin_grbl/ecmcGrbl.cpp +++ b/ecmc_plugin_grbl/ecmcGrbl.cpp @@ -109,9 +109,9 @@ ecmcGrbl::ecmcGrbl(char* configStr, grblInitDone_ = 0; cfgAutoEnableAtStart_ = 0; autoEnableExecuted_ = 0; - timeToNextExeMs_ = 0; - //grbl default rate 30khz.. - grblExeCycles_ = 30.0/(1/exeSampleTimeMs); + timeToNextExeMs_ = 0; + grblCommandBufferIndex_ = 0; + grblCommandBuffer_.clear(); if(!(grblCommandBufferMutex_ = epicsMutexCreate())) { throw std::runtime_error("Error: Failed create mutex thread for write()."); @@ -256,11 +256,11 @@ void ecmcGrbl::doWriteWorker() { } // GRBL ready, now we can send comamnds - for(;;) { - if(grblCommandBuffer_.size()>0 && getEcmcEpicsIOCState()==16 && autoEnableExecuted_) { + for(;;) { + if(grblCommandBuffer_.size() > grblCommandBufferIndex_ && getEcmcEpicsIOCState()==16 && autoEnableExecuted_) { epicsMutexLock(grblCommandBufferMutex_); - std::string command = grblCommandBuffer_.front(); - grblCommandBuffer_.pop(); + std::string command = grblCommandBuffer_[grblCommandBufferIndex_]; + epicsMutexUnlock(grblCommandBufferMutex_); // wait for grbl while(serial_get_rx_buffer_available() <= strlen(command.c_str())+1) { @@ -269,7 +269,7 @@ void ecmcGrbl::doWriteWorker() { printf("Write command to grbl: %s\n",command.c_str()); ecmc_write_command_serial(strdup(command.c_str())); reply = ""; - + grblCommandBufferIndex_++; // Wait for reply! for(;;) { while(serial_get_tx_buffer_count()==0) { @@ -282,11 +282,12 @@ void ecmcGrbl::doWriteWorker() { printf("GRBL Reply: OK: %s\n",reply.c_str()); break; } else if(reply.find(ECMC_PLUGIN_GRBL_GRBL_ERR_STRING) != std::string::npos) { - printf("GRBL Reply: ERROR: %s\n",reply.c_str()); - + printf("GRBL Reply: ERROR: %s (%s)\n",reply.c_str(),command.c_str()); + errorCode_ = ECMC_PLUGIN_GRBL_COMMAND_ERROR_CODE; + // Stop Motion here!! break; } else { - // keep waiting + // keep waiting (no break) printf("GRBL Reply: Non protocol: %s\n",reply.c_str()); } } @@ -455,9 +456,19 @@ void ecmcGrbl::syncPositionIfNotEnabled() { } } +// prepare for rt here +int ecmcGrbl::enterRT() { + return 0; +} + // grb realtime thread!!! -void ecmcGrbl::grblRTexecute() { +int ecmcGrbl::grblRTexecute(int ecmcError) { + if(ecmcError) { + //Stop motion here!!! + return errorCode_; + } + syncPositionIfNotEnabled(); autoEnableAtStart(); @@ -478,9 +489,6 @@ void ecmcGrbl::grblRTexecute() { } // write to ecmc if(cfgXAxisId_>=0) { -// if(grblInitDone_ && autoEnableExecuted_) { -// printf("[X_AXIS]= %lf/%lf=%lf, cycles %d\n",double(sys_position[X_AXIS]),double(settings.steps_per_mm[X_AXIS]),double(sys_position[X_AXIS])/double(settings.steps_per_mm[X_AXIS]),grblExeCycles_); -// } setAxisExtSetPos(cfgXAxisId_,double(sys_position[X_AXIS])/double(settings.steps_per_mm[X_AXIS])); } if(cfgYAxisId_>=0) { @@ -492,6 +500,7 @@ void ecmcGrbl::grblRTexecute() { // if(cfgSpindleAxisId_>=0) { // setAxisTargetVel(xxx); // } + return errorCode_; } //void ecmcGrbl::setExecute(int execute) { @@ -508,7 +517,7 @@ std::string ecmcGrbl::to_string(int value) { void ecmcGrbl::addCommand(std::string command) { printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__); epicsMutexLock(grblCommandBufferMutex_); - grblCommandBuffer_.push(command.c_str()); + grblCommandBuffer_.push_back(command.c_str()); epicsMutexUnlock(grblCommandBufferMutex_); printf("%s:%s:%d: Buffer size %d\n",__FILE__,__FUNCTION__,__LINE__,grblCommandBuffer_.size()); } diff --git a/ecmc_plugin_grbl/ecmcGrbl.h b/ecmc_plugin_grbl/ecmcGrbl.h index 46e05ed..1084647 100644 --- a/ecmc_plugin_grbl/ecmcGrbl.h +++ b/ecmc_plugin_grbl/ecmcGrbl.h @@ -22,7 +22,7 @@ #include #include #include -#include +#include #include class ecmcGrbl : public asynPortDriver { @@ -43,7 +43,8 @@ class ecmcGrbl : public asynPortDriver { void doReadWorker(); void doMainWorker(); void doWriteWorker(); - void grblRTexecute(); + int enterRT(); + int grblRTexecute(int ecmcError); void addCommand(std::string command); private: @@ -65,11 +66,12 @@ class ecmcGrbl : public asynPortDriver { int errorCode_; double exeSampleTimeMs_; int grblInitDone_; - std::queue grblCommandBuffer_; + std::vector grblCommandBuffer_; + int grblCommandBufferIndex_; epicsMutexId grblCommandBufferMutex_; bool firstCommandWritten_; int autoEnableExecuted_; - int grblExeCycles_; + int grblExeCycles_; double timeToNextExeMs_; }; diff --git a/ecmc_plugin_grbl/ecmcGrblDefs.h b/ecmc_plugin_grbl/ecmcGrblDefs.h index 57c7f2b..464c3d0 100644 --- a/ecmc_plugin_grbl/ecmcGrblDefs.h +++ b/ecmc_plugin_grbl/ecmcGrblDefs.h @@ -24,6 +24,7 @@ #define ECMC_PLUGIN_ASYN_PREFIX "plugin.grbl" #define ECMC_PLUGIN_GRBL_GENERAL_ERROR_CODE 0x100 +#define ECMC_PLUGIN_GRBL_COMMAND_ERROR_CODE 0x101 #define ECMC_PLUGIN_GRBL_GRBL_STARTUP_STRING "for help]" #define ECMC_PLUGIN_GRBL_GRBL_OK_STRING "ok" #define ECMC_PLUGIN_GRBL_GRBL_ERR_STRING "error" diff --git a/ecmc_plugin_grbl/ecmcGrblWrap.cpp b/ecmc_plugin_grbl/ecmcGrblWrap.cpp index 4f0c25f..6608531 100644 --- a/ecmc_plugin_grbl/ecmcGrblWrap.cpp +++ b/ecmc_plugin_grbl/ecmcGrblWrap.cpp @@ -57,9 +57,16 @@ int createGrbl(char* configStr, int exeSampleTimeMs) { return 0; } -int execute() { +int enterRT() { if(grbl){ - grbl->grblRTexecute(); + return grbl->enterRT(); + } + return 0; +} + +int realtime(int ecmcError) { + if(grbl){ + return grbl->grblRTexecute(ecmcError); } return 0; } diff --git a/ecmc_plugin_grbl/ecmcGrblWrap.h b/ecmc_plugin_grbl/ecmcGrblWrap.h index 4bb685a..2dd15c3 100644 --- a/ecmc_plugin_grbl/ecmcGrblWrap.h +++ b/ecmc_plugin_grbl/ecmcGrblWrap.h @@ -30,9 +30,13 @@ extern "C" { */ int createGrbl(char *configStr, int exeSampleTimeMs); +/** \brief prepare for RT\n + */ +int enterRT(); + /** \brief execute from rt loop\n */ -int execute(); +int realtime(int ecmcError); // Delete object void deleteGrbl(); diff --git a/ecmc_plugin_grbl/ecmcPluginGrbl.c b/ecmc_plugin_grbl/ecmcPluginGrbl.c index bba78ce..8ea8649 100644 --- a/ecmc_plugin_grbl/ecmcPluginGrbl.c +++ b/ecmc_plugin_grbl/ecmcPluginGrbl.c @@ -81,15 +81,14 @@ void grblDestruct(void) int grblRealtime(int ecmcError) { lastEcmcError = ecmcError; - execute(); - return 0; + return realtime(ecmcError); } /** Link to data source here since all sources should be availabe at this stage * (for example ecmc PLC variables are defined only at enter of realtime) **/ int grblEnterRT(){ - return 0; + return enterRT(); } /** Optional function. diff --git a/iocsh/test.script b/iocsh/test.script index 4b32745..946f556 100644 --- a/iocsh/test.script +++ b/iocsh/test.script @@ -68,7 +68,7 @@ epicsEnvUnset(ECMC_PLUGIN_CONFIG) ecmcGrblAddCommand("G1X20Y20F180"); ecmcGrblAddCommand("G2X0Y0R20"); ecmcGrblAddCommand("G0X10Y10"); -ecmcGrblAddCommand("G2X0adadsdY0R20"); +#ecmcGrblAddCommand("G2X0adadsdY0R20"); ecmcGrblAddCommand("G1X0Y20"); ecmcGrblAddCommand("G1X10Y0F360"); ecmcGrblAddCommand("G4P1");