motion/ecmc_plugin_grbl
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Give control to ecmc and reset grbl if engage limit.

Browse Source
This commit is contained in:
Anders Sandstrom
2022-01-27 14:52:40 +01:00
parent f4e25dacaa
commit 4d54e03b9e
6 changed files with 861 additions and 27178 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ecmc_plugin_grbl/ecmcGrbl.cpp
+102 -3
View File
@@ -21,7 +21,8 @@
#include "ecmcAsynPortDriverUtils.h"
#include "epicsThread.h"
#include "ecmcMotion.h"
#include <iostream>
#include <fstream>
extern "C" {
#include "grbl.h"
@@ -120,6 +121,8 @@ ecmcGrbl::ecmcGrbl(char* configStr,
autoEnableExecuted_ = 0;
timeToNextExeMs_ = 0;
writerBusy_ = 0;
limitsSummary_ = 0;
limitsSummaryOld_ = 0;
grblCommandBufferIndex_ = 0;
grblCommandBuffer_.clear();
@@ -291,10 +294,15 @@ void ecmcGrbl::doWriteWorker() {
for(;;) {
if( (grblCommandBuffer_.size() > grblCommandBufferIndex_) &&
executeCmd_) {
epicsMutexLock(grblCommandBufferMutex_);
std::string command = grblCommandBuffer_[grblCommandBufferIndex_];
std::string commandRaw = grblCommandBuffer_[grblCommandBufferIndex_];
epicsMutexUnlock(grblCommandBufferMutex_);
std::string command = commandRaw.substr(0, commandRaw.find(ECMC_CONFIG_FILE_COMMENT_CHAR));
if(command.length() == 0) {
continue;
}
// wait for grbl
while(serial_get_rx_buffer_available() <= strlen(command.c_str())+1) {
@@ -495,9 +503,14 @@ double ecmcGrbl::getEcmcAxisActPos(int axis) {
}
void ecmcGrbl::preExeAxes() {
limitsSummary_ = 1;
preExeAxis(cfgXAxisId_,X_AXIS);
preExeAxis(cfgYAxisId_,Y_AXIS);
preExeAxis(cfgZAxisId_,Z_AXIS);
// Kill everything if limit switch violation
giveControlToEcmcIfNeeded();
//spindle
autoEnableAxisAtStart(cfgSpindleAxisId_);
if(getAllConfiguredAxisEnabled()) {
@@ -506,8 +519,59 @@ void ecmcGrbl::preExeAxes() {
}
void ecmcGrbl::preExeAxis(int ecmcAxisId, int grblAxisId) {
if(ecmcAxisId < 0) {
return;
}
syncAxisPositionIfNotEnabled(ecmcAxisId, grblAxisId);
autoEnableAxisAtStart(ecmcAxisId);
checkLimits(ecmcAxisId);
}
void ecmcGrbl::checkLimits(int ecmcAxisId) {
int lim = 0;
getAxisLimitSwitchBwd(ecmcAxisId,&lim);
limitsSummary_ = limitsSummary_ && lim;
getAxisLimitSwitchFwd(ecmcAxisId,&lim);
limitsSummary_ = limitsSummary_ && lim;
}
void ecmcGrbl::giveControlToEcmcIfNeeded() {
// Give total control to ecmc at negitive edge of any limit switch
if(!limitsSummary_ && limitsSummaryOld_) {
printf("####################### ECMC IN CONTROL\n");
int source = ECMC_DATA_SOURCE_INTERNAL;
if(cfgXAxisId_>=0) {
getAxisTrajSource(cfgXAxisId_,&source);
if(source == ECMC_DATA_SOURCE_EXTERNAL) {
setAxisTrajSource(cfgXAxisId_,ECMC_DATA_SOURCE_INTERNAL);
}
}
if(cfgYAxisId_>=0) {
getAxisTrajSource(cfgYAxisId_,&source);
if(source == ECMC_DATA_SOURCE_EXTERNAL) {
setAxisTrajSource(cfgYAxisId_,ECMC_DATA_SOURCE_INTERNAL);
}
}
if(cfgZAxisId_>=0) {
getAxisTrajSource(cfgZAxisId_,&source);
if(source == ECMC_DATA_SOURCE_EXTERNAL) {
setAxisTrajSource(cfgZAxisId_,ECMC_DATA_SOURCE_INTERNAL);
}
}
// reset grbl
setExecute(0);
setReset(0);
errorCode_ = ECMC_PLUGIN_LIMIT_SWITCH_VIOLATION_ERROR_CODE;
// Also reset for safety
autoEnableExecuted_ = 1;
cfgAutoStart_ = 0;
cfgAutoEnableAtStart_ = 0;
}
limitsSummaryOld_ = limitsSummary_;
}
void ecmcGrbl::syncAxisPositionIfNotEnabled(int ecmcAxisId, int grblAxisId) {
@@ -657,3 +721,38 @@ void ecmcGrbl::addCommand(std::string command) {
printf("%s:%s:%d: Buffer size %d\n",__FILE__,__FUNCTION__,__LINE__,grblCommandBuffer_.size());
}
}
void ecmcGrbl::loadFile(std::string fileName, int append) {
if(cfgDbgMode_){
printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);
}
std::ifstream file;
file.open(fileName);
if (!file.good()) {
if(cfgDbgMode_){
printf("%s:%s:%d: ERROR: File not found: %s (0x%x)\n",
__FILE__,__FUNCTION__,__LINE__,fileName,ECMC_PLUGIN_LOAD_FILE_ERROR_CODE);
}
errorCode_ = ECMC_PLUGIN_LOAD_FILE_ERROR_CODE;
throw std::runtime_error("Error: File not found.");
return;
}
// Clear buffer (since not append)
if(!append) {
setExecute(0);
epicsMutexLock(grblCommandBufferMutex_);
grblCommandBuffer_.clear();
epicsMutexUnlock(grblCommandBufferMutex_);
}
std::string line, lineNoComments;
int lineNumber = 1;
int errorCode = 0;
while (std::getline(file, line)) {
if(lineNoComments.length()>0) {
addCommand(lineNoComments);
}
}
}
ecmc_plugin_grbl/ecmcGrbl.h
+7 -1
View File
@@ -43,14 +43,16 @@ class ecmcGrbl : public asynPortDriver {
void doReadWorker();
void doMainWorker();
void doWriteWorker();
void addCommand(std::string command);
void loadFile(std::string filename, int append);
int enterRT();
int grblRTexecute(int ecmcError);
void addCommand(std::string command);
int setExecute(int exe);
int setHalt(int halt);
int setResume(int resume);
int setReset(int reset);
int getBusy();
private:
void parseConfigStr(char *configStr);
void preExeAxes();
@@ -58,6 +60,8 @@ class ecmcGrbl : public asynPortDriver {
void preExeAxis(int ecmcAxisId, int grblAxisId);
void postExeAxis(int ecmcAxisId, int grblAxisId);
void autoEnableAxisAtStart(int ecmcAxisId);
void checkLimits(int ecmcAxisId);
void giveControlToEcmcIfNeeded();
bool getEcmcAxisEnabled(int ecmcAxisId);
bool getAllConfiguredAxisEnabled();
double getEcmcAxisActPos(int axis);
@@ -88,6 +92,8 @@ class ecmcGrbl : public asynPortDriver {
int grblExeCycles_;
double timeToNextExeMs_;
bool writerBusy_;
int limitsSummary_;
int limitsSummaryOld_;
};
#endif /* ECMC_GRBL_H_ */
ecmc_plugin_grbl/ecmcGrblDefs.h
+4
View File
@@ -23,10 +23,14 @@
#define ECMC_PLUGIN_AUTO_START_OPTION_CMD "AUTO_START="
#define ECMC_PLUGIN_ASYN_PREFIX "plugin.grbl"
#define ECMC_CONFIG_FILE_COMMENT_CHAR "#"
#define ECMC_PLUGIN_GRBL_GENERAL_ERROR_CODE 0x100
#define ECMC_PLUGIN_GRBL_COMMAND_ERROR_CODE 0x101
#define ECMC_PLUGIN_AXIS_AT_LIMIT_ERROR_CODE 0x102
#define ECMC_PLUGIN_LOAD_FILE_ERROR_CODE 0x103
#define ECMC_PLUGIN_LIMIT_SWITCH_VIOLATION_ERROR_CODE 0x104
#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
+58 -1
View File
@@ -163,11 +163,68 @@ static void initCallFunc_0(const iocshArgBuf *args) {
ecmcGrblAddCommand(args[0].sval);
}
/**
* EPICS iocsh shell command: ecmcGrblLoadGCodeFile
*/
void ecmcGrblLoadFilePrintHelp() {
printf("\n");
printf(" Use ecmcGrblLoadFile(<filename>,<append>)\n");
printf(" <filename> : Filename containg g-code.\n");
printf(" <append> : 0: reset grbl, clear all current commands in buffer before \n");
printf (" loading file (default).\n");
printf(" : 1: append commands in file last in buffer. (grbl is not reset)\n");
printf("\n");
}
int ecmcGrblLoadFile(const char* filename, int append) {
if(!filename) {
printf("Error: filename.\n");
ecmcGrblLoadFilePrintHelp();
return asynError;
}
if(strcmp(filename,"-h") == 0 || strcmp(filename,"--help") == 0 ) {
ecmcGrblLoadFilePrintHelp();
return asynSuccess;
}
if(!grbl) {
printf("Plugin not initialized/loaded.\n");
return asynError;
}
try {
grbl->loadFile(filename, append);
}
catch(std::exception& e) {
printf("Exception: %s. Load file command failed.\n",e.what());
return asynError;
}
return asynSuccess;
}
static const iocshArg initArg0_1 =
{ " Filename", iocshArgString };
static const iocshArg initArg1_1 =
{ " Append", iocshArgInt };
static const iocshArg *const initArgs_1[] = { &initArg0_1,
&initArg1_1};
static const iocshFuncDef initFuncDef_1 = { "ecmcGrblLoadFile", 2, initArgs_1 };
static void initCallFunc_1(const iocshArgBuf *args) {
ecmcGrblLoadFile(args[0].sval,args[1].ival);
}
///**
// * Register all functions
//*/
void ecmcGrblPluginDriverRegister(void) {
iocshRegister(&initFuncDef_0, initCallFunc_0); // ecmcCANOpenAddMaster
iocshRegister(&initFuncDef_0, initCallFunc_0); // ecmcGrblAddCommand
iocshRegister(&initFuncDef_1, initCallFunc_1); // ecmcGrblLoadFile
}
epicsExportRegistrar(ecmcGrblPluginDriverRegister);
iocsh/log.log
+686 -27170
View File
File diff suppressed because it is too large Load Diff
iocsh/test.script
+4 -3
View File
@@ -65,8 +65,9 @@ epicsEnvUnset(ECMC_PLUGIN_FILNAME)
epicsEnvUnset(ECMC_PLUGIN_CONFIG)
ecmcGrblAddCommand("G1X20Y20F180");
ecmcGrblAddCommand("G2X0Y0R20");
ecmcGrblAddCommand("G1X20Y20F360");
ecmcGrblAddCommand("G1X0Y0F360");
#ecmcGrblAddCommand("G2X0Y0R20");
#ecmcGrblAddCommand("G0X10Y10");
#ecmcGrblAddCommand("G1X0Y0");
@@ -79,7 +80,7 @@ ecmcGrblAddCommand("G2X0Y0R20");
##############################################################################
## PLC 0
$(SCRIPTEXEC) $(ecmccfg_DIR)loadPLCFile.cmd, "PLC_ID=0, SAMPLE_RATE_MS=100,FILE=./plc/grbl.plc")
#$(SCRIPTEXEC) $(ecmccfg_DIR)loadPLCFile.cmd, "PLC_ID=0, SAMPLE_RATE_MS=100,FILE=./plc/grbl.plc")
##############################################################################
############# Configure diagnostics: