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

Restructure and add command to add configs

Browse Source
This commit is contained in:
Anders Sandstrom
2022-02-07 17:52:32 +01:00
parent cedc2dfa65
commit 652d4d21f5
6 changed files with 1150 additions and 1121 deletions
Download Patch File Download Diff File Expand all files Collapse all files

276
ecmc_plugin_grbl/ecmcGrbl.cpp
View File

@@ -111,17 +111,23 @@ ecmcGrbl::ecmcGrbl(char* configStr,
timeToNextExeMs_ = 0;
writerBusy_ = 0;
spindleAcceleration_ = 0;
unrecoverableError_ = 0;
cfgAutoEnableTimeOutSecs_ = ECMC_PLUGIN_AUTO_ENABLE_TIME_OUT_SEC;
autoEnableTimeOutCounter_ = 0;
grblCommandBufferIndex_ = 0;
grblCommandBuffer_.clear();
grblConfigBuffer_.clear();
memset(&ecmcData_,0,sizeof(ecmcStatusData));
if(!(grblCommandBufferMutex_ = epicsMutexCreate())) {
throw std::runtime_error("GRBL: ERROR: Failed create mutex thread for write().");
if(!(grblConfigBufferMutex_ = epicsMutexCreate())) {
throw std::runtime_error("GRBL: ERROR: Failed create mutex config buffer.");
}
if(!(grblCommandBufferMutex_ = epicsMutexCreate())) {
throw std::runtime_error("GRBL: ERROR: Failed create mutex for command buffer.");
}
parseConfigStr(configStr); // Assigns all configs
@@ -240,112 +246,168 @@ void ecmcGrbl::doWriteWorker() {
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) {
// Wait for grbl startup string before send comamnds"['$' for help]"
// basically flush buffer
if(cfgDbgMode_){
printf("GRBL: INFO: Wait for startup\n");
}
while (grblReadReply() != ECMC_GRBL_REPLY_START) {
delay_ms(2);
}
if(cfgDbgMode_){
printf("GRBL: INFO: Ready for commands\n");
}
if(cfgDbgMode_){
printf("GRBL: INFO: Wait for IOC state RUN \n");
}
// wait for epics state
while(getEcmcEpicsIOCState()!=16) {
delay_ms(2);
}
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 ) {
if(cfgDbgMode_){
printf("GRBL: INFO: Ready for commands: %s\n",reply.c_str());
}
break;
}
}
// wait for epics state && auto enable at start
while(getEcmcEpicsIOCState()!=16) {
delay_ms(2);
}
if(cfgDbgMode_){
printf("GRBL: INFO: Configuration start\n");
}
// GRBL ready, now we can send comamnds
for(;;) {
if( (grblCommandBuffer_.size() > grblCommandBufferIndex_) &&
executeCmd_ && ecmcData_.allEnabled && grblInitDone_) {
epicsMutexLock(grblCommandBufferMutex_);
std::string commandRaw = grblCommandBuffer_[grblCommandBufferIndex_];
epicsMutexUnlock(grblCommandBufferMutex_);
// Apply configs
int index = 0;
while(index < grblConfigBuffer_.size()) {
epicsMutexLock(grblConfigBufferMutex_);
std::string commandRaw = grblConfigBuffer_[index];
epicsMutexUnlock(grblConfigBufferMutex_);
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) {
delay_ms(1);
}
if(cfgDbgMode_){
printf("GRBL: INFO: Write command (command[%d] = %s)\n",
grblCommandBufferIndex_,
command.c_str());
}
//Write command (will block untill written)
grblWriteCommand(command);
ecmc_write_command_serial(strdup(command.c_str()));
reply = "";
// will block untill answer
grblReplyType replyStat = grblReadReply();
// 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) {
if(reply.find(ECMC_PLUGIN_GRBL_GRBL_OK_STRING) != std::string::npos) {
if(cfgDbgMode_){
printf("GRBL: INFO: Reply OK (command[%d] = %s)\n",
grblCommandBufferIndex_,
command.c_str());
}
break;
} else if(reply.find(ECMC_PLUGIN_GRBL_GRBL_ERR_STRING) != std::string::npos) {
if(cfgDbgMode_){
printf("GRBL: ERROR: Reply ERROR (command[%d] = %s)\n",
grblCommandBufferIndex_,
command.c_str());
}
errorCode_ = ECMC_PLUGIN_GRBL_COMMAND_ERROR_CODE;
// stop motion
setExecute(0);
setReset(1);
setReset(0);
break;
} else if(reply.find(ECMC_PLUGIN_GRBL_GRBL_STARTUP_STRING) != std::string::npos ) {
if(cfgDbgMode_){
printf("GRBL: INFO: Ready for commands: %s\n",reply.c_str());
}
// system has reset
setExecute(0);
break;
} else {
// keep waiting (no break)
if(cfgDbgMode_){
printf("GRBL: INFO: Reply non protocol related: %s\n",reply.c_str());
}
}
}
if(replyStat != ECMC_GRBL_REPLY_OK) {
errorCode_ = ECMC_PLUGIN_CONFIG_ERROR_CODE;
printf("GRBL: ERROR: Plugin suspended due to configuration failed on command %s\n",command.c_str());
printf("GRBL: ERROR: Restart of IOC needed.\n");
unrecoverableError_ = 1;
setExecute(0);
return; //kill thread
}
grblCommandBufferIndex_++;
index++;
}
if(cfgDbgMode_){
printf("GRBL: INFO: Configuration ready\n");
}
else {
if( (( grblCommandBufferIndex_ >= grblCommandBuffer_.size()) || !executeCmd_ ) &&
grblInitDone_) {
writerBusy_ = 0;
}
// Wait for right condition to start
delay_ms(5);
if(cfgDbgMode_){
printf("GRBL: INFO: Start load g-code\n");
}
// GRBL ready, now we can send g-code comamnds
for(;;) {
if( (grblCommandBuffer_.size() > grblCommandBufferIndex_) &&
executeCmd_ && ecmcData_.allEnabled && grblInitDone_) {
epicsMutexLock(grblCommandBufferMutex_);
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;
}
//Write command (will block untill written)
grblWriteCommand(command);
// will block untill answer
grblReplyType replyStat = grblReadReply();
if(replyStat != ECMC_GRBL_REPLY_OK) {
errorCode_ = ECMC_PLUGIN_GRBL_COMMAND_ERROR_CODE;
// stop motion
setExecute(0);
setReset(0);
setReset(1);
setReset(0);
grblCommandBufferIndex_ = 0;
break; // for loop
}
grblCommandBufferIndex_++;
}
else {
if( (( grblCommandBufferIndex_ >= grblCommandBuffer_.size()) || !executeCmd_ ) &&
grblInitDone_) {
writerBusy_ = 0;
}
// Wait for right condition to start
delay_ms(5);
}
}
}
}
void ecmcGrbl::grblWriteCommand(std::string command) {
// wait for grbl
while(serial_get_rx_buffer_available() <= strlen(command.c_str())+1) {
delay_ms(1);
}
if(cfgDbgMode_){
printf("GRBL: INFO: Write command (command[%d] = %s)\n",
grblCommandBufferIndex_,
command.c_str());
}
ecmc_write_command_serial(strdup(command.c_str()));
}
grblReplyType ecmcGrbl::grblReadReply() {
std::string 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) {
if(reply.find(ECMC_PLUGIN_GRBL_GRBL_OK_STRING) != std::string::npos) {
if(cfgDbgMode_){
printf("GRBL: INFO: Reply OK\n");
}
return ECMC_GRBL_REPLY_OK;
} else if(reply.find(ECMC_PLUGIN_GRBL_GRBL_ERR_STRING) != std::string::npos) {
if(cfgDbgMode_){
printf("GRBL: ERROR: Reply ERROR\n");
}
return ECMC_GRBL_REPLY_ERROR;
} else if(reply.find(ECMC_PLUGIN_GRBL_GRBL_STARTUP_STRING) != std::string::npos ) {
if(cfgDbgMode_){
printf("GRBL: INFO: Ready for commands: %s\n",reply.c_str());
}
return ECMC_GRBL_REPLY_START;
} else {
// keep waiting (no break)
if(cfgDbgMode_){
printf("GRBL: INFO: Reply non protocol related: %s\n",reply.c_str());
}
return ECMC_GRBL_REPLY_NON_PROTOCOL;
}
}
}
return ECMC_GRBL_REPLY_NON_PROTOCOL;
}
// Main grbl worker (copied from grbl main.c)
@@ -681,7 +743,7 @@ void ecmcGrbl::readEcmcStatus(int ecmcError) {
// grb realtime thread!!!
int ecmcGrbl::grblRTexecute(int ecmcError) {
if(getEcmcEpicsIOCState()!=16 || !grblInitDone_) {
if(getEcmcEpicsIOCState()!=16 || !grblInitDone_ || unrecoverableError_) {
return 0;
}
@@ -838,7 +900,7 @@ void ecmcGrbl::resetError() {
void ecmcGrbl::addCommand(std::string command) {
if(cfgDbgMode_){
printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);
printf("%s:%s:%d:command %s\n",__FILE__,__FUNCTION__,__LINE__,command.c_str());
}
epicsMutexLock(grblCommandBufferMutex_);
grblCommandBuffer_.push_back(command.c_str());
@@ -851,7 +913,7 @@ void ecmcGrbl::addCommand(std::string command) {
void ecmcGrbl::loadFile(std::string fileName, int append) {
if(cfgDbgMode_){
printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);
printf("%s:%s:%d: file %s, append %d\n",__FILE__,__FUNCTION__,__LINE__,fileName.c_str(),append);
}
std::ifstream file;
@@ -882,3 +944,27 @@ void ecmcGrbl::loadFile(std::string fileName, int append) {
}
}
}
void ecmcGrbl::addConfig(std::string command) {
if(cfgDbgMode_){
printf("%s:%s:%d:command %s\n",__FILE__,__FUNCTION__,__LINE__,command.c_str());
}
if (getEcmcEpicsIOCState() == 16) {
if(cfgDbgMode_){
printf("%s:%s:%d: GRBL: ERROR: Configuratoin can only be applied during startup:(0x%x)\n",
__FILE__,__FUNCTION__,__LINE__,ECMC_PLUGIN_CONFIG_ERROR_CODE);
}
return;
}
if(cfgDbgMode_){
printf("%s:%s:%d:command %s\n",__FILE__,__FUNCTION__,__LINE__,command.c_str());
}
epicsMutexLock(grblConfigBufferMutex_);
grblConfigBuffer_.push_back(command.c_str());
epicsMutexUnlock(grblConfigBufferMutex_);
if(cfgDbgMode_){
printf("%s:%s:%d: GRBL: INFO: Buffer size %d\n",
__FILE__,__FUNCTION__,__LINE__,grblConfigBuffer_.size());
}
}

13
ecmc_plugin_grbl/ecmcGrbl.h
View File

@@ -48,6 +48,13 @@ typedef struct {
bool allLimitsOKOld;
} ecmcStatusData;
enum grblReplyType {
ECMC_GRBL_REPLY_START = 0,
ECMC_GRBL_REPLY_OK = 1,
ECMC_GRBL_REPLY_ERROR = 2,
ECMC_GRBL_REPLY_NON_PROTOCOL = 3
};
class ecmcGrbl : public asynPortDriver {
public:
@@ -66,6 +73,7 @@ class ecmcGrbl : public asynPortDriver {
void doMainWorker();
void doWriteWorker();
void addCommand(std::string command);
void addConfig(std::string command);
void loadFile(std::string filename, int append);
int enterRT();
int grblRTexecute(int ecmcError);
@@ -97,6 +105,8 @@ class ecmcGrbl : public asynPortDriver {
bool getEcmcAxisLimitBwd(int ecmcAxisId);
bool getEcmcAxisLimitFwd(int ecmcAxisId);
static std::string to_string(int value);
grblReplyType grblReadReply();
void grblWriteCommand(std::string command);
int cfgDbgMode_;
int cfgXAxisId_;
int cfgYAxisId_;
@@ -113,6 +123,8 @@ class ecmcGrbl : public asynPortDriver {
int errorCodeOld_;
double exeSampleTimeMs_;
int grblInitDone_;
std::vector<std::string> grblConfigBuffer_;
epicsMutexId grblConfigBufferMutex_;
std::vector<std::string> grblCommandBuffer_;
unsigned int grblCommandBufferIndex_;
epicsMutexId grblCommandBufferMutex_;
@@ -123,6 +135,7 @@ class ecmcGrbl : public asynPortDriver {
double spindleAcceleration_;
int cfgAutoEnableTimeOutSecs_;
int autoEnableTimeOutCounter_;
int unrecoverableError_;
ecmcStatusData ecmcData_;
};

1
ecmc_plugin_grbl/ecmcGrblDefs.h
View File

@@ -32,6 +32,7 @@
#define ECMC_PLUGIN_LIMIT_SWITCH_VIOLATION_ERROR_CODE 0x104
#define ECMC_PLUGIN_SPINDLE_ACC_ERROR_CODE 0x105
#define ECMC_PLUGIN_AUTO_ENABLE_TIMEOUT_ERROR_CODE 0x106
#define ECMC_PLUGIN_CONFIG_ERROR_CODE 0x107
#define ECMC_PLUGIN_AUTO_ENABLE_TIME_OUT_SEC 10

75
ecmc_plugin_grbl/ecmcGrblWrap.cpp
View File

@@ -154,7 +154,6 @@ void deleteGrbl() {
}
}
/**
* EPICS iocsh shell command: ecmcGrblAddCommand
*/
@@ -261,12 +260,86 @@ static void initCallFunc_1(const iocshArgBuf *args) {
ecmcGrblLoadFile(args[0].sval,args[1].ival);
}
/*
$11 - Junction deviation, mm
$12 – Arc tolerance, mm
$30 - Max spindle speed, RPM
$31 - Min spindle speed, RPM
$100, $101 and $102 – [X,Y,Z] steps/mm
$110, $111 and $112 – [X,Y,Z] Max rate, mm/min
$120, $121, $122 – [X,Y,Z] Acceleration, mm/sec^2*/
/**
* EPICS iocsh shell command: ecmcGrblAddConfig
*/
void ecmcGrblAddConfigPrintHelp() {
printf("\n");
printf(" Use ecmcGrblAddConfig(<command>)\n");
printf(" <command> : Grbl command.\n");
printf("\n");
printf(" Supported grbl comamnds:\n");
printf(" $11 - Junction deviation, mm\n");
printf(" $12 – Arc tolerance, mm\n");
printf(" $30 - Max spindle speed, RPM\n");
printf(" $31 - Min spindle speed, RPM\n");
printf(" $100, $101 and $102 – [X,Y,Z] steps/mm\n");
printf(" $110, $111 and $112 – [X,Y,Z] Max rate, mm/min\n");
printf(" $120, $121, $122 – [X,Y,Z] Acceleration, mm/sec^2\n");
printf("\n");
printf(" Example: Set resolution to 1 micrometer\n");
printf(" ecmcGrblAddConfig(\"$100=1000\") \n");
printf("\n");
}
int ecmcGrblAddConfig(const char* command) {
if(!command) {
printf("Error: command.\n");
ecmcGrblAddConfigPrintHelp();
return asynError;
}
if(strcmp(command,"-h") == 0 || strcmp(command,"--help") == 0 ) {
ecmcGrblAddConfigPrintHelp();
return asynSuccess;
}
if(!grbl) {
printf("Plugin not initialized/loaded.\n");
return asynError;
}
try {
grbl->addConfig(command);
}
catch(std::exception& e) {
printf("Exception: %s. Add config failed.\n",e.what());
return asynError;
}
return asynSuccess;
}
static const iocshArg initArg0_2 =
{ " Grbl Config", iocshArgString };
static const iocshArg *const initArgs_2[] = { &initArg0_2};
static const iocshFuncDef initFuncDef_2 = { "ecmcGrblAddConfig", 1, initArgs_2 };
static void initCallFunc_2(const iocshArgBuf *args) {
ecmcGrblAddConfig(args[0].sval);
}
///**
// * Register all functions
//*/
void ecmcGrblPluginDriverRegister(void) {
iocshRegister(&initFuncDef_0, initCallFunc_0); // ecmcGrblAddCommand
iocshRegister(&initFuncDef_1, initCallFunc_1); // ecmcGrblLoadFile
iocshRegister(&initFuncDef_2, initCallFunc_2); // ecmcGrblAddConfig
}
epicsExportRegistrar(ecmcGrblPluginDriverRegister);

1903
iocsh/log.log
View File

File diff suppressed because it is too large Load Diff

3
iocsh/test.script
View File

@@ -88,6 +88,7 @@ epicsEnvUnset(ECMC_PLUGIN_FILNAME)
epicsEnvUnset(ECMC_PLUGIN_CONFIG)
ecmcGrblLoadFile("./plc/gcode.nc",0)
asynSetTraceMask(MC_CPU1, -1, 0x0)
#ecmcGrblAddCommand("G1X20Y20F360");
#ecmcGrblAddCommand("G4P2");
@@ -123,7 +124,7 @@ $(SCRIPTEXEC) ($(ecmccfg_DIR)setAppMode.cmd)
#asynSetTraceMask(MC_CPU1, -1, 0x41)
# Note need to test 0x21 later..
#asynSetTraceMask(MC_CPU1, -1, 0x21)
asynSetTraceMask(MC_CPU1, -1, 0x41)
asynSetTraceMask(MC_CPU1, -1, 0x0)
iocInit()
dbpf $(IOC):Axis1.SPAM 0