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

Integrate as a normal plugin.. WIP

Browse Source
This commit is contained in:
Anders Sandstrom
2022-01-20 13:08:08 +01:00
parent 406b66d020
commit d78e8ef9f6
13 changed files with 823 additions and 34797 deletions
Download Patch File Download Diff File Expand all files Collapse all files

560
ecmc_plugin_grbl/ecmcGrbl.cpp
View File

@@ -3,7 +3,7 @@
* ecmc is distributed subject to a Software License Agreement found
* in file LICENSE that is included with this distribution.
*
* ecmcSocketCAN.cpp
* ecmcGrbl.cpp
*
* Created on: Mar 22, 2020
* Author: anderssandstrom
@@ -15,103 +15,122 @@
#define ECMC_IS_PLUGIN
#include <sstream>
#include "ecmcSocketCAN.h"
#include "ecmcGrbl.h"
#include "ecmcPluginClient.h"
#include "ecmcAsynPortDriver.h"
#include "ecmcAsynPortDriverUtils.h"
#include "epicsThread.h"
extern "C" {
#include "grbl.h"
}
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.
volatile uint8_t sys_probe_state; // Probing state value. Used to coordinate the probing cycle with stepper ISR.
volatile uint8_t sys_rt_exec_state; // Global realtime executor bitflag variable for state management. See EXEC bitmasks.
volatile uint8_t sys_rt_exec_alarm; // Global realtime executor bitflag variable for setting various alarms.
volatile uint8_t sys_rt_exec_motion_override; // Global realtime executor bitflag variable for motion-based overrides.
volatile uint8_t sys_rt_exec_accessory_override; // Global realtime executor bitflag variable for spindle/coolant overrides.
#ifdef DEBUG
volatile uint8_t sys_rt_exec_debug;
#endif
// Start worker for socket read()
void f_worker_read(void *obj) {
if(!obj) {
printf("%s/%s:%d: Error: Worker read thread ecmcSocketCAN object NULL..\n",
printf("%s/%s:%d: Error: Worker read thread ecmcGrbl object NULL..\n",
__FILE__, __FUNCTION__, __LINE__);
return;
}
ecmcSocketCAN * canObj = (ecmcSocketCAN*)obj;
canObj->doReadWorker();
ecmcGrbl * grblObj = (ecmcGrbl*)obj;
grblObj->doReadWorker();
}
// Start worker for socket connect()
void f_worker_connect(void *obj) {
void f_worker_main(void *obj) {
if(!obj) {
printf("%s/%s:%d: Error: Worker connect thread ecmcSocketCAN object NULL..\n",
printf("%s/%s:%d: Error: Worker main thread ecmcGrbl object NULL..\n",
__FILE__, __FUNCTION__, __LINE__);
return;
}
ecmcSocketCAN * canObj = (ecmcSocketCAN*)obj;
canObj->doConnectWorker();
ecmcGrbl * grblObj = (ecmcGrbl*)obj;
grblObj->doMainWorker();
}
/** ecmc ecmcSocketCAN class
/** ecmc ecmcGrbl class
* This object can throw:
* - bad_alloc
* - invalid_argument
* - runtime_error
*/
ecmcSocketCAN::ecmcSocketCAN(char* configStr,
char* portName,
int exeSampleTimeMs) {
// Init
cfgCanIFStr_ = NULL;
cfgDbgMode_ = 0;
cfgAutoConnect_ = 1;
destructs_ = 0;
socketId_ = -1;
connected_ = 0;
writeBuffer_ = NULL;
deviceCounter_ = 0;
refreshNeeded_ = 0;
errorCode_ = 0;
masterDev_ = NULL;
for(int i = 0; i<ECMC_CAN_MAX_DEVICES;i++) {
devices_[i] = NULL;
}
ecmcGrbl::ecmcGrbl(char* configStr,
char* portName,
double exeSampleTimeMs)
: asynPortDriver(portName,
1, /* maxAddr */
asynInt32Mask | asynFloat64Mask | asynFloat32ArrayMask |
asynFloat64ArrayMask | asynEnumMask | asynDrvUserMask |
asynOctetMask | asynInt8ArrayMask | asynInt16ArrayMask |
asynInt32ArrayMask | asynUInt32DigitalMask, /* Interface mask */
asynInt32Mask | asynFloat64Mask | asynFloat32ArrayMask |
asynFloat64ArrayMask | asynEnumMask | asynDrvUserMask |
asynOctetMask | asynInt8ArrayMask | asynInt16ArrayMask |
asynInt32ArrayMask | asynUInt32DigitalMask, /* Interrupt mask */
ASYN_CANBLOCK , /*NOT ASYN_MULTI_DEVICE*/
1, /* Autoconnect */
0, /* Default priority */
0) /* Default stack size */
exeSampleTimeMs_ = exeSampleTimeMs;
memset(&ifr_,0,sizeof(struct ifreq));
memset(&rxmsg_,0,sizeof(struct can_frame));
memset(&addr_,0,sizeof(struct sockaddr_can));
{
// Init
cfgDbgMode_ = 0;
destructs_ = 0;
connected_ = 0;
errorCode_ = 0;
exeSampleTimeMs_ = exeSampleTimeMs;
cfgXAxisId_ = -1;
cfgYAxisId_ = -1;
cfgZAxisId_ = -1;
cfgSpindleAxisId_ = -1;
grblInitDone_ = 0;
parseConfigStr(configStr); // Assigns all configs
// Check valid nfft
if(!cfgCanIFStr_ ) {
throw std::out_of_range("CAN inteface must be defined (can0, vcan0...).");
//Check atleast one valid axis
if(cfgXAxisId_<0 && cfgXAxisId_<0 && cfgXAxisId_<0 && cfgSpindleAxisId_<0) {
throw std::out_of_range("No valid axis choosen.");
}
// Create worker thread for reading socket
std::string threadname = "ecmc." ECMC_PLUGIN_ASYN_PREFIX".read";
std::string threadname = "ecmc." ECMC_PLUGIN_ASYN_PREFIX ".read";
if(epicsThreadCreate(threadname.c_str(), 0, 32768, f_worker_read, this) == NULL) {
throw std::runtime_error("Error: Failed create worker thread for read().");
}
// Create worker thread for connecting socket
threadname = "ecmc." ECMC_PLUGIN_ASYN_PREFIX".connect";
if(epicsThreadCreate(threadname.c_str(), 0, 32768, f_worker_connect, this) == NULL) {
throw std::runtime_error("Error: Failed create worker thread for connect().");
threadname = "ecmc." ECMC_PLUGIN_ASYN_PREFIX ".main";
if(epicsThreadCreate(threadname.c_str(), 0, 32768, f_worker_main, this) == NULL) {
throw std::runtime_error("Error: Failed create worker thread for main().");
}
if(cfgAutoConnect_) {
connectPrivate();
// wait for grblInitDone_!
printf("Waiting for grbl init..");
while(!grblInitDone_) {
delay_ms(100);
printf(".");
}
writeBuffer_ = new ecmcSocketCANWriteBuffer(socketId_, cfgDbgMode_);
initAsyn();
delay_ms(100);
printf("\n");
printf("\n grbl ready for commands!\n");
sleep(1);
testGrbl();
}
ecmcSocketCAN::~ecmcSocketCAN() {
ecmcGrbl::~ecmcGrbl() {
// kill worker
destructs_ = 1; // maybe need todo in other way..
doWriteEvent_.signal();
doConnectEvent_.signal();
for(int i = 0; i<ECMC_CAN_MAX_DEVICES;i++) {
delete devices_[i];
}
}
void ecmcSocketCAN::parseConfigStr(char *configStr) {
void ecmcGrbl::parseConfigStr(char *configStr) {
// check config parameters
if (configStr && configStr[0]) {
@@ -132,337 +151,158 @@ void ecmcSocketCAN::parseConfigStr(char *configStr) {
cfgDbgMode_ = atoi(pThisOption);
}
// ECMC_PLUGIN_CONNECT_OPTION_CMD (1/0)
if (!strncmp(pThisOption, ECMC_PLUGIN_CONNECT_OPTION_CMD, strlen(ECMC_PLUGIN_CONNECT_OPTION_CMD))) {
pThisOption += strlen(ECMC_PLUGIN_DBG_PRINT_OPTION_CMD);
cfgAutoConnect_ = atoi(pThisOption);
// ECMC_PLUGIN_X_AXIS_ID_OPTION_CMD (1..ECMC_MAX_AXES)
if (!strncmp(pThisOption, ECMC_PLUGIN_X_AXIS_ID_OPTION_CMD, strlen(ECMC_PLUGIN_X_AXIS_ID_OPTION_CMD))) {
pThisOption += strlen(ECMC_PLUGIN_X_AXIS_ID_OPTION_CMD);
cfgXAxisId_ = atoi(pThisOption);
}
// ECMC_PLUGIN_IF_OPTION_CMD (Source string)
else if (!strncmp(pThisOption, ECMC_PLUGIN_IF_OPTION_CMD, strlen(ECMC_PLUGIN_IF_OPTION_CMD))) {
pThisOption += strlen(ECMC_PLUGIN_IF_OPTION_CMD);
cfgCanIFStr_=strdup(pThisOption);
// ECMC_PLUGIN_Y_AXIS_ID_OPTION_CMD (1..ECMC_MAX_AXES)
if (!strncmp(pThisOption, ECMC_PLUGIN_Y_AXIS_ID_OPTION_CMD, strlen(ECMC_PLUGIN_Y_AXIS_ID_OPTION_CMD))) {
pThisOption += strlen(ECMC_PLUGIN_Y_AXIS_ID_OPTION_CMD);
cfgYAxisId_ = atoi(pThisOption);
}
// ECMC_PLUGIN_Z_AXIS_ID_OPTION_CMD (1..ECMC_MAX_AXES)
if (!strncmp(pThisOption, ECMC_PLUGIN_Z_AXIS_ID_OPTION_CMD, strlen(ECMC_PLUGIN_Z_AXIS_ID_OPTION_CMD))) {
pThisOption += strlen(ECMC_PLUGIN_Z_AXIS_ID_OPTION_CMD);
cfgZAxisId_ = atoi(pThisOption);
}
// ECMC_PLUGIN_SPINDLE_AXIS_ID_OPTION_CMD (1..ECMC_MAX_AXES)
if (!strncmp(pThisOption, ECMC_PLUGIN_SPINDLE_AXIS_ID_OPTION_CMD, strlen(ECMC_PLUGIN_SPINDLE_AXIS_ID_OPTION_CMD))) {
pThisOption += strlen(ECMC_PLUGIN_SPINDLE_AXIS_ID_OPTION_CMD);
cfgSpindleAxisId_ = atoi(pThisOption);
}
pThisOption = pNextOption;
}
free(pOptions);
}
if(!cfgCanIFStr_) {
throw std::invalid_argument( "CAN interface not defined.");
}
}
// For connect commands over asyn or plc. let worker connect
void ecmcSocketCAN::connectExternal() {
if(!connected_) {
doConnectEvent_.signal(); // let worker start
}
}
void ecmcSocketCAN::connectPrivate() {
if((socketId_ = socket(PF_CAN, SOCK_RAW, CAN_RAW)) == -1) {
throw std::runtime_error( "Error while opening socket.");
return;
}
strcpy(ifr_.ifr_name, cfgCanIFStr_);
ioctl(socketId_, SIOCGIFINDEX, &ifr_);
addr_.can_family = AF_CAN;
addr_.can_ifindex = ifr_.ifr_ifindex;
printf("%s at index %d\n", cfgCanIFStr_, ifr_.ifr_ifindex);
if(bind(socketId_, (struct sockaddr *)&addr_, sizeof(addr_)) == -1) {
throw std::runtime_error( "Error in socket bind.");
return;
}
connected_ = 1;
}
int ecmcSocketCAN::getConnected() {
return connected_;
}
// Read socket worker
void ecmcSocketCAN::doReadWorker() {
while(true) {
if(destructs_) {
break;
}
// Wait for new CAN frame
int bytes = read(socketId_, &rxmsg_, sizeof(rxmsg_));
if(bytes == -1) {
errorCode_ = errno;
printf("ecmcSocketCAN: read() fail with error %s.\n", strerror(errno));
refreshNeeded_ = 1;
continue;
}
// forward all data to devices (including master)
for(int i = 0; i < deviceCounter_; i++){
devices_[i]->newRxFrame(&rxmsg_);
}
if(cfgDbgMode_) {
// Simulate candump printout
printf("r 0x%03X", rxmsg_.can_id);
printf(" [%d]", rxmsg_.can_dlc);
for(int i=0; i<rxmsg_.can_dlc; i++ ) {
printf(" 0x%02X", rxmsg_.data[i]);
}
printf("\n");
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());
}
}
// Connect socket worker
void ecmcSocketCAN::doConnectWorker() {
while(true) {
if(destructs_) {
return;
}
doConnectEvent_.wait();
if(destructs_) {
return;
}
connectPrivate();
}
}
int ecmcSocketCAN::getlastWritesError() {
if(!writeBuffer_) {
return ECMC_CAN_ERROR_WRITE_BUFFER_NULL;
}
return writeBuffer_->getlastWritesErrorAndReset();
}
int ecmcSocketCAN::addWriteCAN(uint32_t canId,
uint8_t len,
uint8_t data0,
uint8_t data1,
uint8_t data2,
uint8_t data3,
uint8_t data4,
uint8_t data5,
uint8_t data6,
uint8_t data7) {
if(!writeBuffer_) {
return ECMC_CAN_ERROR_WRITE_BUFFER_NULL;
}
writeBuffer_->addWriteCAN(canId,
len,
data0,
data1,
data2,
data3,
data4,
data5,
data6,
data7);
return 0;
}
// Main grbl worker (copied from grbl main.c)
void ecmcGrbl::doMainWorker() {
printf("%s:%s:%d\n",__FILE__,__FUNCTION__,__LINE__);
void ecmcSocketCAN::execute() {
// Initialize system upon power-up.
serial_init(); // Setup serial baud rate and interrupts
ecmc_init_file(); // create and clear file (simulated eeprom)
settings_restore(0b1111); // restore all to defaults
settings_init(); // Load Grbl settings from EEPROM
stepper_init(); // Configure stepper pins and interrupt timers
system_init(); // Configure pinout pins and pin-change interrupt
memset(sys_position,0,sizeof(sys_position)); // Clear machine position.
//sei(); // Enable interrupts
// Initialize system state.
#ifdef FORCE_INITIALIZATION_ALARM
// Force Grbl into an ALARM state upon a power-cycle or hard reset.
sys.state = STATE_ALARM;
#else
sys.state = STATE_IDLE;
#endif
// Check for power-up and set system alarm if homing is enabled to force homing cycle
// by setting Grbl's alarm state. Alarm locks out all g-code commands, including the
// startup scripts, but allows access to settings and internal commands. Only a homing
// cycle '$H' or kill alarm locks '$X' will disable the alarm.
// NOTE: The startup script will run after successful completion of the homing cycle, but
// not after disabling the alarm locks. Prevents motion startup blocks from crashing into
// things uncontrollably. Very bad.
#ifdef HOMING_INIT_LOCK
if (bit_istrue(settings.flags,BITFLAG_HOMING_ENABLE)) { sys.state = STATE_ALARM; }
#endif
// Grbl initialization loop upon power-up or a system abort. For the latter, all processes
// will return to this loop to be cleanly re-initialized.
for(;;) {
if(destructs_) {
return;
}
// Reset system variables.
uint8_t prior_state = sys.state;
memset(&sys, 0, sizeof(system_t)); // Clear system struct variable.
sys.state = prior_state;
sys.f_override = DEFAULT_FEED_OVERRIDE; // Set to 100%
sys.r_override = DEFAULT_RAPID_OVERRIDE; // Set to 100%
sys.spindle_speed_ovr = DEFAULT_SPINDLE_SPEED_OVERRIDE; // Set to 100%
memset(sys_probe_position,0,sizeof(sys_probe_position)); // Clear probe position.
sys_probe_state = 0;
sys_rt_exec_state = 0;
sys_rt_exec_alarm = 0;
sys_rt_exec_motion_override = 0;
sys_rt_exec_accessory_override = 0;
for(int i = 0; i < deviceCounter_; i++){
devices_[i]->execute();
}
// Reset Grbl primary systems.
serial_reset_read_buffer(); // Clear serial read buffer
gc_init(); // Set g-code parser to default state
spindle_init();
coolant_init();
//limits_init();
probe_init();
plan_reset(); // Clear block buffer and planner variables
st_reset(); // Clear stepper subsystem variables.
int writeError=getlastWritesError();
if (writeError) {
errorCode_ = writeError;
refreshNeeded_ = 1;
// Sync cleared gcode and planner positions to current system position.
plan_sync_position();
gc_sync_position();
// Print welcome message. Indicates an initialization has occured at power-up or with a reset.
report_init_message();
// ready for commands through serial interface
grblInitDone_ = 1;
protocol_main_loop();
if(destructs_) {
return;
}
}
refreshAsynParams();
return;
}
// grb realtime thread!!!
void ecmcGrbl::grblRTexecute() {
}
// Avoid issues with std:to_string()
std::string ecmcSocketCAN::to_string(int value) {
std::string ecmcGrbl::to_string(int value) {
std::ostringstream os;
os << value;
return os.str();
}
void ecmcSocketCAN::addMaster(uint32_t nodeId,
const char* name,
int lssSampleTimeMs,
int syncSampleTimeMs,
int heartSampleTimeMs) {
void ecmcGrbl::testGrbl() {
if(masterDev_) {
throw std::runtime_error("Master already added.");
}
if(deviceCounter_ >= ECMC_CAN_MAX_DEVICES) {
throw std::out_of_range("Device array full.");
}
if(nodeId >= 128) {
throw std::out_of_range("Node id out of range.");
}
// test some commands
printf("Test command:$\n");
ecmc_write_command_serial("$\n");
sleep(1);
printf("Test command:G0X10Y100\n");
ecmc_write_command_serial("G0X10Y100\n");
sleep(1);
printf("Test command:$G\n");
ecmc_write_command_serial("$G\n");
sleep(1);
printf("Test command:G4P4\n");
ecmc_write_command_serial("G4P4\n");
printf("Test command:G1X20Y200F20\n");
ecmc_write_command_serial("G1X20Y200F20\n");
printf("Test command:G4P4\n");
ecmc_write_command_serial("G4P4\n");
printf("Test command:G2X40Y220R20\n");
ecmc_write_command_serial("G2X40Y220R20\n");
printf("Test command:$\n");
ecmc_write_command_serial("$\n");
if(lssSampleTimeMs <= 0) {
throw std::out_of_range("LSS sample time ms out of range.");
}
if(syncSampleTimeMs <= 0) {
throw std::out_of_range("Sync sample time ms out of range.");
}
if(heartSampleTimeMs <= 0) {
throw std::out_of_range("Heart sample time ms out of range.");
}
masterDev_ = new ecmcCANOpenMaster(writeBuffer_,
nodeId,
exeSampleTimeMs_,
lssSampleTimeMs,
syncSampleTimeMs,
heartSampleTimeMs,
name,
cfgDbgMode_);
// add as a normal device also for execute and rxframe
devices_[deviceCounter_] = masterDev_;
deviceCounter_++;
}
void ecmcSocketCAN::addDevice(uint32_t nodeId,
const char* name,
int heartTimeoutMs){
if(deviceCounter_ >= ECMC_CAN_MAX_DEVICES) {
throw std::out_of_range("Device array full.");
}
if(nodeId >= 128) {
throw std::out_of_range("Node id out of range.");
}
devices_[deviceCounter_] = new ecmcCANOpenDevice(writeBuffer_,nodeId,exeSampleTimeMs_,name,heartTimeoutMs,cfgDbgMode_);
deviceCounter_++;
}
int ecmcSocketCAN::findDeviceWithNodeId(uint32_t nodeId) {
for(int i=0; i < deviceCounter_;i++) {
if(devices_[i]) {
if(devices_[i]->getNodeId() == nodeId) {
return i;
}
}
}
return -1;
}
void ecmcSocketCAN::addPDO(uint32_t nodeId,
uint32_t cobId,
ecmc_can_direction rw,
uint32_t ODSize,
int readTimeoutMs,
int writeCycleMs, //if <0 then write on demand.
const char* name) {
int devId = findDeviceWithNodeId(nodeId);
if(devId < 0) {
throw std::out_of_range("Node id not found in any configured device.");
}
int errorCode = devices_[devId]->addPDO(cobId,
rw,
ODSize,
readTimeoutMs,
writeCycleMs,
name);
if(errorCode > 0) {
throw std::runtime_error("AddPDO() failed.");
}
}
void ecmcSocketCAN::addSDO(uint32_t nodeId,
uint32_t cobIdTx, // 0x580 + CobId
uint32_t cobIdRx, // 0x600 + Cobid
ecmc_can_direction rw,
uint16_t ODIndex, // Object dictionary index
uint8_t ODSubIndex, // Object dictionary subindex
uint32_t ODSize,
int readSampleTimeMs,
const char* name) {
int devId = findDeviceWithNodeId(nodeId);
if(devId < 0) {
throw std::out_of_range("Node id not found in any configured device.");
}
int errorCode = devices_[devId]->addSDO(cobIdTx,
cobIdRx,
rw,
ODIndex,
ODSubIndex,
ODSize,
readSampleTimeMs,
name);
if(errorCode > 0) {
throw std::runtime_error("AddSDO() failed.");
}
}
void ecmcSocketCAN::initAsyn() {
ecmcAsynPortDriver *ecmcAsynPort = (ecmcAsynPortDriver *)getEcmcAsynPortDriver();
if(!ecmcAsynPort) {
printf("ERROR: ecmcAsynPort NULL.");
throw std::runtime_error( "ERROR: ecmcAsynPort NULL." );
}
// Add resultdata "plugin.can.read.error"
std::string paramName = ECMC_PLUGIN_ASYN_PREFIX + std::string(".read.error");
errorParam_ = ecmcAsynPort->addNewAvailParam(
paramName.c_str(), // name
asynParamInt32, // asyn type
(uint8_t*)&errorCode_, // pointer to data
sizeof(errorCode_), // size of data
ECMC_EC_U32, // ecmc data type
0); // die if fail
if(!errorParam_) {
printf("ERROR: Failed create asyn param for data.");
throw std::runtime_error( "ERROR: Failed create asyn param for: " + paramName);
}
errorParam_->setAllowWriteToEcmc(false); // need to callback here
errorParam_->refreshParam(1); // read once into asyn param lib
ecmcAsynPort->callParamCallbacks(ECMC_ASYN_DEFAULT_LIST, ECMC_ASYN_DEFAULT_ADDR);
// Add resultdata "plugin.can.read.connected"
paramName = ECMC_PLUGIN_ASYN_PREFIX + std::string(".read.connected");
connectedParam_ = ecmcAsynPort->addNewAvailParam(
paramName.c_str(), // name
asynParamInt32, // asyn type
(uint8_t*)&connected_, // pointer to data
sizeof(connected_), // size of data
ECMC_EC_U32, // ecmc data type
0); // die if fail
if(!connectedParam_) {
printf("ERROR: Failed create asyn param for connected.");
throw std::runtime_error( "ERROR: Failed create asyn param for: " + paramName);
}
connectedParam_->setAllowWriteToEcmc(false); // need to callback here
connectedParam_->refreshParam(1); // read once into asyn param lib
ecmcAsynPort->callParamCallbacks(ECMC_ASYN_DEFAULT_LIST, ECMC_ASYN_DEFAULT_ADDR);
}
// only refresh from "execute" thread
void ecmcSocketCAN::refreshAsynParams() {
if(refreshNeeded_) {
connectedParam_->refreshParamRT(1); // read once into asyn param lib
errorParam_->refreshParamRT(1); // read once into asyn param lib
}
refreshNeeded_ = 0;
}

124
ecmc_plugin_grbl/ecmcGrbl.h
View File

@@ -13,136 +13,50 @@
#define ECMC_GRBL_H_
#include <stdexcept>
#include "ecmcDataItem.h"
#include "ecmcAsynPortDriver.h"
#include "ecmcSocketCANDefs.h"
#include "ecmcSocketCANWriteBuffer.h"
#include "ecmcCANOpenDevice.h"
#include "ecmcCANOpenMaster.h"
#include "asynPortDriver.h"
#include "ecmcGrblDefs.h"
#include "inttypes.h"
#include <string>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <net/if.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <linux/can.h>
#include <linux/can/raw.h>
#define ECMC_CAN_MAX_WRITE_CMDS 128
#define ECMC_CAN_MAX_DEVICES 128
#define ECMC_CAN_ERROR_WRITE_FULL 10
#define ECMC_CAN_ERROR_WRITE_BUSY 11
#define ECMC_CAN_ERROR_WRITE_NO_DATA 12
#define ECMC_CAN_ERROR_WRITE_INCOMPLETE 13
#define ECMC_CAN_ERROR_WRITE_BUFFER_NULL 14
class ecmcSocketCAN {
class ecmcGrbl : public asynPortDriver {
public:
/** ecmc ecmcSocketCAN class
/** ecmc ecmcGrbl class
* This object can throw:
* - bad_alloc
* - invalid_argument
* - runtime_error
* - out_of_range
*/
ecmcSocketCAN(char* configStr,
char* portName,
int exeSampelTimeMs);
~ecmcSocketCAN();
ecmcGrbl(char* configStr,
char* portName,
double exeSampelTimeMs);
~ecmcGrbl();
void doReadWorker();
void doWriteWorker();
void doConnectWorker();
void doMainWorker();
void grblRTexecute();
//virtual asynStatus writeInt32(asynUser *pasynUser, epicsInt32 value);
//virtual asynStatus readInt32(asynUser *pasynUser, epicsInt32 *value);
//virtual asynStatus readInt8Array(asynUser *pasynUser, epicsInt8 *value,
// size_t nElements, size_t *nIn);
//virtual asynStatus readFloat64(asynUser *pasynUser, epicsFloat64 *value);
void connectExternal();
int getConnected();
int addWriteCAN(uint32_t canId,
uint8_t len,
uint8_t data0,
uint8_t data1,
uint8_t data2,
uint8_t data3,
uint8_t data4,
uint8_t data5,
uint8_t data6,
uint8_t data7);
int getlastWritesError();
void execute(); // ecmc rt loop
void addMaster(uint32_t nodeId,
const char* name,
int lssSampleTimeMs,
int syncSampleTimeMs,
int heartSampleTimeMs);
void addDevice(uint32_t nodeId,
const char* name,
int heartTimeoutMs);
void addPDO(uint32_t nodeId,
uint32_t cobId,
ecmc_can_direction rw,
uint32_t ODSize,
int readTimeoutMs,
int writeCycleMs, //if <0 then write on demand.
const char* name);
void addSDO(uint32_t nodeId,
uint32_t cobIdTx, // 0x580 + CobId
uint32_t cobIdRx, // 0x600 + Cobid
ecmc_can_direction rw,
uint16_t ODIndex, // Object dictionary index
uint8_t ODSubIndex, // Object dictionary subindex
uint32_t ODSize,
int readSampleTimeMs,
const char* name);
int findDeviceWithNodeId(uint32_t nodeId);
private:
void testGrbl();
void parseConfigStr(char *configStr);
static std::string to_string(int value);
void connectPrivate();
int writeCAN(can_frame *frame);
char* cfgCanIFStr_; // Config: can interface can0, vcan0..
int cfgDbgMode_;
int cfgAutoConnect_;
int cfgXAxisId_;
int cfgYAxisId_;
int cfgZAxisId_;
int cfgSpindleAxisId_;
int destructs_;
int connected_;
epicsEvent doConnectEvent_;
epicsEvent doWriteEvent_;
struct can_frame rxmsg_;
struct ifreq ifr_;
int socketId_;
struct sockaddr_can addr_;
struct can_frame txmsgBuffer_[ECMC_CAN_MAX_WRITE_CMDS];
int exeSampleTimeMs_;
ecmcSocketCANWriteBuffer *writeBuffer_;
int deviceCounter_;
ecmcCANOpenDevice *devices_[ECMC_CAN_MAX_DEVICES];
ecmcCANOpenMaster *masterDev_;
int errorCode_;
int refreshNeeded_;
//ASYN
void initAsyn();
void refreshAsynParams();
ecmcAsynDataItem *errorParam_;
ecmcAsynDataItem *connectedParam_;
int errorCode_;
double exeSampleTimeMs_;
int grblInitDone_;
};
#endif /* ECMC_GRBL_H_ */

26
ecmc_plugin_grbl/ecmcGrblDefs.h
View File

@@ -13,26 +13,14 @@
#ifndef ECMC_GRBL_DEFS_H_
#define ECMC_GRBL_DEFS_H_
#include "grbl.h"
// Options
#define ECMC_PLUGIN_DBG_PRINT_OPTION_CMD "DBG_PRINT="
#define ECMC_PLUGIN_ASYN_PREFIX "plugin.grbl"
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.
volatile uint8_t sys_probe_state; // Probing state value. Used to coordinate the probing cycle with stepper ISR.
volatile uint8_t sys_rt_exec_state; // Global realtime executor bitflag variable for state management. See EXEC bitmasks.
volatile uint8_t sys_rt_exec_alarm; // Global realtime executor bitflag variable for setting various alarms.
volatile uint8_t sys_rt_exec_motion_override; // Global realtime executor bitflag variable for motion-based overrides.
volatile uint8_t sys_rt_exec_accessory_override; // Global realtime executor bitflag variable for spindle/coolant overrides.
#ifdef DEBUG
volatile uint8_t sys_rt_exec_debug;
#endif
#define ECMC_PLUGIN_DBG_PRINT_OPTION_CMD "DBG_PRINT="
#define ECMC_PLUGIN_X_AXIS_ID_OPTION_CMD "X_AXIS="
#define ECMC_PLUGIN_Y_AXIS_ID_OPTION_CMD "Y_AXIS="
#define ECMC_PLUGIN_Z_AXIS_ID_OPTION_CMD "Z_AXIS="
#define ECMC_PLUGIN_SPINDLE_AXIS_ID_OPTION_CMD "SPINDLE_AXIS="
#define ECMC_PLUGIN_ASYN_PREFIX "plugin.grbl"
#define ECMC_PLUGIN_GRBL_GENERAL_ERROR_CODE 1
#endif /* ECMC_GRBL_DEFS_H_ */

938
ecmc_plugin_grbl/ecmcGrblWrap.cpp
View File

@@ -3,9 +3,9 @@
* ecmc is distributed subject to a Software License Agreement found
* in file LICENSE that is included with this distribution.
*
* ecmcFFTWrap.cpp
* ecmeGrblWrap.cpp
*
* Created on: Mar 22, 2020
* Created on: Jan 20, 2022
* Author: anderssandstrom
*
\*************************************************************************/
@@ -16,132 +16,47 @@
#include <vector>
#include <stdexcept>
#include <string>
#include "ecmcSocketCANWrap.h"
#include "ecmcSocketCAN.h"
#include "ecmcSocketCANDefs.h"
#include <epicsTypes.h>
#include <epicsTime.h>
#include <epicsThread.h>
#include <epicsString.h>
#include <epicsTimer.h>
#include <epicsMutex.h>
#include <epicsExport.h>
#include <epicsEvent.h>
#include <iocsh.h>
#include "ecmcGrbl.h"
#include "ecmcGrblDefs.h"
#include "ecmcGrblWrap.h"
#define ECMC_PLUGIN_MAX_PORTNAME_CHARS 64
#define ECMC_PLUGIN_PORTNAME_PREFIX "PLUGIN.CAN"
#define ECMC_PLUGIN_PORTNAME_PREFIX "PLUGIN.GRBL"
static ecmcSocketCAN* can = NULL;
static char portNameBuffer[ECMC_PLUGIN_MAX_PORTNAME_CHARS];
static ecmcGrbl* grbl = NULL;
static char portNameBuffer[ECMC_PLUGIN_MAX_PORTNAME_CHARS];
int createSocketCAN(char* configStr, int exeSampleTimeMs) {
// create new ecmcFFT object
int createGrbl(char* configStr, int exeSampleTimeMs) {
// create asynport name for new object ()
memset(portNameBuffer, 0, ECMC_PLUGIN_MAX_PORTNAME_CHARS);
snprintf (portNameBuffer, ECMC_PLUGIN_MAX_PORTNAME_CHARS,
ECMC_PLUGIN_PORTNAME_PREFIX);
try {
can = new ecmcSocketCAN(configStr, portNameBuffer, exeSampleTimeMs);
grbl = new ecmcGrbl(configStr, portNameBuffer, exeSampleTimeMs);
}
catch(std::exception& e) {
if(can) {
delete can;
if(grbl) {
delete grbl;
}
printf("Exception: %s. Plugin will unload.\n",e.what());
return ECMC_PLUGIN_SOCKETCAN_ERROR_CODE;
return ECMC_PLUGIN_GRBL_GENERAL_ERROR_CODE;
}
return 0;
}
int connectSocketCAN() {
if(can){
try {
can->connectExternal();
}
catch(std::exception& e) {
printf("Exception: %s.\n",e.what());
return ECMC_PLUGIN_SOCKETCAN_ERROR_CODE;
}
}
else {
return ECMC_PLUGIN_SOCKETCAN_ERROR_CODE;
}
return 0;
}
int getSocketCANConnectd() {
if(can){
try {
return can->getConnected();
}
catch(std::exception& e) {
printf("Exception: %s.\n",e.what());
return 0;
}
}
return 0;
}
int getlastWritesError() {
if(can){
try {
return can->getlastWritesError();
}
catch(std::exception& e) {
printf("Exception: %s.\n",e.what());
return 1;
}
}
return 1;
}
int execute() {
if(can){
can->execute();
if(grbl){
grbl->grblRTexecute();
}
return 0;
}
int addWriteSocketCAN( double canId,
double len,
double data0,
double data1,
double data2,
double data3,
double data4,
double data5,
double data6,
double data7) {
if(can){
try {
return can->addWriteCAN((uint32_t) canId,
(uint8_t) len,
(uint8_t) data0,
(uint8_t) data1,
(uint8_t) data2,
(uint8_t) data3,
(uint8_t) data4,
(uint8_t) data5,
(uint8_t) data6,
(uint8_t) data7);
}
catch(std::exception& e) {
printf("Exception: %s.\n",e.what());
return ECMC_PLUGIN_SOCKETCAN_ERROR_CODE;
}
}
return ECMC_PLUGIN_SOCKETCAN_ERROR_CODE;
}
void deleteSocketCAN() {
if(can) {
delete (can);
void deleteGrbl() {
if(grbl) {
delete (grbl);
}
}
@@ -150,411 +65,412 @@ void deleteSocketCAN() {
* EPICS iocsh shell command: ecmcCANOpenAddMaster
*/
void ecmcCANOpenAddMasterPrintHelp() {
printf("\n");
printf(" Use ecmcCANOpenAddMaster(<name>, <node id>,....)\n");
printf(" <name> : Name of master device.\n");
printf(" <node id> : CANOpen node id of master.\n");
printf(" <LSS sample time ms> : Sample time for LSS.\n");
printf(" <Sync sample time ms> : Sample time for SYNC.\n");
printf(" <NMT Heartbeat sample time ms> : Sample time for NMT Heartbeat.\n");
printf("\n");
}
int ecmcCANOpenAddMaster(const char* name,
int nodeId,
int lssSampleTimeMs,
int syncSampleTimeMs,
int heartSampleTimeMs) {
if(!name) {
printf("Error: name.\n");
ecmcCANOpenAddMasterPrintHelp();
return asynError;
}
if(strcmp(name,"-h") == 0 || strcmp(name,"--help") == 0 ) {
ecmcCANOpenAddMasterPrintHelp();
return asynSuccess;
}
if(!can) {
printf("Plugin not initialized/loaded.\n");
return asynError;
}
try {
can->addMaster((uint32_t)nodeId,
name,
lssSampleTimeMs,
syncSampleTimeMs,
heartSampleTimeMs);
}
catch(std::exception& e) {
printf("Exception: %s. Add master failed.\n",e.what());
return asynError;
}
return asynSuccess;
}
static const iocshArg initArg0_0 =
{ "Name", iocshArgString };
static const iocshArg initArg1_0 =
{ "Node Id", iocshArgInt };
static const iocshArg initArg2_0 =
{ "LSS sample time ms", iocshArgInt };
static const iocshArg initArg3_0 =
{ "Sync sample time ms", iocshArgInt };
static const iocshArg initArg4_0 =
{ "NMT Heart sample time ms", iocshArgInt };
static const iocshArg *const initArgs_0[] = { &initArg0_0,
&initArg1_0,
&initArg2_0,
&initArg3_0,
&initArg4_0};
static const iocshFuncDef initFuncDef_0 = { "ecmcCANOpenAddMaster", 5, initArgs_0 };
static void initCallFunc_0(const iocshArgBuf *args) {
ecmcCANOpenAddMaster(args[0].sval,
args[1].ival,
args[2].ival,
args[3].ival,
args[4].ival);
}
/**
* EPICS iocsh shell command: ecmcCANOpenAddDevice
*/
void ecmcCANOpenAddDevicePrintHelp() {
printf("\n");
printf(" Use ecmcCANOpenAddDevice(<name>, <node id>)\n");
printf(" <name> : Name of device.\n");
printf(" <node id> : CANOpen node id of device.\n");
printf(" <NMT Heartbeat timeout ms> : Timeout for NMT Heartbeat.\n");
printf("\n");
}
int ecmcCANOpenAddDevice(const char* name, int nodeId,int heartTimeOutMs) {
if(!name) {
printf("Error: name.\n");
ecmcCANOpenAddDevicePrintHelp();
return asynError;
}
if(strcmp(name,"-h") == 0 || strcmp(name,"--help") == 0 ) {
ecmcCANOpenAddDevicePrintHelp();
return asynSuccess;
}
if(!can) {
printf("Plugin not initialized/loaded.\n");
return asynError;
}
if(heartTimeOutMs < 0) {
printf("Invalid NMT heartbeat timeout.\n");
return asynError;
}
try {
can->addDevice((uint32_t)nodeId,name,heartTimeOutMs);
}
catch(std::exception& e) {
printf("Exception: %s. Add device failed.\n",e.what());
return asynError;
}
return asynSuccess;
}
static const iocshArg initArg0_1 =
{ "Name", iocshArgString };
static const iocshArg initArg1_1 =
{ "Node Id", iocshArgInt };
static const iocshArg initArg2_1 =
{ "NMT Heart timeout ms", iocshArgInt };
static const iocshArg *const initArgs_1[] = { &initArg0_1,
&initArg1_1,
&initArg2_1};
static const iocshFuncDef initFuncDef_1 = { "ecmcCANOpenAddDevice", 3, initArgs_1 };
static void initCallFunc_1(const iocshArgBuf *args) {
ecmcCANOpenAddDevice(args[0].sval, args[1].ival, args[2].ival);
}
/**
* EPICS iocsh shell command: ecmcCANOpenAddSDO
*/
void ecmcCANOpenAddSDOPrintHelp() {
printf("\n");
printf(" Use ecmcCANOpenAddSDO(<name>, <node id>,.....)\n");
printf(" <name> : Name of master device.\n");
printf(" <node id> : CANOpen node id of device/master.\n");
printf(" <cob id tx> : CANOpen cob id of Tx of slave SDO.\n");
printf(" <cob id rx> : CANOpen cob id of Rx of slave SDO.\n");
printf(" <dir> : Direction 1=write and 2=read.\n");
printf(" <ODIndex> : OD index of SDO.\n");
printf(" <ODSubIndex> : OD sub index of SDO.\n");
printf(" <ODSize> : OS Size.\n");
printf(" <readSampleTimeMs>: Sample time for read in ms (write is always on demand).\n");
printf("\n");
}
int ecmcCANOpenAddSDO(const char* name,
int nodeId,
int cobIdTx,
int cobIdRx,
int dir,
int ODIndex,
int ODSubIndex,
int ODSize,
int readSampleTimeMs) {
if(!name) {
printf("Error: name.\n");
ecmcCANOpenAddSDOPrintHelp();
return asynError;
}
if(strcmp(name,"-h") == 0 || strcmp(name,"--help") == 0 ) {
ecmcCANOpenAddSDOPrintHelp();
return asynSuccess;
}
if(cobIdRx < 0) {
printf("Error: invalid cobIdRx.\n");
ecmcCANOpenAddSDOPrintHelp();
return asynError;
}
if(cobIdTx < 0) {
printf("Error: invalid cobIdTx.\n");
ecmcCANOpenAddSDOPrintHelp();
return asynError;
}
if(dir > 2 || dir <= 0) {
printf("Error: invalid dir.\n");
ecmcCANOpenAddSDOPrintHelp();
return asynError;
}
if(ODIndex < 0) {
printf("Error: invalid ODIndex.\n");
ecmcCANOpenAddSDOPrintHelp();
return asynError;
}
if(ODSubIndex < 0) {
printf("Error: invalid ODSubIndex.\n");
ecmcCANOpenAddSDOPrintHelp();
return asynError;
}
if(ODSize < 0) {
printf("Error: invalid ODSize.\n");
ecmcCANOpenAddSDOPrintHelp();
return asynError;
}
if(readSampleTimeMs < 0) {
printf("Error: invalid readSampleTimeMs.\n");
ecmcCANOpenAddSDOPrintHelp();
return asynError;
}
ecmc_can_direction tempDir = DIR_READ;
if(dir == 1) {
tempDir = DIR_WRITE;
}
try {
can->addSDO((uint32_t)nodeId,
cobIdTx,
cobIdRx,
tempDir,
ODIndex,
ODSubIndex,
ODSize,
readSampleTimeMs,
name);
}
catch(std::exception& e) {
printf("Exception: %s. Add PDO failed.\n",e.what());
return asynError;
}
return asynSuccess;
}
static const iocshArg initArg0_2 =
{ "Name", iocshArgString };
static const iocshArg initArg1_2 =
{ "Node Id", iocshArgInt };
static const iocshArg initArg2_2 =
{ "COB id TX", iocshArgInt };
static const iocshArg initArg3_2 =
{ "COB id RX", iocshArgInt };
static const iocshArg initArg4_2 =
{ "Direction", iocshArgInt };
static const iocshArg initArg5_2 =
{ "OD Index", iocshArgInt };
static const iocshArg initArg6_2 =
{ "OD sub index", iocshArgInt };
static const iocshArg initArg7_2 =
{ "OD size", iocshArgInt };
static const iocshArg initArg8_2 =
{ "Read sample time ms", iocshArgInt };
static const iocshArg *const initArgs_2[] = { &initArg0_2,
&initArg1_2,
&initArg2_2,
&initArg3_2,
&initArg4_2,
&initArg5_2,
&initArg6_2,
&initArg7_2,
&initArg8_2};
static const iocshFuncDef initFuncDef_2 = { "ecmcCANOpenAddSDO", 9, initArgs_2 };
static void initCallFunc_2(const iocshArgBuf *args) {
ecmcCANOpenAddSDO(args[0].sval,
args[1].ival,
args[2].ival,
args[3].ival,
args[4].ival,
args[5].ival,
args[6].ival,
args[7].ival,
args[8].ival);
}
/**
* EPICS iocsh shell command: ecmcCANOpenAddPDO
*/
void ecmcCANOpenAddPDOPrintHelp() {
printf("\n");
printf(" Use \"ecmcCANOpenAddPDO(<name>, <node id>\n");
printf(" <name> : Name of master device.\n");
printf(" <node id> : CANOpen node id of device/master.\n");
printf(" <cob id> : CANOpen cob id of PDO.\n");
printf(" <dir> : Direction 1=write and 2=read.\n");
printf(" <ODSize> : Size of PDO (max 8 bytes).\n");
printf(" <readTimeoutMs> : Readtimeout in ms.\n");
printf(" <writeCycleMs> : Cycle time for write (if <= 0 then only write on change).\n");
printf("\n");
}
int ecmcCANOpenAddPDO(const char* name,
int nodeId,
int cobId,
int dir,
int ODSize,
int readTimeoutMs,
int writeCycleMs) {
if(!name) {
printf("Error: name.\n");
ecmcCANOpenAddPDOPrintHelp();
return asynError;
}
if(strcmp(name,"-h") == 0 || strcmp(name,"--help") == 0 ) {
ecmcCANOpenAddPDOPrintHelp();
return asynSuccess;
}
if(dir > 2 || dir <= 0) {
printf("Error: invalid dir.\n");
ecmcCANOpenAddPDOPrintHelp();
return asynError;
}
if(ODSize < 0) {
printf("Error: invalid ODSize.\n");
ecmcCANOpenAddPDOPrintHelp();
return asynError;
}
if(readTimeoutMs < 0) {
printf("Error: invalid readTimeoutMs.\n");
ecmcCANOpenAddPDOPrintHelp();
return asynError;
}
if(writeCycleMs < 0) {
printf("Error: invalid writeCycleMs.\n");
ecmcCANOpenAddPDOPrintHelp();
return asynError;
}
ecmc_can_direction tempDir = DIR_READ;
if(dir == 1) {
tempDir = DIR_WRITE;
}
try {
can->addPDO((uint32_t)nodeId,
cobId,
tempDir,
ODSize,
readTimeoutMs,
writeCycleMs,name);
}
catch(std::exception& e) {
printf("Exception: %s. Add PDO failed.\n",e.what());
return asynError;
}
return asynSuccess;
}
static const iocshArg initArg0_3 =
{ "Name", iocshArgString };
static const iocshArg initArg1_3 =
{ "Node Id", iocshArgInt };
static const iocshArg initArg2_3 =
{ "COB Id", iocshArgInt };
static const iocshArg initArg3_3 =
{ "Direction", iocshArgInt };
static const iocshArg initArg4_3 =
{ "ODSize", iocshArgInt };
static const iocshArg initArg5_3 =
{ "Read Timeout ms", iocshArgInt };
static const iocshArg initArg6_3 =
{ "Write cycle ms", iocshArgInt };
static const iocshArg *const initArgs_3[] = { &initArg0_3,
&initArg1_3,
&initArg2_3,
&initArg3_3,
&initArg4_3,
&initArg5_3,
&initArg6_3
};
static const iocshFuncDef initFuncDef_3 = { "ecmcCANOpenAddPDO", 7, initArgs_3 };
static void initCallFunc_3(const iocshArgBuf *args) {
ecmcCANOpenAddPDO(args[0].sval,
args[1].ival,
args[2].ival,
args[3].ival,
args[4].ival,
args[5].ival,
args[6].ival);
}
/**
* Register all functions
*/
void ecmcCANPluginDriverRegister(void) {
iocshRegister(&initFuncDef_0, initCallFunc_0); // ecmcCANOpenAddMaster
iocshRegister(&initFuncDef_1, initCallFunc_1); // ecmcCANOpenAddDevice
iocshRegister(&initFuncDef_2, initCallFunc_2); // ecmcCANOpenAddSDO
iocshRegister(&initFuncDef_3, initCallFunc_3); // ecmcCANOpenAddPDO
}
epicsExportRegistrar(ecmcCANPluginDriverRegister);
//void ecmcCANOpenAddMasterPrintHelp() {
// printf("\n");
// printf(" Use ecmcCANOpenAddMaster(<name>, <node id>,....)\n");
// printf(" <name> : Name of master device.\n");
// printf(" <node id> : CANOpen node id of master.\n");
// printf(" <LSS sample time ms> : Sample time for LSS.\n");
// printf(" <Sync sample time ms> : Sample time for SYNC.\n");
// printf(" <NMT Heartbeat sample time ms> : Sample time for NMT Heartbeat.\n");
// printf("\n");
//}
//
//int ecmcCANOpenAddMaster(const char* name,
// int nodeId,
// int lssSampleTimeMs,
// int syncSampleTimeMs,
// int heartSampleTimeMs) {
//
// if(!name) {
// printf("Error: name.\n");
// ecmcCANOpenAddMasterPrintHelp();
// return asynError;
// }
//
// if(strcmp(name,"-h") == 0 || strcmp(name,"--help") == 0 ) {
// ecmcCANOpenAddMasterPrintHelp();
// return asynSuccess;
// }
//
// if(!grbl) {
// printf("Plugin not initialized/loaded.\n");
// return asynError;
// }
//
// try {
// grbl->addMaster((uint32_t)nodeId,
// name,
// lssSampleTimeMs,
// syncSampleTimeMs,
// heartSampleTimeMs);
// }
// catch(std::exception& e) {
// printf("Exception: %s. Add master failed.\n",e.what());
// return asynError;
// }
//
// return asynSuccess;
//}
//
//static const iocshArg initArg0_0 =
//{ "Name", iocshArgString };
//static const iocshArg initArg1_0 =
//{ "Node Id", iocshArgInt };
//static const iocshArg initArg2_0 =
//{ "LSS sample time ms", iocshArgInt };
//static const iocshArg initArg3_0 =
//{ "Sync sample time ms", iocshArgInt };
//static const iocshArg initArg4_0 =
//{ "NMT Heart sample time ms", iocshArgInt };
//
//static const iocshArg *const initArgs_0[] = { &initArg0_0,
// &initArg1_0,
// &initArg2_0,
// &initArg3_0,
// &initArg4_0};
//
//static const iocshFuncDef initFuncDef_0 = { "ecmcCANOpenAddMaster", 5, initArgs_0 };
//static void initCallFunc_0(const iocshArgBuf *args) {
// ecmcCANOpenAddMaster(args[0].sval,
// args[1].ival,
// args[2].ival,
// args[3].ival,
// args[4].ival);
//}
//
///**
// * EPICS iocsh shell command: ecmcCANOpenAddDevice
//*/
//
//void ecmcCANOpenAddDevicePrintHelp() {
// printf("\n");
// printf(" Use ecmcCANOpenAddDevice(<name>, <node id>)\n");
// printf(" <name> : Name of device.\n");
// printf(" <node id> : CANOpen node id of device.\n");
// printf(" <NMT Heartbeat timeout ms> : Timeout for NMT Heartbeat.\n");
// printf("\n");
//}
//
//int ecmcCANOpenAddDevice(const char* name, int nodeId,int heartTimeOutMs) {
// if(!name) {
// printf("Error: name.\n");
// ecmcCANOpenAddDevicePrintHelp();
// return asynError;
// }
//
// if(strcmp(name,"-h") == 0 || strcmp(name,"--help") == 0 ) {
// ecmcCANOpenAddDevicePrintHelp();
// return asynSuccess;
// }
//
// if(!grbl) {
// printf("Plugin not initialized/loaded.\n");
// return asynError;
// }
//
// if(heartTimeOutMs < 0) {
// printf("Invalid NMT heartbeat timeout.\n");
// return asynError;
// }
//
// try {
// grbl->addDevice((uint32_t)nodeId,name,heartTimeOutMs);
// }
// catch(std::exception& e) {
// printf("Exception: %s. Add device failed.\n",e.what());
// return asynError;
// }
//
// return asynSuccess;
//}
//
//static const iocshArg initArg0_1 =
//{ "Name", iocshArgString };
//static const iocshArg initArg1_1 =
//{ "Node Id", iocshArgInt };
//static const iocshArg initArg2_1 =
//{ "NMT Heart timeout ms", iocshArgInt };
//
//static const iocshArg *const initArgs_1[] = { &initArg0_1,
// &initArg1_1,
// &initArg2_1};
//
//static const iocshFuncDef initFuncDef_1 = { "ecmcCANOpenAddDevice", 3, initArgs_1 };
//static void initCallFunc_1(const iocshArgBuf *args) {
// ecmcCANOpenAddDevice(args[0].sval, args[1].ival, args[2].ival);
//}
//
///**
// * EPICS iocsh shell command: ecmcCANOpenAddSDO
//*/
//
//void ecmcCANOpenAddSDOPrintHelp() {
// printf("\n");
// printf(" Use ecmcCANOpenAddSDO(<name>, <node id>,.....)\n");
// printf(" <name> : Name of master device.\n");
// printf(" <node id> : CANOpen node id of device/master.\n");
// printf(" <cob id tx> : CANOpen cob id of Tx of slave SDO.\n");
// printf(" <cob id rx> : CANOpen cob id of Rx of slave SDO.\n");
// printf(" <dir> : Direction 1=write and 2=read.\n");
// printf(" <ODIndex> : OD index of SDO.\n");
// printf(" <ODSubIndex> : OD sub index of SDO.\n");
// printf(" <ODSize> : OS Size.\n");
// printf(" <readSampleTimeMs>: Sample time for read in ms (write is always on demand).\n");
// printf("\n");
//}
//
//int ecmcCANOpenAddSDO(const char* name,
// int nodeId,
// int cobIdTx,
// int cobIdRx,
// int dir,
// int ODIndex,
// int ODSubIndex,
// int ODSize,
// int readSampleTimeMs) {
// if(!name) {
// printf("Error: name.\n");
// ecmcCANOpenAddSDOPrintHelp();
// return asynError;
// }
//
// if(strcmp(name,"-h") == 0 || strcmp(name,"--help") == 0 ) {
// ecmcCANOpenAddSDOPrintHelp();
// return asynSuccess;
// }
//
// if(cobIdRx < 0) {
// printf("Error: invalid cobIdRx.\n");
// ecmcCANOpenAddSDOPrintHelp();
// return asynError;
// }
//
// if(cobIdTx < 0) {
// printf("Error: invalid cobIdTx.\n");
// ecmcCANOpenAddSDOPrintHelp();
// return asynError;
// }
//
// if(dir > 2 || dir <= 0) {
// printf("Error: invalid dir.\n");
// ecmcCANOpenAddSDOPrintHelp();
// return asynError;
// }
//
// if(ODIndex < 0) {
// printf("Error: invalid ODIndex.\n");
// ecmcCANOpenAddSDOPrintHelp();
// return asynError;
// }
//
// if(ODSubIndex < 0) {
// printf("Error: invalid ODSubIndex.\n");
// ecmcCANOpenAddSDOPrintHelp();
// return asynError;
// }
//
// if(ODSize < 0) {
// printf("Error: invalid ODSize.\n");
// ecmcCANOpenAddSDOPrintHelp();
// return asynError;
// }
//
// if(readSampleTimeMs < 0) {
// printf("Error: invalid readSampleTimeMs.\n");
// ecmcCANOpenAddSDOPrintHelp();
// return asynError;
// }
//
// ecmc_can_direction tempDir = DIR_READ;
// if(dir == 1) {
// tempDir = DIR_WRITE;
// }
//
// try {
// grbl->addSDO((uint32_t)nodeId,
// cobIdTx,
// cobIdRx,
// tempDir,
// ODIndex,
// ODSubIndex,
// ODSize,
// readSampleTimeMs,
// name);
//
//
// }
// catch(std::exception& e) {
// printf("Exception: %s. Add PDO failed.\n",e.what());
// return asynError;
// }
// return asynSuccess;
//}
//
//static const iocshArg initArg0_2 =
//{ "Name", iocshArgString };
//static const iocshArg initArg1_2 =
//{ "Node Id", iocshArgInt };
//static const iocshArg initArg2_2 =
//{ "COB id TX", iocshArgInt };
//static const iocshArg initArg3_2 =
//{ "COB id RX", iocshArgInt };
//static const iocshArg initArg4_2 =
//{ "Direction", iocshArgInt };
//static const iocshArg initArg5_2 =
//{ "OD Index", iocshArgInt };
//static const iocshArg initArg6_2 =
//{ "OD sub index", iocshArgInt };
//static const iocshArg initArg7_2 =
//{ "OD size", iocshArgInt };
//static const iocshArg initArg8_2 =
//{ "Read sample time ms", iocshArgInt };
//
//static const iocshArg *const initArgs_2[] = { &initArg0_2,
// &initArg1_2,
// &initArg2_2,
// &initArg3_2,
// &initArg4_2,
// &initArg5_2,
// &initArg6_2,
// &initArg7_2,
// &initArg8_2};
//
//static const iocshFuncDef initFuncDef_2 = { "ecmcCANOpenAddSDO", 9, initArgs_2 };
//static void initCallFunc_2(const iocshArgBuf *args) {
// ecmcCANOpenAddSDO(args[0].sval,
// args[1].ival,
// args[2].ival,
// args[3].ival,
// args[4].ival,
// args[5].ival,
// args[6].ival,
// args[7].ival,
// args[8].ival);
//}
//
///**
// * EPICS iocsh shell command: ecmcCANOpenAddPDO
//*/
//void ecmcCANOpenAddPDOPrintHelp() {
// printf("\n");
// printf(" Use \"ecmcCANOpenAddPDO(<name>, <node id>\n");
// printf(" <name> : Name of master device.\n");
// printf(" <node id> : CANOpen node id of device/master.\n");
// printf(" <cob id> : CANOpen cob id of PDO.\n");
// printf(" <dir> : Direction 1=write and 2=read.\n");
// printf(" <ODSize> : Size of PDO (max 8 bytes).\n");
// printf(" <readTimeoutMs> : Readtimeout in ms.\n");
// printf(" <writeCycleMs> : Cycle time for write (if <= 0 then only write on change).\n");
// printf("\n");
//}
//
//int ecmcCANOpenAddPDO(const char* name,
// int nodeId,
// int cobId,
// int dir,
// int ODSize,
// int readTimeoutMs,
// int writeCycleMs) {
// if(!name) {
// printf("Error: name.\n");
// ecmcCANOpenAddPDOPrintHelp();
// return asynError;
// }
//
// if(strcmp(name,"-h") == 0 || strcmp(name,"--help") == 0 ) {
// ecmcCANOpenAddPDOPrintHelp();
// return asynSuccess;
// }
//
// if(dir > 2 || dir <= 0) {
// printf("Error: invalid dir.\n");
// ecmcCANOpenAddPDOPrintHelp();
// return asynError;
// }
//
// if(ODSize < 0) {
// printf("Error: invalid ODSize.\n");
// ecmcCANOpenAddPDOPrintHelp();
// return asynError;
// }
//
// if(readTimeoutMs < 0) {
// printf("Error: invalid readTimeoutMs.\n");
// ecmcCANOpenAddPDOPrintHelp();
// return asynError;
// }
//
// if(writeCycleMs < 0) {
// printf("Error: invalid writeCycleMs.\n");
// ecmcCANOpenAddPDOPrintHelp();
// return asynError;
// }
//
// ecmc_can_direction tempDir = DIR_READ;
// if(dir == 1) {
// tempDir = DIR_WRITE;
// }
//
// try {
// grbl->addPDO((uint32_t)nodeId,
// cobId,
// tempDir,
// ODSize,
// readTimeoutMs,
// writeCycleMs,name);
// }
// catch(std::exception& e) {
// printf("Exception: %s. Add PDO failed.\n",e.what());
// return asynError;
// }
// return asynSuccess;
//}
//
//static const iocshArg initArg0_3 =
//{ "Name", iocshArgString };
//static const iocshArg initArg1_3 =
//{ "Node Id", iocshArgInt };
//static const iocshArg initArg2_3 =
//{ "COB Id", iocshArgInt };
//static const iocshArg initArg3_3 =
//{ "Direction", iocshArgInt };
//static const iocshArg initArg4_3 =
//{ "ODSize", iocshArgInt };
//static const iocshArg initArg5_3 =
//{ "Read Timeout ms", iocshArgInt };
//static const iocshArg initArg6_3 =
//{ "Write cycle ms", iocshArgInt };
//
//static const iocshArg *const initArgs_3[] = { &initArg0_3,
// &initArg1_3,
// &initArg2_3,
// &initArg3_3,
// &initArg4_3,
// &initArg5_3,
// &initArg6_3
// };
//
//static const iocshFuncDef initFuncDef_3 = { "ecmcCANOpenAddPDO", 7, initArgs_3 };
//static void initCallFunc_3(const iocshArgBuf *args) {
// ecmcCANOpenAddPDO(args[0].sval,
// args[1].ival,
// args[2].ival,
// args[3].ival,
// args[4].ival,
// args[5].ival,
// args[6].ival);
//}
//
///**
// * Register all functions
//*/
//void ecmcCANPluginDriverRegister(void) {
// iocshRegister(&initFuncDef_0, initCallFunc_0); // ecmcCANOpenAddMaster
// iocshRegister(&initFuncDef_1, initCallFunc_1); // ecmcCANOpenAddDevice
// iocshRegister(&initFuncDef_2, initCallFunc_2); // ecmcCANOpenAddSDO
// iocshRegister(&initFuncDef_3, initCallFunc_3); // ecmcCANOpenAddPDO
//}
//
//epicsExportRegistrar(ecmcCANPluginDriverRegister);
//

38
ecmc_plugin_grbl/ecmcGrblWrap.h
View File

@@ -11,7 +11,8 @@
\*************************************************************************/
#ifndef ECMC_GRBL_WRAP_H_
#define ECMC_GRBL_WRAP_H_
#include "ecmcSocketCANDefs.h"
#include "ecmcGrblDefs.h"
# ifdef __cplusplus
extern "C" {
@@ -27,43 +28,14 @@ extern "C" {
*
* \return 0 if success or otherwise an error code.\n
*/
int createSocketCAN(char *configStr, int exeSampleTimeMs);
/** \brief Connect to SocketCAN interface\n
*/
int connectSocketCAN();
/** \brief Connected to can interface\n
*/
int getSocketCANConnectd();
/** \brief Get last error from writes\n
*/
int getlastWritesError();
int createGrbl(char *configStr, int exeSampleTimeMs);
/** \brief execute from rt loop\n
*/
int execute();
/** \brief add CAN frame to write buffer
*/
int addWriteSocketCAN( double canId,
double len,
double data0,
double data1,
double data2,
double data3,
double data4,
double data5,
double data6,
double data7);
/** \brief Delete SocketCAN object\n
*
* Should be called when destructs.\n
*/
void deleteSocketCAN();
// Delete object
void deleteGrbl();
# ifdef __cplusplus
}

256
ecmc_plugin_grbl/ecmcPluginGrbl.c
View File

@@ -29,6 +29,7 @@ extern "C" {
#include "ecmcPluginDefs.h"
#include "ecmcPluginClient.h"
#include "ecmcGrblDefs.h"
#include "ecmcGrblWrap.h"
#include <stdio.h>
#include <stdlib.h>
@@ -43,84 +44,84 @@ pthread_t tid;
void *ecmc_grbl_client_simulation_thread(void *ptr) {
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());
}
// 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());
// }
}
// copied for grbl main.c
void *ecmc_grbl_main_thread(void *ptr) {
printf("%s:%s:%d\n",__FILE__,__FUNCTION__,__LINE__);
// Initialize system upon power-up.
serial_init(); // Setup serial baud rate and interrupts
ecmc_init_file(); // create and clear file (simulated eeprom)
settings_restore(0b1111); // restore all to defaults
settings_init(); // Load Grbl settings from EEPROM
stepper_init(); // Configure stepper pins and interrupt timers
system_init(); // Configure pinout pins and pin-change interrupt
memset(sys_position,0,sizeof(sys_position)); // Clear machine position.
//sei(); // Enable interrupts
// Initialize system state.
#ifdef FORCE_INITIALIZATION_ALARM
// Force Grbl into an ALARM state upon a power-cycle or hard reset.
sys.state = STATE_ALARM;
#else
sys.state = STATE_IDLE;
#endif
// Check for power-up and set system alarm if homing is enabled to force homing cycle
// by setting Grbl's alarm state. Alarm locks out all g-code commands, including the
// startup scripts, but allows access to settings and internal commands. Only a homing
// cycle '$H' or kill alarm locks '$X' will disable the alarm.
// NOTE: The startup script will run after successful completion of the homing cycle, but
// not after disabling the alarm locks. Prevents motion startup blocks from crashing into
// things uncontrollably. Very bad.
#ifdef HOMING_INIT_LOCK
if (bit_istrue(settings.flags,BITFLAG_HOMING_ENABLE)) { sys.state = STATE_ALARM; }
#endif
// Grbl initialization loop upon power-up or a system abort. For the latter, all processes
// will return to this loop to be cleanly re-initialized.
for(;;) {
// Reset system variables.
uint8_t prior_state = sys.state;
memset(&sys, 0, sizeof(system_t)); // Clear system struct variable.
sys.state = prior_state;
sys.f_override = DEFAULT_FEED_OVERRIDE; // Set to 100%
sys.r_override = DEFAULT_RAPID_OVERRIDE; // Set to 100%
sys.spindle_speed_ovr = DEFAULT_SPINDLE_SPEED_OVERRIDE; // Set to 100%
memset(sys_probe_position,0,sizeof(sys_probe_position)); // Clear probe position.
sys_probe_state = 0;
sys_rt_exec_state = 0;
sys_rt_exec_alarm = 0;
sys_rt_exec_motion_override = 0;
sys_rt_exec_accessory_override = 0;
// Reset Grbl primary systems.
serial_reset_read_buffer(); // Clear serial read buffer
gc_init(); // Set g-code parser to default state
spindle_init();
coolant_init();
limits_init();
probe_init();
plan_reset(); // Clear block buffer and planner variables
st_reset(); // Clear stepper subsystem variables.
// Sync cleared gcode and planner positions to current system position.
plan_sync_position();
gc_sync_position();
// Print welcome message. Indicates an initialization has occured at power-up or with a reset.
report_init_message();
// ready for commands through serial interface
initDone = 1;
protocol_main_loop();
}
//printf("%s:%s:%d\n",__FILE__,__FUNCTION__,__LINE__);
//
//// Initialize system upon power-up.
//serial_init(); // Setup serial baud rate and interrupts
//ecmc_init_file(); // create and clear file (simulated eeprom)
//settings_restore(0b1111); // restore all to defaults
//settings_init(); // Load Grbl settings from EEPROM
//stepper_init(); // Configure stepper pins and interrupt timers
//system_init(); // Configure pinout pins and pin-change interrupt
//memset(sys_position,0,sizeof(sys_position)); // Clear machine position.
////sei(); // Enable interrupts
//// Initialize system state.
//#ifdef FORCE_INITIALIZATION_ALARM
// // Force Grbl into an ALARM state upon a power-cycle or hard reset.
// sys.state = STATE_ALARM;
//#else
// sys.state = STATE_IDLE;
//#endif
//
//// Check for power-up and set system alarm if homing is enabled to force homing cycle
//// by setting Grbl's alarm state. Alarm locks out all g-code commands, including the
//// startup scripts, but allows access to settings and internal commands. Only a homing
//// cycle '$H' or kill alarm locks '$X' will disable the alarm.
//// NOTE: The startup script will run after successful completion of the homing cycle, but
//// not after disabling the alarm locks. Prevents motion startup blocks from crashing into
//// things uncontrollably. Very bad.
//#ifdef HOMING_INIT_LOCK
// if (bit_istrue(settings.flags,BITFLAG_HOMING_ENABLE)) { sys.state = STATE_ALARM; }
//#endif
// // Grbl initialization loop upon power-up or a system abort. For the latter, all processes
//// will return to this loop to be cleanly re-initialized.
//for(;;) {
// // Reset system variables.
// uint8_t prior_state = sys.state;
// memset(&sys, 0, sizeof(system_t)); // Clear system struct variable.
// sys.state = prior_state;
// sys.f_override = DEFAULT_FEED_OVERRIDE; // Set to 100%
// sys.r_override = DEFAULT_RAPID_OVERRIDE; // Set to 100%
// sys.spindle_speed_ovr = DEFAULT_SPINDLE_SPEED_OVERRIDE; // Set to 100%
// memset(sys_probe_position,0,sizeof(sys_probe_position)); // Clear probe position.
// sys_probe_state = 0;
// sys_rt_exec_state = 0;
// sys_rt_exec_alarm = 0;
// sys_rt_exec_motion_override = 0;
// sys_rt_exec_accessory_override = 0;
//
// // Reset Grbl primary systems.
// serial_reset_read_buffer(); // Clear serial read buffer
// gc_init(); // Set g-code parser to default state
// spindle_init();
// coolant_init();
// limits_init();
// probe_init();
// plan_reset(); // Clear block buffer and planner variables
// st_reset(); // Clear stepper subsystem variables.
//
// // Sync cleared gcode and planner positions to current system position.
// plan_sync_position();
// gc_sync_position();
//
// // Print welcome message. Indicates an initialization has occured at power-up or with a reset.
// report_init_message();
//
// // ready for commands through serial interface
// initDone = 1;
// protocol_main_loop();
//}
}
/** Optional.
@@ -130,64 +131,73 @@ void *ecmc_grbl_main_thread(void *ptr) {
**/
int grblConstruct(char *configStr)
{
// only allow one loaded module
if(alreadyLoaded) {
return 1;
}
alreadyLoaded = 1;
// create SocketCAN object and register data callback
// create grbl object and register data callback
lastConfStr = strdup(configStr);
return createGrbl(lastConfStr, getEcmcSampleTimeMS());
// start grbl simulated client thread!
int err;
err = pthread_create(&(tid), NULL, *ecmc_grbl_client_simulation_thread, NULL);
if (err != 0) {
printf("\n Can't create thread :[%s]", strerror(err));
return 1;
}
else {
printf("\n grbl simulated client thread created successfully\n");
}
// start grbl main thread and wait for init done!
printf("%s:%s:%d\n",__FILE__,__FUNCTION__,__LINE__);
err = pthread_create(&(tid), NULL, *ecmc_grbl_main_thread, NULL);
if (err != 0) {
printf("\n Can't create thread :[%s]", strerror(err));
return 1;
}
else {
printf("\n grbl main thread created successfully\n");
}
// int err;
//
// err = pthread_create(&(tid), NULL, *ecmc_grbl_client_simulation_thread, NULL);
// if (err != 0) {
// printf("\n Can't create thread :[%s]", strerror(err));
// return 1;
// }
// else {
// printf("\n grbl simulated client thread created successfully\n");
// }
//
// // start grbl main thread and wait for init done!
// printf("%s:%s:%d\n",__FILE__,__FUNCTION__,__LINE__);
//
// err = pthread_create(&(tid), NULL, *ecmc_grbl_main_thread, NULL);
// if (err != 0) {
// printf("\n Can't create thread :[%s]", strerror(err));
// return 1;
// }
// else {
// printf("\n grbl main thread created successfully\n");
// }
// whait for initDone!
printf("Waiting for grbl init..");
while(!initDone) {
sleep(1);
printf(".");
}
// printf("Waiting for grbl init..");
// while(!initDone) {
// sleep(1);
// printf(".");
// }
printf("\n");
printf("\n grbl ready for commands!\n");
sleep(1);
// test some commands
printf("Test command:G0X10Y100\n");
ecmc_write_command_serial("G0X10Y100\n");
printf("Test command:G4P4\n");
ecmc_write_command_serial("G4P4\n");
printf("Test command:G1X20Y200F20\n");
ecmc_write_command_serial("G1X20Y200F20\n");
printf("Test command:G4P4\n");
ecmc_write_command_serial("G4P4\n");
printf("Test command:G2X40Y220R20\n");
ecmc_write_command_serial("G2X40Y220R20\n");
printf("Test command:$\n");
ecmc_write_command_serial("$\n");
// printf("\n");
// printf("\n grbl ready for commands!\n");
// sleep(1);
//
// // test some commands
// printf("Test command:$\n");
// ecmc_write_command_serial("$\n");
// sleep(1);
// printf("Test command:G0X10Y100\n");
// ecmc_write_command_serial("G0X10Y100\n");
// sleep(1);
// printf("Test command:$G\n");
// ecmc_write_command_serial("$G\n");
// sleep(1);
// printf("Test command:G4P4\n");
// ecmc_write_command_serial("G4P4\n");
// printf("Test command:G1X20Y200F20\n");
// ecmc_write_command_serial("G1X20Y200F20\n");
// printf("Test command:G4P4\n");
// ecmc_write_command_serial("G4P4\n");
// printf("Test command:G2X40Y220R20\n");
// ecmc_write_command_serial("G2X40Y220R20\n");
// printf("Test command:$\n");
// ecmc_write_command_serial("$\n");
//
//printf("Test command:$J=X10.0Y-1.5\n");
//ecmc_write_command_serial("$J=X10.0Y-1.5\0");
@@ -197,7 +207,7 @@ int grblConstruct(char *configStr)
//ecmc_write_command_serial("?\0");
//printf("Test command:G1X200Y100\n");
//ecmc_write_command_serial("G1X200Y100\0");
return 0;
// printf("system_execute_line(G0 X11), %d \n ",system_execute_line("G0X11\0"));
// printf("end\n");
@@ -229,7 +239,7 @@ void grblDestruct(void)
if(lastConfStr){
free(lastConfStr);
}
//deleteSocketCAN();
deleteGrbl();
}
/** Optional function.

4
grbl/limits.c
View File

@@ -190,8 +190,8 @@ void limits_go_home(uint8_t cycle_mask)
// #else
// float fail_distance = (-DUAL_AXIS_HOMING_FAIL_AXIS_LENGTH_PERCENT/100.0)*settings.max_travel[X_AXIS];
// #endif
// fail_distance = min(fail_distance, DUAL_AXIS_HOMING_FAIL_DISTANCE_MAX);
// fail_distance = max(fail_distance, DUAL_AXIS_HOMING_FAIL_DISTANCE_MIN);
// fail_distance = min_grbl(fail_distance, DUAL_AXIS_HOMING_FAIL_DISTANCE_MAX);
// fail_distance = max_grbl(fail_distance, DUAL_AXIS_HOMING_FAIL_DISTANCE_MIN);
// int32_t dual_fail_distance = trunc(fail_distance*settings.steps_per_mm[DUAL_AXIS_SELECT]);
// // int32_t dual_fail_distance = trunc((DUAL_AXIS_HOMING_TRIGGER_FAIL_DISTANCE)*settings.steps_per_mm[DUAL_AXIS_SELECT]);
// #endif

2
grbl/nuts_bolts.c
View File

@@ -211,7 +211,7 @@ float limit_value_by_axis_maximum(float *max_value, float *unit_vec)
float limit_value = SOME_LARGE_VALUE;
for (idx=0; idx<N_AXIS; idx++) {
if (unit_vec[idx] != 0) { // Avoid divide by zero.
limit_value = min(limit_value,fabs(max_value[idx]/unit_vec[idx]));
limit_value = min_grbl(limit_value,fabs(max_value[idx]/unit_vec[idx]));
}
}
return(limit_value);

4
grbl/nuts_bolts.h
View File

@@ -54,8 +54,8 @@
#define clear_vector(a) memset(a, 0, sizeof(a))
#define clear_vector_float(a) memset(a, 0.0, sizeof(float)*N_AXIS)
// #define clear_vector_long(a) memset(a, 0.0, sizeof(long)*N_AXIS)
#define max(a,b) (((a) > (b)) ? (a) : (b))
#define min(a,b) (((a) < (b)) ? (a) : (b))
#define max_grbl(a,b) (((a) > (b)) ? (a) : (b))
#define min_grbl(a,b) (((a) < (b)) ? (a) : (b))
#define isequal_position_vector(a,b) !(memcmp(a, b, sizeof(float)*N_AXIS))
// Bit field and masking macros

8
grbl/planner.c
View File

@@ -139,7 +139,7 @@ static void planner_recalculate()
plan_block_t *current = &block_buffer[block_index];
// Calculate maximum entry speed for last block in buffer, where the exit speed is always zero.
current->entry_speed_sqr = min( current->max_entry_speed_sqr, 2*current->acceleration*current->millimeters);
current->entry_speed_sqr = min_grbl( current->max_entry_speed_sqr, 2*current->acceleration*current->millimeters);
block_index = plan_prev_block_index(block_index);
if (block_index == block_buffer_planned) { // Only two plannable blocks in buffer. Reverse pass complete.
@@ -357,7 +357,7 @@ uint8_t plan_buffer_line(float *target, plan_line_data_t *pl_data)
target_steps[idx] = lround(target[idx]*settings.steps_per_mm[idx]);
block->steps[idx] = labs(target_steps[idx]-position_steps[idx]);
}
block->step_event_count = max(block->step_event_count, block->steps[idx]);
block->step_event_count = max_grbl(block->step_event_count, block->steps[idx]);
if (idx == A_MOTOR) {
delta_mm = (target_steps[X_AXIS]-position_steps[X_AXIS] + target_steps[Y_AXIS]-position_steps[Y_AXIS])/settings.steps_per_mm[idx];
} else if (idx == B_MOTOR) {
@@ -368,7 +368,7 @@ uint8_t plan_buffer_line(float *target, plan_line_data_t *pl_data)
#else
target_steps[idx] = lround(target[idx]*settings.steps_per_mm[idx]);
block->steps[idx] = labs(target_steps[idx]-position_steps[idx]);
block->step_event_count = max(block->step_event_count, block->steps[idx]);
block->step_event_count = max_grbl(block->step_event_count, block->steps[idx]);
delta_mm = (target_steps[idx] - position_steps[idx])/settings.steps_per_mm[idx];
#endif
unit_vec[idx] = delta_mm; // Store unit vector numerator
@@ -445,7 +445,7 @@ uint8_t plan_buffer_line(float *target, plan_line_data_t *pl_data)
convert_delta_vector_to_unit_vector(junction_unit_vec);
float junction_acceleration = limit_value_by_axis_maximum(settings.acceleration, junction_unit_vec);
float sin_theta_d2 = sqrt(0.5*(1.0-junction_cos_theta)); // Trig half angle identity. Always positive.
block->max_junction_speed_sqr = max( MINIMUM_JUNCTION_SPEED*MINIMUM_JUNCTION_SPEED,
block->max_junction_speed_sqr = max_grbl( MINIMUM_JUNCTION_SPEED*MINIMUM_JUNCTION_SPEED,
(junction_acceleration * settings.junction_deviation * sin_theta_d2)/(1.0-sin_theta_d2) );
}
}

10
grbl/protocol.c
View File

@@ -414,8 +414,8 @@ void protocol_exec_rt_system()
if (rt_exec & EXEC_FEED_OVR_COARSE_MINUS) { new_f_override -= FEED_OVERRIDE_COARSE_INCREMENT; }
if (rt_exec & EXEC_FEED_OVR_FINE_PLUS) { new_f_override += FEED_OVERRIDE_FINE_INCREMENT; }
if (rt_exec & EXEC_FEED_OVR_FINE_MINUS) { new_f_override -= FEED_OVERRIDE_FINE_INCREMENT; }
new_f_override = min(new_f_override,MAX_FEED_RATE_OVERRIDE);
new_f_override = max(new_f_override,MIN_FEED_RATE_OVERRIDE);
new_f_override = min_grbl(new_f_override,MAX_FEED_RATE_OVERRIDE);
new_f_override = max_grbl(new_f_override,MIN_FEED_RATE_OVERRIDE);
uint8_t new_r_override = sys.r_override;
if (rt_exec & EXEC_RAPID_OVR_RESET) { new_r_override = DEFAULT_RAPID_OVERRIDE; }
@@ -442,8 +442,8 @@ void protocol_exec_rt_system()
if (rt_exec & EXEC_SPINDLE_OVR_COARSE_MINUS) { last_s_override -= SPINDLE_OVERRIDE_COARSE_INCREMENT; }
if (rt_exec & EXEC_SPINDLE_OVR_FINE_PLUS) { last_s_override += SPINDLE_OVERRIDE_FINE_INCREMENT; }
if (rt_exec & EXEC_SPINDLE_OVR_FINE_MINUS) { last_s_override -= SPINDLE_OVERRIDE_FINE_INCREMENT; }
last_s_override = min(last_s_override,MAX_SPINDLE_SPEED_OVERRIDE);
last_s_override = max(last_s_override,MIN_SPINDLE_SPEED_OVERRIDE);
last_s_override = min_grbl(last_s_override,MAX_SPINDLE_SPEED_OVERRIDE);
last_s_override = max_grbl(last_s_override,MIN_SPINDLE_SPEED_OVERRIDE);
if (last_s_override != sys.spindle_speed_ovr) {
sys.spindle_speed_ovr = last_s_override;
@@ -573,7 +573,7 @@ static void protocol_exec_rt_suspend()
if (bit_isfalse(sys.suspend,SUSPEND_RESTART_RETRACT)) {
memcpy(restore_target,parking_target,sizeof(parking_target));
retract_waypoint += restore_target[PARKING_AXIS];
retract_waypoint = min(retract_waypoint,PARKING_TARGET);
retract_waypoint = min_grbl(retract_waypoint,PARKING_TARGET);
}
// Execute slow pull-out parking retract motion. Parking requires homing enabled, the

33648
iocsh/log.log
View File

File diff suppressed because it is too large Load Diff

2
iocsh/test.script
View File

@@ -25,7 +25,7 @@ epicsEnvSet("PLUGIN_VER" ,"develop")
require ecmc_plugin_grbl $(PLUGIN_VER)
epicsEnvSet(ECMC_PLUGIN_FILNAME,"/home/pi/epics/base-7.0.5/require/${E3_REQUIRE_VERSION}/siteMods/ecmc_plugin_grbl/$(PLUGIN_VER)/lib/${EPICS_HOST_ARCH=linux-x86_64}/libecmc_plugin_grbl.so")
epicsEnvSet(ECMC_PLUGIN_CONFIG,"DBG_PRINT=1;") # Only one option implemented in this plugin
epicsEnvSet(ECMC_PLUGIN_CONFIG,"DBG_PRINT=1;X_AXIS=1;") # Only one option implemented in this plugin
${SCRIPTEXEC} ${ecmccfg_DIR}loadPlugin.cmd, "PLUGIN_ID=0,FILE=${ECMC_PLUGIN_FILNAME},CONFIG='${ECMC_PLUGIN_CONFIG}', REPORT=1"
epicsEnvUnset(ECMC_PLUGIN_FILNAME)
epicsEnvUnset(ECMC_PLUGIN_CONFIG)