From 5462f2ad5e5a61161e3deb02b2c2226d7fe1135c Mon Sep 17 00:00:00 2001
From: Anders Sandstrom <anders.sandstrom@esss.se>
Date: Sun, 30 Jan 2022 14:25:22 +0100
Subject: [PATCH] WIP

---
 ecmc_plugin_grbl/ecmcGrbl.cpp     |  4 ++--
 grbl/grbl_protocol.c              | 12 ++++++++----
 iocsh/cfg/{test.gcode => test.nc} |  1 +
 iocsh/test.script                 |  2 +-
 4 files changed, 12 insertions(+), 7 deletions(-)
 rename iocsh/cfg/{test.gcode => test.nc} (95%)

diff --git a/ecmc_plugin_grbl/ecmcGrbl.cpp b/ecmc_plugin_grbl/ecmcGrbl.cpp
index c3c9ca5..282138c 100644
--- a/ecmc_plugin_grbl/ecmcGrbl.cpp
+++ b/ecmc_plugin_grbl/ecmcGrbl.cpp
@@ -159,13 +159,13 @@ ecmcGrbl::ecmcGrbl(char* configStr,
 
   // Create worker thread for main grbl loop
   std::string threadname = "ecmc.grbl.main";
-  if(epicsThreadCreate(threadname.c_str(), epicsThreadPriorityMedium, 32768, f_worker_main, this) == NULL) {
+  if(epicsThreadCreate(threadname.c_str(), 0, 32768, f_worker_main, this) == NULL) {
     throw std::runtime_error("GRBL: ERROR: Failed create worker thread for main().");
   }
 
   // Create worker thread for write socket
   threadname = "ecmc.grbl.write";
-  if(epicsThreadCreate(threadname.c_str(), epicsThreadPriorityLow, 32768, f_worker_write, this) == NULL) {
+  if(epicsThreadCreate(threadname.c_str(), 0, 32768, f_worker_write, this) == NULL) {
     throw std::runtime_error("GRBL: ERROR: Failed create worker thread for write().");
   }
 
diff --git a/grbl/grbl_protocol.c b/grbl/grbl_protocol.c
index 86c0432..f23d34f 100644
--- a/grbl/grbl_protocol.c
+++ b/grbl/grbl_protocol.c
@@ -73,7 +73,7 @@ void protocol_main_loop()
   uint8_t line_flags = 0;
   uint8_t char_counter = 0;
   uint8_t c;
-  delay_us(100);  // added for ecmc (I think..)
+  delay_ms(1);  // added for ecmc (I think..)
   for (;;) {
     // Process one line of incoming serial data, as the data becomes available. Performs an
     // initial filtering by removing spaces and comments and capitalizing all letters.    
@@ -110,7 +110,7 @@ void protocol_main_loop()
         // Reset tracking data for next line.
         line_flags = 0;
         char_counter = 0;
-        //delay_us(100);  // added for ecmc
+        delay_us(100);  // added for ecmc
       } else {
 
         if (line_flags) {
@@ -236,7 +236,8 @@ void protocol_exec_rt_system()
         // cycles. Hard limits typically occur while unattended or not paying attention. Gives
         // the user and a GUI time to do what is needed before resetting, like killing the
         // incoming stream. The same could be said about soft limits. While the position is not
-        // lost, continued streaming could cause a serious crash if by chance it gets executed.        
+        // lost, continued streaming could cause a serious crash if by chance it gets executed.
+        delay_us(100); // added for ecmc
       } while (bit_isfalse(sys_rt_exec_state,EXEC_RESET));
     }
     system_clear_exec_alarm(); // Clear alarm
@@ -639,7 +640,10 @@ static void protocol_exec_rt_suspend()
             spindle_set_state(SPINDLE_DISABLE,0.0); // De-energize
             coolant_set_state(COOLANT_DISABLE); // De-energize
             st_go_idle(); // Disable steppers
-            while (!(sys.abort)) { protocol_exec_rt_system(); } // Do nothing until reset.
+            while (!(sys.abort)) { 
+              protocol_exec_rt_system();
+              delay_us(100); // added for ecmc
+            } // Do nothing until reset.
             return; // Abort received. Return to re-initialize.
           }    
           
diff --git a/iocsh/cfg/test.gcode b/iocsh/cfg/test.nc
similarity index 95%
rename from iocsh/cfg/test.gcode
rename to iocsh/cfg/test.nc
index 5856a0c..f7a20dd 100644
--- a/iocsh/cfg/test.gcode
+++ b/iocsh/cfg/test.nc
@@ -3,6 +3,7 @@ G4P2
 G2X0Y0R20
 G4P2
 G0X10Y10
+G2X10Y-10R10
 G4P2
 G1X0Y0
 G1X20Y20F360
diff --git a/iocsh/test.script b/iocsh/test.script
index d319bbb..af446ba 100644
--- a/iocsh/test.script
+++ b/iocsh/test.script
@@ -64,7 +64,7 @@ ${SCRIPTEXEC} ${ecmccfg_DIR}loadPlugin.cmd, "PLUGIN_ID=0,FILE=${ECMC_PLUGIN_FILN
 epicsEnvUnset(ECMC_PLUGIN_FILNAME)
 epicsEnvUnset(ECMC_PLUGIN_CONFIG)
 
-ecmcGrblLoadFile("./cfg/test.gcode",0)
+ecmcGrblLoadFile("./cfg/test.nc",0)
 
 #ecmcGrblAddCommand("G1X20Y20F360");
 #ecmcGrblAddCommand("G4P2");