WIP

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());
 }

ecmc_plugin_grbl/ecmcGrbl.h

@@ -22,7 +22,7 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <unistd.h>
-#include <queue>
+#include <vector>
 #include <string.h>
 
 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<std::string> grblCommandBuffer_;
+  std::vector<std::string> grblCommandBuffer_;
+  int                   grblCommandBufferIndex_;
   epicsMutexId          grblCommandBufferMutex_;
   bool                  firstCommandWritten_;
   int                   autoEnableExecuted_;
-  int                   grblExeCycles_;
+  int                   grblExeCycles_;  
   double timeToNextExeMs_;
 };
 

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"

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;
 }

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();

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.

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");