Wait for reply before send new command. (WIP)

2022-01-25 15:56:46 +01:00
parent fc194a029b
commit 634b147a56
4 changed files with 1032 additions and 2598 deletions
--- a/ecmc_plugin_grbl/ecmcGrbl.cpp
+++ b/ecmc_plugin_grbl/ecmcGrbl.cpp
@@ -27,6 +27,7 @@ extern "C" {
 #include "grbl.h"
 }

+int enableDebugPrintouts = 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.
@@ -39,8 +40,6 @@ volatile uint8_t sys_rt_exec_accessory_override; // Global realtime executor bit
  volatile uint8_t sys_rt_exec_debug;
 #endif

-int enableDebugPrintouts = 0;
-
 // Start worker for socket read()
 void f_worker_read(void *obj) {
  if(!obj) {
@@ -227,18 +226,37 @@ void ecmcGrbl::doReadWorker() {
  // simulate serial connection here (need mutex)
  printf("%s:%s:%d\n",__FILE__,__FUNCTION__,__LINE__);
  for(;;) {
-    while(serial_get_tx_buffer_count()==0) {
-      delay_ms(1);      
-    }
-    printf("%c",ecmc_get_char_from_grbl_tx_buffer());
+    //while(serial_get_tx_buffer_count()==0) {
+    //  delay_ms(1);      
+    //}
+    //printf("%c",ecmc_get_char_from_grbl_tx_buffer());
+    delay_ms(100);
  }
 }

 // Write socket worker
 void ecmcGrbl::doWriteWorker() {
  // simulate serial connection here (need mutex)
+  std::string reply = "";
  printf("%s:%s:%d\n",__FILE__,__FUNCTION__,__LINE__);
+  
+  // Wait for grbl startup string before send comamnds"['$' for help]" 
+  // basically flush buffer
  for(;;) {
+    while(serial_get_tx_buffer_count()==0) {
+      delay_ms(1);
+    }
+    char c = ecmc_get_char_from_grbl_tx_buffer();
+    reply += c;
+    if(c == '\n' && 
+       reply.find(ECMC_PLUGIN_GRBL_GRBL_STARTUP_STRING) != std::string::npos ) {
+       printf("GRBL READY FOR COMMANDS: %s\n",reply.c_str());
+       break;
+    }
+  }
+  
+  // GRBL ready, now we can send comamnds
+  for(;;) {  
    if(grblCommandBuffer_.size()>0 && getEcmcEpicsIOCState()==16 && autoEnableExecuted_) {
      //printf("%s:%s:%d: Command in buffer!!!\n",__FILE__,__FUNCTION__,__LINE__);
      epicsMutexLock(grblCommandBufferMutex_);
@@ -247,12 +265,29 @@ void ecmcGrbl::doWriteWorker() {
      epicsMutexUnlock(grblCommandBufferMutex_);
      //printf("%s:%s:%d: Command length %d!!!\n",__FILE__,__FUNCTION__,__LINE__,strlen(command.c_str()));
      //printf("%s:%s:%d: Available bytes %d!!!\n",__FILE__,__FUNCTION__,__LINE__,serial_get_rx_buffer_available());
-      // wait for grbl      
+      // wait for grbl            
      while(serial_get_rx_buffer_available() <= strlen(command.c_str())+1) {
-        delay_ms(2);
+        delay_ms(1);
+      }
+      printf("Writing command: %s\n",command.c_str());
+      ecmc_write_command_serial(strdup(command.c_str()));      
+      reply = "";
+
+      // Wait for reply!
+      for(;;) {
+        while(serial_get_tx_buffer_count()==0) {
+          delay_ms(1);      
+        }
+        char c = ecmc_get_char_from_grbl_tx_buffer();
+        reply += c;
+        if(c == '\n'&& reply.length() > 1) {
+          printf("Reply from grbl %s\n",reply.c_str());
+          //#define ECMC_PLUGIN_GRBL_GRBL_OK_STRING "ok"
+          //#define ECMC_PLUGIN_GRBL_GRBL_ERR_STRING "err"
+          //Example "error:2"
+          break;
+        }
      }
-      ecmc_write_command_serial(strdup(command.c_str()));
-      //printf("%s:%s:%d: Writing!!\n",__FILE__,__FUNCTION__,__LINE__);
    }
    else {
      delay_ms(5);
--- a/ecmc_plugin_grbl/ecmcGrblDefs.h
+++ b/ecmc_plugin_grbl/ecmcGrblDefs.h
@@ -24,5 +24,8 @@
 #define ECMC_PLUGIN_ASYN_PREFIX          "plugin.grbl"

 #define ECMC_PLUGIN_GRBL_GENERAL_ERROR_CODE 1
+#define ECMC_PLUGIN_GRBL_GRBL_STARTUP_STRING "for help]"
+#define ECMC_PLUGIN_GRBL_GRBL_OK_STRING "ok"
+#define ECMC_PLUGIN_GRBL_GRBL_ERR_STRING "err"

 #endif  /* ECMC_GRBL_DEFS_H_ */
--- a/iocsh/log.log
+++ b/iocsh/log.log
--- a/iocsh/test.script
+++ b/iocsh/test.script
@@ -64,126 +64,129 @@ ${SCRIPTEXEC} ${ecmccfg_DIR}loadPlugin.cmd, "PLUGIN_ID=0,FILE=${ECMC_PLUGIN_FILN
 epicsEnvUnset(ECMC_PLUGIN_FILNAME)
 epicsEnvUnset(ECMC_PLUGIN_CONFIG)

+ecmcGrblAddCommand("G1X20TRRY2");
 ecmcGrblAddCommand("G1X20Y20F180");
 ecmcGrblAddCommand("G4P1");
+ecmcGrblAddCommand("G1X20TRRY2");
 ecmcGrblAddCommand("G2X0Y0R20");
 ecmcGrblAddCommand("G0X10Y10");
 ecmcGrblAddCommand("G4P1");
+ecmcGrblAddCommand("G1X20TRRY2");
 ecmcGrblAddCommand("G1X10Y0F360");
 ecmcGrblAddCommand("G4P1");
 ecmcGrblAddCommand("G1X50Y50F180");
 ecmcGrblAddCommand("G4P1");
 ecmcGrblAddCommand("G1X0Y0F360");
-ecmcGrblAddCommand("G1X20Y20F180");
-ecmcGrblAddCommand("G4P1");
-ecmcGrblAddCommand("G2X0Y0R20");
-ecmcGrblAddCommand("G0X10Y10");
-ecmcGrblAddCommand("G4P1");
-ecmcGrblAddCommand("G1X10Y0F360");
-ecmcGrblAddCommand("G4P1");
-ecmcGrblAddCommand("G1X50Y50F180");
-ecmcGrblAddCommand("G4P1");
-ecmcGrblAddCommand("G1X0Y0F360");
-//ecmcGrblAddCommand("G1X20Y20F180");
-//ecmcGrblAddCommand("G4P1");
-//ecmcGrblAddCommand("G2X0Y0R20");
-//ecmcGrblAddCommand("G0X10Y10");
-//ecmcGrblAddCommand("G4P1");
-//ecmcGrblAddCommand("G1X10Y0F360");
-//ecmcGrblAddCommand("G4P1");
-//ecmcGrblAddCommand("G1X50Y50F180");
-//ecmcGrblAddCommand("G4P1");
-//ecmcGrblAddCommand("G1X0Y0F360");
-//ecmcGrblAddCommand("G1X20Y20F180");
-//ecmcGrblAddCommand("G4P1");
-//ecmcGrblAddCommand("G2X0Y0R20");
-//ecmcGrblAddCommand("G0X10Y10");
-//ecmcGrblAddCommand("G4P1");
-//ecmcGrblAddCommand("G1X10Y0F360");
-//ecmcGrblAddCommand("G4P1");
-//ecmcGrblAddCommand("G1X50Y50F180");
-//ecmcGrblAddCommand("G4P1");
-//ecmcGrblAddCommand("G1X0Y0F360");
-//ecmcGrblAddCommand("G1X20Y20F180");
-//ecmcGrblAddCommand("G4P1");
-//ecmcGrblAddCommand("G2X0Y0R20");
-//ecmcGrblAddCommand("G0X10Y10");
-//ecmcGrblAddCommand("G4P1");
-//ecmcGrblAddCommand("G1X10Y0F360");
-//ecmcGrblAddCommand("G4P1");
-//ecmcGrblAddCommand("G1X50Y50F180");
-//ecmcGrblAddCommand("G4P1");
-//ecmcGrblAddCommand("G1X0Y0F360");
-//ecmcGrblAddCommand("G1X20Y20F180");
-//ecmcGrblAddCommand("G4P1");
-//ecmcGrblAddCommand("G2X0Y0R20");
-//ecmcGrblAddCommand("G0X10Y10");
-//ecmcGrblAddCommand("G4P1");
-//ecmcGrblAddCommand("G1X10Y0F360");
-//ecmcGrblAddCommand("G4P1");
-//ecmcGrblAddCommand("G1X50Y50F180");
-//ecmcGrblAddCommand("G4P1");
-//ecmcGrblAddCommand("G1X0Y0F360");
-//ecmcGrblAddCommand("G1X20Y20F180");
-//ecmcGrblAddCommand("G4P1");
-//ecmcGrblAddCommand("G2X0Y0R20");
-//ecmcGrblAddCommand("G0X10Y10");
-//ecmcGrblAddCommand("G4P1");
-//ecmcGrblAddCommand("G1X10Y0F360");
-//ecmcGrblAddCommand("G4P1");
-//ecmcGrblAddCommand("G1X50Y50F180");
-//ecmcGrblAddCommand("G4P1");
-//ecmcGrblAddCommand("G1X0Y0F360");
-//ecmcGrblAddCommand("G1X20Y20F180");
-//ecmcGrblAddCommand("G4P1");
-//ecmcGrblAddCommand("G2X0Y0R20");
-//ecmcGrblAddCommand("G0X10Y10");
-//ecmcGrblAddCommand("G4P1");
-//ecmcGrblAddCommand("G1X10Y0F360");
-//ecmcGrblAddCommand("G4P1");
-//ecmcGrblAddCommand("G1X50Y50F180");
-//ecmcGrblAddCommand("G4P1");
-//ecmcGrblAddCommand("G1X0Y0F360");
-//ecmcGrblAddCommand("G1X20Y20F180");
-//ecmcGrblAddCommand("G4P1");
-//ecmcGrblAddCommand("G2X0Y0R20");
-//ecmcGrblAddCommand("G0X10Y10");
-//ecmcGrblAddCommand("G4P1");
-//ecmcGrblAddCommand("G1X10Y0F360");
-//ecmcGrblAddCommand("G4P1");
-//ecmcGrblAddCommand("G1X50Y50F180");
-//ecmcGrblAddCommand("G4P1");
-//ecmcGrblAddCommand("G1X0Y0F360");
-//ecmcGrblAddCommand("G1X20Y20F180");
-//ecmcGrblAddCommand("G4P1");
-//ecmcGrblAddCommand("G2X0Y0R20");
-//ecmcGrblAddCommand("G0X10Y10");
-//ecmcGrblAddCommand("G4P1");
-//ecmcGrblAddCommand("G1X10Y0F360");
-//ecmcGrblAddCommand("G4P1");
-//ecmcGrblAddCommand("G1X50Y50F180");
-//ecmcGrblAddCommand("G4P1");
-//ecmcGrblAddCommand("G1X0Y0F360");
-//ecmcGrblAddCommand("G1X20Y20F180");
-//ecmcGrblAddCommand("G4P1");
-//ecmcGrblAddCommand("G2X0Y0R20");
-//ecmcGrblAddCommand("G0X10Y10");
-//ecmcGrblAddCommand("G4P1");
-//ecmcGrblAddCommand("G1X10Y0F360");
-//ecmcGrblAddCommand("G4P1");
-//ecmcGrblAddCommand("G1X50Y50F180");
-//ecmcGrblAddCommand("G4P1");
-//ecmcGrblAddCommand("G1X0Y0F360");
-//ecmcGrblAddCommand("G1X20Y20F180");
-//ecmcGrblAddCommand("G4P1");
-//ecmcGrblAddCommand("G2X0Y0R20");
-//ecmcGrblAddCommand("G0X10Y10");
-//ecmcGrblAddCommand("G4P1");
-//ecmcGrblAddCommand("G1X10Y0F360");
-//ecmcGrblAddCommand("G4P1");
-//ecmcGrblAddCommand("G1X50Y50F180");
-//ecmcGrblAddCommand("G4P1");
-//ecmcGrblAddCommand("G1X0Y0F360");
+#ecmcGrblAddCommand("G1X20Y20F180");
+#ecmcGrblAddCommand("G4P1");
+#ecmcGrblAddCommand("G2X0Y0R20");
+#ecmcGrblAddCommand("G0X10Y10");
+#ecmcGrblAddCommand("G4P1");
+#ecmcGrblAddCommand("G1X10Y0F360");
+#ecmcGrblAddCommand("G4P1");
+#ecmcGrblAddCommand("G1X50Y50F180");
+#ecmcGrblAddCommand("G4P1");
+#ecmcGrblAddCommand("G1X0Y0F360");
+#ecmcGrblAddCommand("G1X20Y20F180");
+#ecmcGrblAddCommand("G4P1");
+#ecmcGrblAddCommand("G2X0Y0R20");
+#ecmcGrblAddCommand("G0X10Y10");
+#ecmcGrblAddCommand("G4P1");
+#ecmcGrblAddCommand("G1X10Y0F360");
+#ecmcGrblAddCommand("G4P1");
+#ecmcGrblAddCommand("G1X50Y50F180");
+#ecmcGrblAddCommand("G4P1");
+#ecmcGrblAddCommand("G1X0Y0F360");
+#ecmcGrblAddCommand("G1X20Y20F180");
+#ecmcGrblAddCommand("G4P1");
+#ecmcGrblAddCommand("G2X0Y0R20");
+#ecmcGrblAddCommand("G0X10Y10");
+#ecmcGrblAddCommand("G4P1");
+#ecmcGrblAddCommand("G1X10Y0F360");
+#ecmcGrblAddCommand("G4P1");
+#ecmcGrblAddCommand("G1X50Y50F180");
+#ecmcGrblAddCommand("G4P1");
+#ecmcGrblAddCommand("G1X0Y0F360");
+#ecmcGrblAddCommand("G1X20Y20F180");
+#ecmcGrblAddCommand("G4P1");
+#ecmcGrblAddCommand("G2X0Y0R20");
+#ecmcGrblAddCommand("G0X10Y10");
+#ecmcGrblAddCommand("G4P1");
+#ecmcGrblAddCommand("G1X10Y0F360");
+#ecmcGrblAddCommand("G4P1");
+#ecmcGrblAddCommand("G1X50Y50F180");
+#ecmcGrblAddCommand("G4P1");
+#ecmcGrblAddCommand("G1X0Y0F360");
+#ecmcGrblAddCommand("G1X20Y20F180");
+#ecmcGrblAddCommand("G4P1");
+#ecmcGrblAddCommand("G2X0Y0R20");
+#ecmcGrblAddCommand("G0X10Y10");
+#ecmcGrblAddCommand("G4P1");
+#ecmcGrblAddCommand("G1X10Y0F360");
+#ecmcGrblAddCommand("G4P1");
+#ecmcGrblAddCommand("G1X50Y50F180");
+#ecmcGrblAddCommand("G4P1");
+#ecmcGrblAddCommand("G1X0Y0F360");
+#ecmcGrblAddCommand("G1X20Y20F180");
+#ecmcGrblAddCommand("G4P1");
+#ecmcGrblAddCommand("G2X0Y0R20");
+#ecmcGrblAddCommand("G0X10Y10");
+#ecmcGrblAddCommand("G4P1");
+#ecmcGrblAddCommand("G1X10Y0F360");
+#ecmcGrblAddCommand("G4P1");
+#ecmcGrblAddCommand("G1X50Y50F180");
+#ecmcGrblAddCommand("G4P1");
+#ecmcGrblAddCommand("G1X0Y0F360");
+#ecmcGrblAddCommand("G1X20Y20F180");
+#ecmcGrblAddCommand("G4P1");
+#ecmcGrblAddCommand("G2X0Y0R20");
+#ecmcGrblAddCommand("G0X10Y10");
+#ecmcGrblAddCommand("G4P1");
+#ecmcGrblAddCommand("G1X10Y0F360");
+#ecmcGrblAddCommand("G4P1");
+#ecmcGrblAddCommand("G1X50Y50F180");
+#ecmcGrblAddCommand("G4P1");
+#ecmcGrblAddCommand("G1X0Y0F360");
+#ecmcGrblAddCommand("G1X20Y20F180");
+#ecmcGrblAddCommand("G4P1");
+#ecmcGrblAddCommand("G2X0Y0R20");
+#ecmcGrblAddCommand("G0X10Y10");
+#ecmcGrblAddCommand("G4P1");
+#ecmcGrblAddCommand("G1X10Y0F360");
+#ecmcGrblAddCommand("G4P1");
+#ecmcGrblAddCommand("G1X50Y50F180");
+#ecmcGrblAddCommand("G4P1");
+#ecmcGrblAddCommand("G1X0Y0F360");
+#ecmcGrblAddCommand("G1X20Y20F180");
+#ecmcGrblAddCommand("G4P1");
+#ecmcGrblAddCommand("G2X0Y0R20");
+#ecmcGrblAddCommand("G0X10Y10");
+#ecmcGrblAddCommand("G4P1");
+#ecmcGrblAddCommand("G1X10Y0F360");
+#ecmcGrblAddCommand("G4P1");
+#ecmcGrblAddCommand("G1X50Y50F180");
+#ecmcGrblAddCommand("G4P1");
+#ecmcGrblAddCommand("G1X0Y0F360");
+#ecmcGrblAddCommand("G1X20Y20F180");
+#ecmcGrblAddCommand("G4P1");
+#ecmcGrblAddCommand("G2X0Y0R20");
+#ecmcGrblAddCommand("G0X10Y10");
+#ecmcGrblAddCommand("G4P1");
+#ecmcGrblAddCommand("G1X10Y0F360");
+#ecmcGrblAddCommand("G4P1");
+#ecmcGrblAddCommand("G1X50Y50F180");
+#ecmcGrblAddCommand("G4P1");
+#ecmcGrblAddCommand("G1X0Y0F360");
+#ecmcGrblAddCommand("G1X20Y20F180");
+#ecmcGrblAddCommand("G4P1");
+#ecmcGrblAddCommand("G2X0Y0R20");
+#ecmcGrblAddCommand("G0X10Y10");
+#ecmcGrblAddCommand("G4P1");
+#ecmcGrblAddCommand("G1X10Y0F360");
+#ecmcGrblAddCommand("G4P1");
+#ecmcGrblAddCommand("G1X50Y50F180");
+#ecmcGrblAddCommand("G4P1");
+#ecmcGrblAddCommand("G1X0Y0F360");

 ##############################################################################
 ## PLC 0
@@ -201,7 +204,9 @@ ecmcConfigOrDie "Cfg.SetDiagAxisEnable(0)"

 $(SCRIPTEXEC) ($(ecmccfg_DIR)setAppMode.cmd)

-asynSetTraceMask(MC_CPU1, -1, 0x41)
+#asynSetTraceMask(MC_CPU1, -1, 0x41)
+# Note need to test 0x21 later..
+asynSetTraceMask(MC_CPU1, -1, 0x0)
 iocInit()

 dbpf $(IOC):Axis1.SPAM 0