First execute as ecmc plugin..
This commit is contained in:
Anders Sandstrom
468
ecmc_plugin_grbl/ecmcGrbl.cpp
Normal file
468
ecmc_plugin_grbl/ecmcGrbl.cpp
Normal file
@@ -0,0 +1,468 @@
|
||||
/*************************************************************************\
|
||||
* Copyright (c) 2019 European Spallation Source ERIC
|
||||
* ecmc is distributed subject to a Software License Agreement found
|
||||
* in file LICENSE that is included with this distribution.
|
||||
*
|
||||
* ecmcSocketCAN.cpp
|
||||
*
|
||||
* Created on: Mar 22, 2020
|
||||
* Author: anderssandstrom
|
||||
* Credits to https://github.com/sgreg/dynamic-loading
|
||||
*
|
||||
\*************************************************************************/
|
||||
|
||||
// Needed to get headers in ecmc right...
|
||||
#define ECMC_IS_PLUGIN
|
||||
|
||||
#include <sstream>
|
||||
#include "ecmcSocketCAN.h"
|
||||
#include "ecmcPluginClient.h"
|
||||
#include "ecmcAsynPortDriver.h"
|
||||
#include "ecmcAsynPortDriverUtils.h"
|
||||
#include "epicsThread.h"
|
||||
|
||||
// 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",
|
||||
__FILE__, __FUNCTION__, __LINE__);
|
||||
return;
|
||||
}
|
||||
ecmcSocketCAN * canObj = (ecmcSocketCAN*)obj;
|
||||
canObj->doReadWorker();
|
||||
}
|
||||
|
||||
// Start worker for socket connect()
|
||||
void f_worker_connect(void *obj) {
|
||||
if(!obj) {
|
||||
printf("%s/%s:%d: Error: Worker connect thread ecmcSocketCAN object NULL..\n",
|
||||
__FILE__, __FUNCTION__, __LINE__);
|
||||
return;
|
||||
}
|
||||
ecmcSocketCAN * canObj = (ecmcSocketCAN*)obj;
|
||||
canObj->doConnectWorker();
|
||||
}
|
||||
|
||||
/** ecmc ecmcSocketCAN 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;
|
||||
}
|
||||
|
||||
exeSampleTimeMs_ = exeSampleTimeMs;
|
||||
|
||||
memset(&ifr_,0,sizeof(struct ifreq));
|
||||
memset(&rxmsg_,0,sizeof(struct can_frame));
|
||||
memset(&addr_,0,sizeof(struct sockaddr_can));
|
||||
|
||||
parseConfigStr(configStr); // Assigns all configs
|
||||
// Check valid nfft
|
||||
if(!cfgCanIFStr_ ) {
|
||||
throw std::out_of_range("CAN inteface must be defined (can0, vcan0...).");
|
||||
}
|
||||
|
||||
// Create worker thread for reading socket
|
||||
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().");
|
||||
}
|
||||
|
||||
if(cfgAutoConnect_) {
|
||||
connectPrivate();
|
||||
}
|
||||
writeBuffer_ = new ecmcSocketCANWriteBuffer(socketId_, cfgDbgMode_);
|
||||
initAsyn();
|
||||
}
|
||||
|
||||
ecmcSocketCAN::~ecmcSocketCAN() {
|
||||
// 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) {
|
||||
|
||||
// check config parameters
|
||||
if (configStr && configStr[0]) {
|
||||
char *pOptions = strdup(configStr);
|
||||
char *pThisOption = pOptions;
|
||||
char *pNextOption = pOptions;
|
||||
|
||||
while (pNextOption && pNextOption[0]) {
|
||||
pNextOption = strchr(pNextOption, ';');
|
||||
if (pNextOption) {
|
||||
*pNextOption = '\0'; /* Terminate */
|
||||
pNextOption++; /* Jump to (possible) next */
|
||||
}
|
||||
|
||||
// ECMC_PLUGIN_DBG_PRINT_OPTION_CMD (1/0)
|
||||
if (!strncmp(pThisOption, ECMC_PLUGIN_DBG_PRINT_OPTION_CMD, strlen(ECMC_PLUGIN_DBG_PRINT_OPTION_CMD))) {
|
||||
pThisOption += strlen(ECMC_PLUGIN_DBG_PRINT_OPTION_CMD);
|
||||
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_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);
|
||||
}
|
||||
|
||||
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");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
void ecmcSocketCAN::execute() {
|
||||
|
||||
for(int i = 0; i < deviceCounter_; i++){
|
||||
devices_[i]->execute();
|
||||
}
|
||||
|
||||
int writeError=getlastWritesError();
|
||||
if (writeError) {
|
||||
errorCode_ = writeError;
|
||||
refreshNeeded_ = 1;
|
||||
}
|
||||
refreshAsynParams();
|
||||
return;
|
||||
}
|
||||
|
||||
// Avoid issues with std:to_string()
|
||||
std::string ecmcSocketCAN::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) {
|
||||
|
||||
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.");
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
148
ecmc_plugin_grbl/ecmcGrbl.h
Normal file
148
ecmc_plugin_grbl/ecmcGrbl.h
Normal file
@@ -0,0 +1,148 @@
|
||||
/*************************************************************************\
|
||||
* Copyright (c) 2019 European Spallation Source ERIC
|
||||
* ecmc is distributed subject to a Software License Agreement found
|
||||
* in file LICENSE that is included with this distribution.
|
||||
*
|
||||
* ecmcGrbl.h
|
||||
*
|
||||
* Created on: jan 12, 2022
|
||||
* Author: anderssandstrom
|
||||
*
|
||||
\*************************************************************************/
|
||||
#ifndef ECMC_GRBL_H_
|
||||
#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 "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 {
|
||||
public:
|
||||
|
||||
/** ecmc ecmcSocketCAN class
|
||||
* This object can throw:
|
||||
* - bad_alloc
|
||||
* - invalid_argument
|
||||
* - runtime_error
|
||||
* - out_of_range
|
||||
*/
|
||||
ecmcSocketCAN(char* configStr,
|
||||
char* portName,
|
||||
int exeSampelTimeMs);
|
||||
~ecmcSocketCAN();
|
||||
|
||||
void doReadWorker();
|
||||
void doWriteWorker();
|
||||
void doConnectWorker();
|
||||
|
||||
//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 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 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_;
|
||||
};
|
||||
|
||||
#endif /* ECMC_GRBL_H_ */
|
||||
38
ecmc_plugin_grbl/ecmcGrblDefs.h
Normal file
38
ecmc_plugin_grbl/ecmcGrblDefs.h
Normal file
@@ -0,0 +1,38 @@
|
||||
/*************************************************************************\
|
||||
* Copyright (c) 2019 European Spallation Source ERIC
|
||||
* ecmc is distributed subject to a Software License Agreement found
|
||||
* in file LICENSE that is included with this distribution.
|
||||
*
|
||||
* ecmcGrblDefs.h
|
||||
*
|
||||
* Created on: Jan 12, 2022
|
||||
* Author: anderssandstrom
|
||||
*
|
||||
\*************************************************************************/
|
||||
|
||||
#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
|
||||
|
||||
|
||||
|
||||
|
||||
#endif /* ECMC_GRBL_DEFS_H_ */
|
||||
560
ecmc_plugin_grbl/ecmcGrblWrap.cpp
Normal file
560
ecmc_plugin_grbl/ecmcGrblWrap.cpp
Normal file
@@ -0,0 +1,560 @@
|
||||
/*************************************************************************\
|
||||
* Copyright (c) 2019 European Spallation Source ERIC
|
||||
* ecmc is distributed subject to a Software License Agreement found
|
||||
* in file LICENSE that is included with this distribution.
|
||||
*
|
||||
* ecmcFFTWrap.cpp
|
||||
*
|
||||
* Created on: Mar 22, 2020
|
||||
* Author: anderssandstrom
|
||||
*
|
||||
\*************************************************************************/
|
||||
|
||||
// Needed to get headers in ecmc right...
|
||||
#define ECMC_IS_PLUGIN
|
||||
|
||||
#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>
|
||||
|
||||
|
||||
#define ECMC_PLUGIN_MAX_PORTNAME_CHARS 64
|
||||
#define ECMC_PLUGIN_PORTNAME_PREFIX "PLUGIN.CAN"
|
||||
|
||||
static ecmcSocketCAN* can = NULL;
|
||||
static char portNameBuffer[ECMC_PLUGIN_MAX_PORTNAME_CHARS];
|
||||
|
||||
int createSocketCAN(char* configStr, int exeSampleTimeMs) {
|
||||
|
||||
// create new ecmcFFT object
|
||||
|
||||
// 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);
|
||||
}
|
||||
catch(std::exception& e) {
|
||||
if(can) {
|
||||
delete can;
|
||||
}
|
||||
printf("Exception: %s. Plugin will unload.\n",e.what());
|
||||
return ECMC_PLUGIN_SOCKETCAN_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();
|
||||
}
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* 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);
|
||||
72
ecmc_plugin_grbl/ecmcGrblWrap.h
Normal file
72
ecmc_plugin_grbl/ecmcGrblWrap.h
Normal file
@@ -0,0 +1,72 @@
|
||||
/*************************************************************************\
|
||||
* Copyright (c) 2019 European Spallation Source ERIC
|
||||
* ecmc is distributed subject to a Software License Agreement found
|
||||
* in file LICENSE that is included with this distribution.
|
||||
*
|
||||
* ecmcGrblWrap.h
|
||||
*
|
||||
* Created on: Jan 12, 2022
|
||||
* Author: anderssandstrom
|
||||
*
|
||||
\*************************************************************************/
|
||||
#ifndef ECMC_GRBL_WRAP_H_
|
||||
#define ECMC_GRBL_WRAP_H_
|
||||
#include "ecmcSocketCANDefs.h"
|
||||
|
||||
# ifdef __cplusplus
|
||||
extern "C" {
|
||||
# endif // ifdef __cplusplus
|
||||
|
||||
/** \brief Create new SocketCAN object
|
||||
*
|
||||
* The configuration string needs to define tha can interface by:\n
|
||||
* "IF=<data source>;"\n
|
||||
* Example:\n
|
||||
* "IF=can0";\n
|
||||
* \param[in] configStr Configuration string.\n
|
||||
*
|
||||
* \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();
|
||||
|
||||
/** \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();
|
||||
|
||||
# ifdef __cplusplus
|
||||
}
|
||||
# endif // ifdef __cplusplus
|
||||
|
||||
#endif /* ECMC_GRBL_WRAP_H_ */
|
||||
221
ecmc_plugin_grbl/ecmcPluginGrbl.c
Normal file
221
ecmc_plugin_grbl/ecmcPluginGrbl.c
Normal file
@@ -0,0 +1,221 @@
|
||||
/*************************************************************************\
|
||||
* Copyright (c) 2019 European Spallation Source ERIC
|
||||
* ecmc is distributed subject to a Software License Agreement found
|
||||
* in file LICENSE that is included with this distribution.
|
||||
*
|
||||
* ecmcPluginExample.cpp
|
||||
*
|
||||
* Created on: Mar 21, 2020
|
||||
* Author: anderssandstrom
|
||||
* Credits to https://github.com/sgreg/dynamic-loading
|
||||
*
|
||||
\*************************************************************************/
|
||||
|
||||
// Needed to get headers in ecmc right...
|
||||
#define ECMC_IS_PLUGIN
|
||||
#define ECMC_EXAMPLE_PLUGIN_VERSION 2
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif // ifdef __cplusplus
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "ecmcPluginDefs.h"
|
||||
#include "ecmcPluginClient.h"
|
||||
#include "ecmcGrblDefs.h"
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <unistd.h>
|
||||
#include <string.h>
|
||||
|
||||
static int lastEcmcError = 0;
|
||||
static char* lastConfStr = NULL;
|
||||
static int alreadyLoaded = 0;
|
||||
|
||||
/** Optional.
|
||||
* Will be called once after successfull load into ecmc.
|
||||
* Return value other than 0 will be considered error.
|
||||
* configStr can be used for configuration parameters.
|
||||
**/
|
||||
int grblConstruct(char *configStr)
|
||||
{
|
||||
if(alreadyLoaded) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
alreadyLoaded = 1;
|
||||
// create SocketCAN object and register data callback
|
||||
lastConfStr = strdup(configStr);
|
||||
|
||||
// Initialize system upon power-up.
|
||||
serial_init(); // Setup serial baud rate and interrupts
|
||||
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();
|
||||
|
||||
// Start Grbl main loop. Processes program inputs and executes them.
|
||||
protocol_main_loop();
|
||||
//}
|
||||
|
||||
return 0; //createSocketCAN(configStr,getEcmcSampleTimeMS());
|
||||
}
|
||||
|
||||
/** Optional function.
|
||||
* Will be called once at unload.
|
||||
**/
|
||||
void grblDestruct(void)
|
||||
{
|
||||
if(lastConfStr){
|
||||
free(lastConfStr);
|
||||
}
|
||||
//deleteSocketCAN();
|
||||
}
|
||||
|
||||
/** Optional function.
|
||||
* Will be called each realtime cycle if definded
|
||||
* ecmcError: Error code of ecmc. Makes it posible for
|
||||
* this plugin to react on ecmc errors
|
||||
* Return value other than 0 will be considered to be an error code in ecmc.
|
||||
**/
|
||||
int grblRealtime(int ecmcError)
|
||||
{
|
||||
lastEcmcError = ecmcError;
|
||||
return 0; //execute();
|
||||
}
|
||||
|
||||
/** Link to data source here since all sources should be availabe at this stage
|
||||
* (for example ecmc PLC variables are defined only at enter of realtime)
|
||||
**/
|
||||
int grblEnterRT(){
|
||||
return 0;
|
||||
}
|
||||
|
||||
/** Optional function.
|
||||
* Will be called once just before leaving realtime mode
|
||||
* Return value other than 0 will be considered error.
|
||||
**/
|
||||
int grblExitRT(void){
|
||||
return 0;
|
||||
}
|
||||
// Plc function for connect to can
|
||||
double grbl_connect() {
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Register data for plugin so ecmc know what to use
|
||||
struct ecmcPluginData pluginDataDef = {
|
||||
// Allways use ECMC_PLUG_VERSION_MAGIC
|
||||
.ifVersion = ECMC_PLUG_VERSION_MAGIC,
|
||||
// Name
|
||||
.name = "ecmcPluginGrbl",
|
||||
// Description
|
||||
.desc = "grbl plugin for use with ecmc.",
|
||||
// Option description
|
||||
.optionDesc = "\n "ECMC_PLUGIN_DBG_PRINT_OPTION_CMD"<1/0> : Enables/disables printouts from plugin, default = disabled (=0).\n",
|
||||
// Plugin version
|
||||
.version = ECMC_EXAMPLE_PLUGIN_VERSION,
|
||||
// Optional construct func, called once at load. NULL if not definded.
|
||||
.constructFnc = grblConstruct,
|
||||
// Optional destruct func, called once at unload. NULL if not definded.
|
||||
.destructFnc = grblDestruct,
|
||||
// Optional func that will be called each rt cycle. NULL if not definded.
|
||||
.realtimeFnc = grblRealtime,
|
||||
// Optional func that will be called once just before enter realtime mode
|
||||
.realtimeEnterFnc = grblEnterRT,
|
||||
// Optional func that will be called once just before exit realtime mode
|
||||
.realtimeExitFnc = grblExitRT,
|
||||
// PLC funcs
|
||||
.funcs[0] =
|
||||
{ /*----can_connect----*/
|
||||
// Function name (this is the name you use in ecmc plc-code)
|
||||
.funcName = "grbl_connect",
|
||||
// Function description
|
||||
.funcDesc = "double grbl_connect() : Connect to grbl interface (from config str).",
|
||||
/**
|
||||
* 7 different prototypes allowed (only doubles since reg in plc).
|
||||
* Only funcArg${argCount} func shall be assigned the rest set to NULL.
|
||||
**/
|
||||
.funcArg0 = grbl_connect,
|
||||
.funcArg1 = NULL,
|
||||
.funcArg2 = NULL,
|
||||
.funcArg3 = NULL,
|
||||
.funcArg4 = NULL,
|
||||
.funcArg5 = NULL,
|
||||
.funcArg6 = NULL,
|
||||
.funcArg7 = NULL,
|
||||
.funcArg8 = NULL,
|
||||
.funcArg9 = NULL,
|
||||
.funcArg10 = NULL,
|
||||
.funcGenericObj = NULL,
|
||||
},
|
||||
.funcs[1] = {0}, // last element set all to zero..
|
||||
// PLC consts
|
||||
.consts[0] = {0}, // last element set all to zero..
|
||||
};
|
||||
|
||||
ecmc_plugin_register(pluginDataDef);
|
||||
|
||||
# ifdef __cplusplus
|
||||
}
|
||||
# endif // ifdef __cplusplus
|
||||
@@ -57,7 +57,7 @@
|
||||
#include "probe.h"
|
||||
#include "protocol.h"
|
||||
#include "report.h"
|
||||
//#include "serial.h"
|
||||
#include "serial.h"
|
||||
#include "spindle_control.h"
|
||||
#include "stepper.h"
|
||||
#include "jog.h"
|
||||
|
||||
160
grbl/report.c
160
grbl/report.c
@@ -31,10 +31,10 @@
|
||||
|
||||
// Internal report utilities to reduce flash with repetitive tasks turned into functions.
|
||||
void report_util_setting_prefix(uint8_t n) { serial_write('$'); print_uint8_base10(n); serial_write('='); }
|
||||
static void report_util_line_feed() { printPgmString(PSTR("\r\n")); }
|
||||
static void report_util_line_feed() { printPgmString(("\r\n")); }
|
||||
static void report_util_feedback_line_feed() { serial_write(']'); report_util_line_feed(); }
|
||||
static void report_util_gcode_modes_G() { printPgmString(PSTR(" G")); }
|
||||
static void report_util_gcode_modes_M() { printPgmString(PSTR(" M")); }
|
||||
static void report_util_gcode_modes_G() { printPgmString((" G")); }
|
||||
static void report_util_gcode_modes_M() { printPgmString((" M")); }
|
||||
// static void report_util_comment_line_feed() { serial_write(')'); report_util_line_feed(); }
|
||||
static void report_util_axis_values(float *axis_value) {
|
||||
uint8_t idx;
|
||||
@@ -49,28 +49,28 @@ static void report_util_setting_string(uint8_t n) {
|
||||
serial_write(' ');
|
||||
serial_write('(');
|
||||
switch(n) {
|
||||
case 0: printPgmString(PSTR("stp pulse")); break;
|
||||
case 1: printPgmString(PSTR("idl delay")); break;
|
||||
case 2: printPgmString(PSTR("stp inv")); break;
|
||||
case 3: printPgmString(PSTR("dir inv")); break;
|
||||
case 4: printPgmString(PSTR("stp en inv")); break;
|
||||
case 5: printPgmString(PSTR("lim inv")); break;
|
||||
case 6: printPgmString(PSTR("prb inv")); break;
|
||||
case 10: printPgmString(PSTR("rpt")); break;
|
||||
case 11: printPgmString(PSTR("jnc dev")); break;
|
||||
case 12: printPgmString(PSTR("arc tol")); break;
|
||||
case 13: printPgmString(PSTR("rpt inch")); break;
|
||||
case 20: printPgmString(PSTR("sft lim")); break;
|
||||
case 21: printPgmString(PSTR("hrd lim")); break;
|
||||
case 22: printPgmString(PSTR("hm cyc")); break;
|
||||
case 23: printPgmString(PSTR("hm dir inv")); break;
|
||||
case 24: printPgmString(PSTR("hm feed")); break;
|
||||
case 25: printPgmString(PSTR("hm seek")); break;
|
||||
case 26: printPgmString(PSTR("hm delay")); break;
|
||||
case 27: printPgmString(PSTR("hm pulloff")); break;
|
||||
case 30: printPgmString(PSTR("rpm max")); break;
|
||||
case 31: printPgmString(PSTR("rpm min")); break;
|
||||
case 32: printPgmString(PSTR("laser")); break;
|
||||
case 0: printPgmString(("stp pulse")); break;
|
||||
case 1: printPgmString(("idl delay")); break;
|
||||
case 2: printPgmString(("stp inv")); break;
|
||||
case 3: printPgmString(("dir inv")); break;
|
||||
case 4: printPgmString(("stp en inv")); break;
|
||||
case 5: printPgmString(("lim inv")); break;
|
||||
case 6: printPgmString(("prb inv")); break;
|
||||
case 10: printPgmString(("rpt")); break;
|
||||
case 11: printPgmString(("jnc dev")); break;
|
||||
case 12: printPgmString(("arc tol")); break;
|
||||
case 13: printPgmString(("rpt inch")); break;
|
||||
case 20: printPgmString(("sft lim")); break;
|
||||
case 21: printPgmString(("hrd lim")); break;
|
||||
case 22: printPgmString(("hm cyc")); break;
|
||||
case 23: printPgmString(("hm dir inv")); break;
|
||||
case 24: printPgmString(("hm feed")); break;
|
||||
case 25: printPgmString(("hm seek")); break;
|
||||
case 26: printPgmString(("hm delay")); break;
|
||||
case 27: printPgmString(("hm pulloff")); break;
|
||||
case 30: printPgmString(("rpm max")); break;
|
||||
case 31: printPgmString(("rpm min")); break;
|
||||
case 32: printPgmString(("laser")); break;
|
||||
default:
|
||||
n -= AXIS_SETTINGS_START_VAL;
|
||||
uint8_t idx = 0;
|
||||
@@ -80,10 +80,10 @@ static void report_util_setting_string(uint8_t n) {
|
||||
}
|
||||
serial_write(n+'x');
|
||||
switch (idx) {
|
||||
case 0: printPgmString(PSTR(":stp/mm")); break;
|
||||
case 1: printPgmString(PSTR(":mm/min")); break;
|
||||
case 2: printPgmString(PSTR(":mm/s^2")); break;
|
||||
case 3: printPgmString(PSTR(":mm max")); break;
|
||||
case 0: printPgmString((":stp/mm")); break;
|
||||
case 1: printPgmString((":mm/min")); break;
|
||||
case 2: printPgmString((":mm/s^2")); break;
|
||||
case 3: printPgmString((":mm max")); break;
|
||||
}
|
||||
break;
|
||||
}
|
||||
@@ -113,9 +113,9 @@ void report_status_message(uint8_t status_code)
|
||||
{
|
||||
switch(status_code) {
|
||||
case STATUS_OK: // STATUS_OK
|
||||
printPgmString(PSTR("ok\r\n")); break;
|
||||
printPgmString(("ok\r\n")); break;
|
||||
default:
|
||||
printPgmString(PSTR("error:"));
|
||||
printPgmString(("error:"));
|
||||
print_uint8_base10(status_code);
|
||||
report_util_line_feed();
|
||||
}
|
||||
@@ -124,7 +124,7 @@ void report_status_message(uint8_t status_code)
|
||||
// Prints alarm messages.
|
||||
void report_alarm_message(uint8_t alarm_code)
|
||||
{
|
||||
printPgmString(PSTR("ALARM:"));
|
||||
printPgmString(("ALARM:"));
|
||||
print_uint8_base10(alarm_code);
|
||||
report_util_line_feed();
|
||||
delay_ms(500); // Force delay to ensure message clears serial write buffer.
|
||||
@@ -137,30 +137,30 @@ void report_alarm_message(uint8_t alarm_code)
|
||||
// is installed, the message number codes are less than zero.
|
||||
void report_feedback_message(uint8_t message_code)
|
||||
{
|
||||
printPgmString(PSTR("[MSG:"));
|
||||
printPgmString(("[MSG:"));
|
||||
switch(message_code) {
|
||||
case MESSAGE_CRITICAL_EVENT:
|
||||
printPgmString(PSTR("Reset to continue")); break;
|
||||
printPgmString(("Reset to continue")); break;
|
||||
case MESSAGE_ALARM_LOCK:
|
||||
printPgmString(PSTR("'$H'|'$X' to unlock")); break;
|
||||
printPgmString(("'$H'|'$X' to unlock")); break;
|
||||
case MESSAGE_ALARM_UNLOCK:
|
||||
printPgmString(PSTR("Caution: Unlocked")); break;
|
||||
printPgmString(("Caution: Unlocked")); break;
|
||||
case MESSAGE_ENABLED:
|
||||
printPgmString(PSTR("Enabled")); break;
|
||||
printPgmString(("Enabled")); break;
|
||||
case MESSAGE_DISABLED:
|
||||
printPgmString(PSTR("Disabled")); break;
|
||||
printPgmString(("Disabled")); break;
|
||||
case MESSAGE_SAFETY_DOOR_AJAR:
|
||||
printPgmString(PSTR("Check Door")); break;
|
||||
printPgmString(("Check Door")); break;
|
||||
case MESSAGE_CHECK_LIMITS:
|
||||
printPgmString(PSTR("Check Limits")); break;
|
||||
printPgmString(("Check Limits")); break;
|
||||
case MESSAGE_PROGRAM_END:
|
||||
printPgmString(PSTR("Pgm End")); break;
|
||||
printPgmString(("Pgm End")); break;
|
||||
case MESSAGE_RESTORE_DEFAULTS:
|
||||
printPgmString(PSTR("Restoring defaults")); break;
|
||||
printPgmString(("Restoring defaults")); break;
|
||||
case MESSAGE_SPINDLE_RESTORE:
|
||||
printPgmString(PSTR("Restoring spindle")); break;
|
||||
printPgmString(("Restoring spindle")); break;
|
||||
case MESSAGE_SLEEP_MODE:
|
||||
printPgmString(PSTR("Sleeping")); break;
|
||||
printPgmString(("Sleeping")); break;
|
||||
}
|
||||
report_util_feedback_line_feed();
|
||||
}
|
||||
@@ -169,12 +169,12 @@ void report_feedback_message(uint8_t message_code)
|
||||
// Welcome message
|
||||
void report_init_message()
|
||||
{
|
||||
printPgmString(PSTR("\r\nGrbl " GRBL_VERSION " ['$' for help]\r\n"));
|
||||
printPgmString(("\r\nGrbl " GRBL_VERSION " ['$' for help]\r\n"));
|
||||
}
|
||||
|
||||
// Grbl help message
|
||||
void report_grbl_help() {
|
||||
printPgmString(PSTR("[HLP:$$ $# $G $I $N $x=val $Nx=line $J=line $SLP $C $X $H ~ ! ? ctrl-x]\r\n"));
|
||||
printPgmString(("[HLP:$$ $# $G $I $N $x=val $Nx=line $J=line $SLP $C $X $H ~ ! ? ctrl-x]\r\n"));
|
||||
}
|
||||
|
||||
|
||||
@@ -231,7 +231,7 @@ void report_grbl_settings() {
|
||||
void report_probe_parameters()
|
||||
{
|
||||
// Report in terms of machine position.
|
||||
printPgmString(PSTR("[PRB:"));
|
||||
printPgmString(("[PRB:"));
|
||||
float print_position[N_AXIS];
|
||||
system_convert_array_steps_to_mpos(print_position,sys_probe_position);
|
||||
report_util_axis_values(print_position);
|
||||
@@ -251,20 +251,20 @@ void report_ngc_parameters()
|
||||
report_status_message(STATUS_SETTING_READ_FAIL);
|
||||
return;
|
||||
}
|
||||
printPgmString(PSTR("[G"));
|
||||
printPgmString(("[G"));
|
||||
switch (coord_select) {
|
||||
case 6: printPgmString(PSTR("28")); break;
|
||||
case 7: printPgmString(PSTR("30")); break;
|
||||
case 6: printPgmString(("28")); break;
|
||||
case 7: printPgmString(("30")); break;
|
||||
default: print_uint8_base10(coord_select+54); break; // G54-G59
|
||||
}
|
||||
serial_write(':');
|
||||
report_util_axis_values(coord_data);
|
||||
report_util_feedback_line_feed();
|
||||
}
|
||||
printPgmString(PSTR("[G92:")); // Print G92,G92.1 which are not persistent in memory
|
||||
printPgmString(("[G92:")); // Print G92,G92.1 which are not persistent in memory
|
||||
report_util_axis_values(gc_state.coord_offset);
|
||||
report_util_feedback_line_feed();
|
||||
printPgmString(PSTR("[TLO:")); // Print tool length offset value
|
||||
printPgmString(("[TLO:")); // Print tool length offset value
|
||||
printFloat_CoordValue(gc_state.tool_length_offset);
|
||||
report_util_feedback_line_feed();
|
||||
report_probe_parameters(); // Print probe parameters. Not persistent in memory.
|
||||
@@ -274,9 +274,9 @@ void report_ngc_parameters()
|
||||
// Print current gcode parser mode state
|
||||
void report_gcode_modes()
|
||||
{
|
||||
printPgmString(PSTR("[GC:G"));
|
||||
printPgmString(("[GC:G"));
|
||||
if (gc_state.modal.motion >= MOTION_MODE_PROBE_TOWARD) {
|
||||
printPgmString(PSTR("38."));
|
||||
printPgmString(("38."));
|
||||
print_uint8_base10(gc_state.modal.motion - (MOTION_MODE_PROBE_TOWARD-2));
|
||||
} else {
|
||||
print_uint8_base10(gc_state.modal.motion);
|
||||
@@ -334,14 +334,14 @@ void report_gcode_modes()
|
||||
}
|
||||
#endif
|
||||
|
||||
printPgmString(PSTR(" T"));
|
||||
printPgmString((" T"));
|
||||
print_uint8_base10(gc_state.tool);
|
||||
|
||||
printPgmString(PSTR(" F"));
|
||||
printPgmString((" F"));
|
||||
printFloat_RateValue(gc_state.feed_rate);
|
||||
|
||||
#ifdef VARIABLE_SPINDLE
|
||||
printPgmString(PSTR(" S"));
|
||||
printPgmString((" S"));
|
||||
printFloat(gc_state.spindle_speed,N_DECIMAL_RPMVALUE);
|
||||
#endif
|
||||
|
||||
@@ -351,7 +351,7 @@ void report_gcode_modes()
|
||||
// Prints specified startup line
|
||||
void report_startup_line(uint8_t n, char *line)
|
||||
{
|
||||
printPgmString(PSTR("$N"));
|
||||
printPgmString(("$N"));
|
||||
print_uint8_base10(n);
|
||||
serial_write('=');
|
||||
printString(line);
|
||||
@@ -369,10 +369,10 @@ void report_execute_startup_message(char *line, uint8_t status_code)
|
||||
// Prints build info line
|
||||
void report_build_info(char *line)
|
||||
{
|
||||
printPgmString(PSTR("[VER:" GRBL_VERSION "." GRBL_VERSION_BUILD ":"));
|
||||
printPgmString(("[VER:" GRBL_VERSION "." GRBL_VERSION_BUILD ":"));
|
||||
printString(line);
|
||||
report_util_feedback_line_feed();
|
||||
printPgmString(PSTR("[OPT:")); // Generate compile-time build option list
|
||||
printPgmString(("[OPT:")); // Generate compile-time build option list
|
||||
#ifdef VARIABLE_SPINDLE
|
||||
serial_write('V');
|
||||
#endif
|
||||
@@ -453,7 +453,7 @@ void report_build_info(char *line)
|
||||
// and has been sent into protocol_execute_line() routine to be executed by Grbl.
|
||||
void report_echo_line_received(char *line)
|
||||
{
|
||||
printPgmString(PSTR("[echo: ")); printString(line);
|
||||
printPgmString(("[echo: ")); printString(line);
|
||||
report_util_feedback_line_feed();
|
||||
}
|
||||
|
||||
@@ -474,21 +474,21 @@ void report_realtime_status()
|
||||
// Report current machine state and sub-states
|
||||
serial_write('<');
|
||||
switch (sys.state) {
|
||||
case STATE_IDLE: printPgmString(PSTR("Idle")); break;
|
||||
case STATE_CYCLE: printPgmString(PSTR("Run")); break;
|
||||
case STATE_IDLE: printPgmString(("Idle")); break;
|
||||
case STATE_CYCLE: printPgmString(("Run")); break;
|
||||
case STATE_HOLD:
|
||||
if (!(sys.suspend & SUSPEND_JOG_CANCEL)) {
|
||||
printPgmString(PSTR("Hold:"));
|
||||
printPgmString(("Hold:"));
|
||||
if (sys.suspend & SUSPEND_HOLD_COMPLETE) { serial_write('0'); } // Ready to resume
|
||||
else { serial_write('1'); } // Actively holding
|
||||
break;
|
||||
} // Continues to print jog state during jog cancel.
|
||||
case STATE_JOG: printPgmString(PSTR("Jog")); break;
|
||||
case STATE_HOMING: printPgmString(PSTR("Home")); break;
|
||||
case STATE_ALARM: printPgmString(PSTR("Alarm")); break;
|
||||
case STATE_CHECK_MODE: printPgmString(PSTR("Check")); break;
|
||||
case STATE_JOG: printPgmString(("Jog")); break;
|
||||
case STATE_HOMING: printPgmString(("Home")); break;
|
||||
case STATE_ALARM: printPgmString(("Alarm")); break;
|
||||
case STATE_CHECK_MODE: printPgmString(("Check")); break;
|
||||
case STATE_SAFETY_DOOR:
|
||||
printPgmString(PSTR("Door:"));
|
||||
printPgmString(("Door:"));
|
||||
if (sys.suspend & SUSPEND_INITIATE_RESTORE) {
|
||||
serial_write('3'); // Restoring
|
||||
} else {
|
||||
@@ -503,7 +503,7 @@ void report_realtime_status()
|
||||
}
|
||||
}
|
||||
break;
|
||||
case STATE_SLEEP: printPgmString(PSTR("Sleep")); break;
|
||||
case STATE_SLEEP: printPgmString(("Sleep")); break;
|
||||
}
|
||||
|
||||
float wco[N_AXIS];
|
||||
@@ -521,16 +521,16 @@ void report_realtime_status()
|
||||
|
||||
// Report machine position
|
||||
if (bit_istrue(settings.status_report_mask,BITFLAG_RT_STATUS_POSITION_TYPE)) {
|
||||
printPgmString(PSTR("|MPos:"));
|
||||
printPgmString(("|MPos:"));
|
||||
} else {
|
||||
printPgmString(PSTR("|WPos:"));
|
||||
printPgmString(("|WPos:"));
|
||||
}
|
||||
report_util_axis_values(print_position);
|
||||
|
||||
// Returns planner and serial read buffer states.
|
||||
#ifdef REPORT_FIELD_BUFFER_STATE
|
||||
if (bit_istrue(settings.status_report_mask,BITFLAG_RT_STATUS_BUFFER_STATE)) {
|
||||
printPgmString(PSTR("|Bf:"));
|
||||
printPgmString(("|Bf:"));
|
||||
print_uint8_base10(plan_get_block_buffer_available());
|
||||
serial_write(',');
|
||||
print_uint8_base10(serial_get_rx_buffer_available());
|
||||
@@ -544,7 +544,7 @@ void report_realtime_status()
|
||||
if (cur_block != NULL) {
|
||||
uint32_t ln = cur_block->line_number;
|
||||
if (ln > 0) {
|
||||
printPgmString(PSTR("|Ln:"));
|
||||
printPgmString(("|Ln:"));
|
||||
printInteger(ln);
|
||||
}
|
||||
}
|
||||
@@ -554,12 +554,12 @@ void report_realtime_status()
|
||||
// Report realtime feed speed
|
||||
#ifdef REPORT_FIELD_CURRENT_FEED_SPEED
|
||||
#ifdef VARIABLE_SPINDLE
|
||||
printPgmString(PSTR("|FS:"));
|
||||
printPgmString(("|FS:"));
|
||||
printFloat_RateValue(st_get_realtime_rate());
|
||||
serial_write(',');
|
||||
printFloat(sys.spindle_speed,N_DECIMAL_RPMVALUE);
|
||||
#else
|
||||
printPgmString(PSTR("|F:"));
|
||||
printPgmString(("|F:"));
|
||||
printFloat_RateValue(st_get_realtime_rate());
|
||||
#endif
|
||||
#endif
|
||||
@@ -569,7 +569,7 @@ void report_realtime_status()
|
||||
uint8_t ctrl_pin_state = system_control_get_state();
|
||||
uint8_t prb_pin_state = probe_get_state();
|
||||
if (lim_pin_state | ctrl_pin_state | prb_pin_state) {
|
||||
printPgmString(PSTR("|Pn:"));
|
||||
printPgmString(("|Pn:"));
|
||||
if (prb_pin_state) { serial_write('P'); }
|
||||
if (lim_pin_state) {
|
||||
#ifdef ENABLE_DUAL_AXIS
|
||||
@@ -606,7 +606,7 @@ void report_realtime_status()
|
||||
sys.report_wco_counter = (REPORT_WCO_REFRESH_BUSY_COUNT-1); // Reset counter for slow refresh
|
||||
} else { sys.report_wco_counter = (REPORT_WCO_REFRESH_IDLE_COUNT-1); }
|
||||
if (sys.report_ovr_counter == 0) { sys.report_ovr_counter = 1; } // Set override on next report.
|
||||
printPgmString(PSTR("|WCO:"));
|
||||
printPgmString(("|WCO:"));
|
||||
report_util_axis_values(wco);
|
||||
}
|
||||
#endif
|
||||
@@ -617,7 +617,7 @@ void report_realtime_status()
|
||||
if (sys.state & (STATE_HOMING | STATE_CYCLE | STATE_HOLD | STATE_JOG | STATE_SAFETY_DOOR)) {
|
||||
sys.report_ovr_counter = (REPORT_OVR_REFRESH_BUSY_COUNT-1); // Reset counter for slow refresh
|
||||
} else { sys.report_ovr_counter = (REPORT_OVR_REFRESH_IDLE_COUNT-1); }
|
||||
printPgmString(PSTR("|Ov:"));
|
||||
printPgmString(("|Ov:"));
|
||||
print_uint8_base10(sys.f_override);
|
||||
serial_write(',');
|
||||
print_uint8_base10(sys.r_override);
|
||||
@@ -627,7 +627,7 @@ void report_realtime_status()
|
||||
uint8_t sp_state = spindle_get_state();
|
||||
uint8_t cl_state = coolant_get_state();
|
||||
if (sp_state || cl_state) {
|
||||
printPgmString(PSTR("|A:"));
|
||||
printPgmString(("|A:"));
|
||||
if (sp_state) { // != SPINDLE_STATE_DISABLE
|
||||
#ifdef VARIABLE_SPINDLE
|
||||
#ifdef USE_SPINDLE_DIR_AS_ENABLE_PIN
|
||||
|
||||
180
grbl/serial.c
180
grbl/serial.c
@@ -36,6 +36,7 @@ volatile uint8_t serial_tx_buffer_tail = 0;
|
||||
// Returns the number of bytes available in the RX serial buffer.
|
||||
uint8_t serial_get_rx_buffer_available()
|
||||
{
|
||||
printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);
|
||||
uint8_t rtail = serial_rx_buffer_tail; // Copy to limit multiple calls to volatile
|
||||
if (serial_rx_buffer_head >= rtail) { return(RX_BUFFER_SIZE - (serial_rx_buffer_head-rtail)); }
|
||||
return((rtail-serial_rx_buffer_head-1));
|
||||
@@ -46,6 +47,7 @@ uint8_t serial_get_rx_buffer_available()
|
||||
// NOTE: Deprecated. Not used unless classic status reports are enabled in config.h.
|
||||
uint8_t serial_get_rx_buffer_count()
|
||||
{
|
||||
printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);
|
||||
uint8_t rtail = serial_rx_buffer_tail; // Copy to limit multiple calls to volatile
|
||||
if (serial_rx_buffer_head >= rtail) { return(serial_rx_buffer_head-rtail); }
|
||||
return (RX_BUFFER_SIZE - (rtail-serial_rx_buffer_head));
|
||||
@@ -56,6 +58,7 @@ uint8_t serial_get_rx_buffer_count()
|
||||
// NOTE: Not used except for debugging and ensuring no TX bottlenecks.
|
||||
uint8_t serial_get_tx_buffer_count()
|
||||
{
|
||||
printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);
|
||||
uint8_t ttail = serial_tx_buffer_tail; // Copy to limit multiple calls to volatile
|
||||
if (serial_tx_buffer_head >= ttail) { return(serial_tx_buffer_head-ttail); }
|
||||
return (TX_RING_BUFFER - (ttail-serial_tx_buffer_head));
|
||||
@@ -64,19 +67,21 @@ uint8_t serial_get_tx_buffer_count()
|
||||
|
||||
void serial_init()
|
||||
{
|
||||
// Set baud rate
|
||||
#if BAUD_RATE < 57600
|
||||
uint16_t UBRR0_value = ((F_CPU / (8L * BAUD_RATE)) - 1)/2 ;
|
||||
UCSR0A &= ~(1 << U2X0); // baud doubler off - Only needed on Uno XXX
|
||||
#else
|
||||
uint16_t UBRR0_value = ((F_CPU / (4L * BAUD_RATE)) - 1)/2;
|
||||
UCSR0A |= (1 << U2X0); // baud doubler on for high baud rates, i.e. 115200
|
||||
#endif
|
||||
UBRR0H = UBRR0_value >> 8;
|
||||
UBRR0L = UBRR0_value;
|
||||
printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);
|
||||
|
||||
// enable rx, tx, and interrupt on complete reception of a byte
|
||||
UCSR0B |= (1<<RXEN0 | 1<<TXEN0 | 1<<RXCIE0);
|
||||
// Set baud rate
|
||||
//#if BAUD_RATE < 57600
|
||||
// uint16_t UBRR0_value = ((F_CPU / (8L * BAUD_RATE)) - 1)/2 ;
|
||||
// UCSR0A &= ~(1 << U2X0); // baud doubler off - Only needed on Uno XXX
|
||||
//#else
|
||||
// uint16_t UBRR0_value = ((F_CPU / (4L * BAUD_RATE)) - 1)/2;
|
||||
// UCSR0A |= (1 << U2X0); // baud doubler on for high baud rates, i.e. 115200
|
||||
//#endif
|
||||
//UBRR0H = UBRR0_value >> 8;
|
||||
//UBRR0L = UBRR0_value;
|
||||
//
|
||||
//// enable rx, tx, and interrupt on complete reception of a byte
|
||||
//UCSR0B |= (1<<RXEN0 | 1<<TXEN0 | 1<<RXCIE0);
|
||||
|
||||
// defaults to 8-bit, no parity, 1 stop bit
|
||||
}
|
||||
@@ -84,6 +89,8 @@ void serial_init()
|
||||
|
||||
// Writes one byte to the TX serial buffer. Called by main program.
|
||||
void serial_write(uint8_t data) {
|
||||
printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);
|
||||
|
||||
// Calculate next head
|
||||
uint8_t next_head = serial_tx_buffer_head + 1;
|
||||
if (next_head == TX_RING_BUFFER) { next_head = 0; }
|
||||
@@ -99,32 +106,33 @@ void serial_write(uint8_t data) {
|
||||
serial_tx_buffer_head = next_head;
|
||||
|
||||
// Enable Data Register Empty Interrupt to make sure tx-streaming is running
|
||||
UCSR0B |= (1 << UDRIE0);
|
||||
//UCSR0B |= (1 << UDRIE0);
|
||||
}
|
||||
|
||||
|
||||
// Data Register Empty Interrupt handler
|
||||
ISR(SERIAL_UDRE)
|
||||
{
|
||||
uint8_t tail = serial_tx_buffer_tail; // Temporary serial_tx_buffer_tail (to optimize for volatile)
|
||||
|
||||
// Send a byte from the buffer
|
||||
UDR0 = serial_tx_buffer[tail];
|
||||
|
||||
// Update tail position
|
||||
tail++;
|
||||
if (tail == TX_RING_BUFFER) { tail = 0; }
|
||||
|
||||
serial_tx_buffer_tail = tail;
|
||||
|
||||
// Turn off Data Register Empty Interrupt to stop tx-streaming if this concludes the transfer
|
||||
if (tail == serial_tx_buffer_head) { UCSR0B &= ~(1 << UDRIE0); }
|
||||
}
|
||||
//ISR(SERIAL_UDRE)
|
||||
//{
|
||||
// uint8_t tail = serial_tx_buffer_tail; // Temporary serial_tx_buffer_tail (to optimize for volatile)
|
||||
//
|
||||
// // Send a byte from the buffer
|
||||
// UDR0 = serial_tx_buffer[tail];
|
||||
//
|
||||
// // Update tail position
|
||||
// tail++;
|
||||
// if (tail == TX_RING_BUFFER) { tail = 0; }
|
||||
//
|
||||
// serial_tx_buffer_tail = tail;
|
||||
//
|
||||
// // Turn off Data Register Empty Interrupt to stop tx-streaming if this concludes the transfer
|
||||
// if (tail == serial_tx_buffer_head) { UCSR0B &= ~(1 << UDRIE0); }
|
||||
//}
|
||||
|
||||
|
||||
// Fetches the first byte in the serial read buffer. Called by main program.
|
||||
uint8_t serial_read()
|
||||
{
|
||||
printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);
|
||||
uint8_t tail = serial_rx_buffer_tail; // Temporary serial_rx_buffer_tail (to optimize for volatile)
|
||||
if (serial_rx_buffer_head == tail) {
|
||||
return SERIAL_NO_DATA;
|
||||
@@ -140,63 +148,63 @@ uint8_t serial_read()
|
||||
}
|
||||
|
||||
|
||||
ISR(SERIAL_RX)
|
||||
{
|
||||
uint8_t data = UDR0;
|
||||
uint8_t next_head;
|
||||
|
||||
// Pick off realtime command characters directly from the serial stream. These characters are
|
||||
// not passed into the main buffer, but these set system state flag bits for realtime execution.
|
||||
switch (data) {
|
||||
case CMD_RESET: mc_reset(); break; // Call motion control reset routine.
|
||||
case CMD_STATUS_REPORT: system_set_exec_state_flag(EXEC_STATUS_REPORT); break; // Set as true
|
||||
case CMD_CYCLE_START: system_set_exec_state_flag(EXEC_CYCLE_START); break; // Set as true
|
||||
case CMD_FEED_HOLD: system_set_exec_state_flag(EXEC_FEED_HOLD); break; // Set as true
|
||||
default :
|
||||
if (data > 0x7F) { // Real-time control characters are extended ACSII only.
|
||||
switch(data) {
|
||||
case CMD_SAFETY_DOOR: system_set_exec_state_flag(EXEC_SAFETY_DOOR); break; // Set as true
|
||||
case CMD_JOG_CANCEL:
|
||||
if (sys.state & STATE_JOG) { // Block all other states from invoking motion cancel.
|
||||
system_set_exec_state_flag(EXEC_MOTION_CANCEL);
|
||||
}
|
||||
break;
|
||||
#ifdef DEBUG
|
||||
case CMD_DEBUG_REPORT: {uint8_t sreg = SREG; cli(); bit_true(sys_rt_exec_debug,EXEC_DEBUG_REPORT); SREG = sreg;} break;
|
||||
#endif
|
||||
case CMD_FEED_OVR_RESET: system_set_exec_motion_override_flag(EXEC_FEED_OVR_RESET); break;
|
||||
case CMD_FEED_OVR_COARSE_PLUS: system_set_exec_motion_override_flag(EXEC_FEED_OVR_COARSE_PLUS); break;
|
||||
case CMD_FEED_OVR_COARSE_MINUS: system_set_exec_motion_override_flag(EXEC_FEED_OVR_COARSE_MINUS); break;
|
||||
case CMD_FEED_OVR_FINE_PLUS: system_set_exec_motion_override_flag(EXEC_FEED_OVR_FINE_PLUS); break;
|
||||
case CMD_FEED_OVR_FINE_MINUS: system_set_exec_motion_override_flag(EXEC_FEED_OVR_FINE_MINUS); break;
|
||||
case CMD_RAPID_OVR_RESET: system_set_exec_motion_override_flag(EXEC_RAPID_OVR_RESET); break;
|
||||
case CMD_RAPID_OVR_MEDIUM: system_set_exec_motion_override_flag(EXEC_RAPID_OVR_MEDIUM); break;
|
||||
case CMD_RAPID_OVR_LOW: system_set_exec_motion_override_flag(EXEC_RAPID_OVR_LOW); break;
|
||||
case CMD_SPINDLE_OVR_RESET: system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_RESET); break;
|
||||
case CMD_SPINDLE_OVR_COARSE_PLUS: system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_COARSE_PLUS); break;
|
||||
case CMD_SPINDLE_OVR_COARSE_MINUS: system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_COARSE_MINUS); break;
|
||||
case CMD_SPINDLE_OVR_FINE_PLUS: system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_FINE_PLUS); break;
|
||||
case CMD_SPINDLE_OVR_FINE_MINUS: system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_FINE_MINUS); break;
|
||||
case CMD_SPINDLE_OVR_STOP: system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_STOP); break;
|
||||
case CMD_COOLANT_FLOOD_OVR_TOGGLE: system_set_exec_accessory_override_flag(EXEC_COOLANT_FLOOD_OVR_TOGGLE); break;
|
||||
#ifdef ENABLE_M7
|
||||
case CMD_COOLANT_MIST_OVR_TOGGLE: system_set_exec_accessory_override_flag(EXEC_COOLANT_MIST_OVR_TOGGLE); break;
|
||||
#endif
|
||||
}
|
||||
// Throw away any unfound extended-ASCII character by not passing it to the serial buffer.
|
||||
} else { // Write character to buffer
|
||||
next_head = serial_rx_buffer_head + 1;
|
||||
if (next_head == RX_RING_BUFFER) { next_head = 0; }
|
||||
|
||||
// Write data to buffer unless it is full.
|
||||
if (next_head != serial_rx_buffer_tail) {
|
||||
serial_rx_buffer[serial_rx_buffer_head] = data;
|
||||
serial_rx_buffer_head = next_head;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//ISR(SERIAL_RX)
|
||||
//{
|
||||
// uint8_t data = UDR0;
|
||||
// uint8_t next_head;
|
||||
//
|
||||
// // Pick off realtime command characters directly from the serial stream. These characters are
|
||||
// // not passed into the main buffer, but these set system state flag bits for realtime execution.
|
||||
// switch (data) {
|
||||
// case CMD_RESET: mc_reset(); break; // Call motion control reset routine.
|
||||
// case CMD_STATUS_REPORT: system_set_exec_state_flag(EXEC_STATUS_REPORT); break; // Set as true
|
||||
// case CMD_CYCLE_START: system_set_exec_state_flag(EXEC_CYCLE_START); break; // Set as true
|
||||
// case CMD_FEED_HOLD: system_set_exec_state_flag(EXEC_FEED_HOLD); break; // Set as true
|
||||
// default :
|
||||
// if (data > 0x7F) { // Real-time control characters are extended ACSII only.
|
||||
// switch(data) {
|
||||
// case CMD_SAFETY_DOOR: system_set_exec_state_flag(EXEC_SAFETY_DOOR); break; // Set as true
|
||||
// case CMD_JOG_CANCEL:
|
||||
// if (sys.state & STATE_JOG) { // Block all other states from invoking motion cancel.
|
||||
// system_set_exec_state_flag(EXEC_MOTION_CANCEL);
|
||||
// }
|
||||
// break;
|
||||
// #ifdef DEBUG
|
||||
// case CMD_DEBUG_REPORT: {uint8_t sreg = SREG; cli(); bit_true(sys_rt_exec_debug,EXEC_DEBUG_REPORT); SREG = sreg;} break;
|
||||
// #endif
|
||||
// case CMD_FEED_OVR_RESET: system_set_exec_motion_override_flag(EXEC_FEED_OVR_RESET); break;
|
||||
// case CMD_FEED_OVR_COARSE_PLUS: system_set_exec_motion_override_flag(EXEC_FEED_OVR_COARSE_PLUS); break;
|
||||
// case CMD_FEED_OVR_COARSE_MINUS: system_set_exec_motion_override_flag(EXEC_FEED_OVR_COARSE_MINUS); break;
|
||||
// case CMD_FEED_OVR_FINE_PLUS: system_set_exec_motion_override_flag(EXEC_FEED_OVR_FINE_PLUS); break;
|
||||
// case CMD_FEED_OVR_FINE_MINUS: system_set_exec_motion_override_flag(EXEC_FEED_OVR_FINE_MINUS); break;
|
||||
// case CMD_RAPID_OVR_RESET: system_set_exec_motion_override_flag(EXEC_RAPID_OVR_RESET); break;
|
||||
// case CMD_RAPID_OVR_MEDIUM: system_set_exec_motion_override_flag(EXEC_RAPID_OVR_MEDIUM); break;
|
||||
// case CMD_RAPID_OVR_LOW: system_set_exec_motion_override_flag(EXEC_RAPID_OVR_LOW); break;
|
||||
// case CMD_SPINDLE_OVR_RESET: system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_RESET); break;
|
||||
// case CMD_SPINDLE_OVR_COARSE_PLUS: system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_COARSE_PLUS); break;
|
||||
// case CMD_SPINDLE_OVR_COARSE_MINUS: system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_COARSE_MINUS); break;
|
||||
// case CMD_SPINDLE_OVR_FINE_PLUS: system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_FINE_PLUS); break;
|
||||
// case CMD_SPINDLE_OVR_FINE_MINUS: system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_FINE_MINUS); break;
|
||||
// case CMD_SPINDLE_OVR_STOP: system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_STOP); break;
|
||||
// case CMD_COOLANT_FLOOD_OVR_TOGGLE: system_set_exec_accessory_override_flag(EXEC_COOLANT_FLOOD_OVR_TOGGLE); break;
|
||||
// #ifdef ENABLE_M7
|
||||
// case CMD_COOLANT_MIST_OVR_TOGGLE: system_set_exec_accessory_override_flag(EXEC_COOLANT_MIST_OVR_TOGGLE); break;
|
||||
// #endif
|
||||
// }
|
||||
// // Throw away any unfound extended-ASCII character by not passing it to the serial buffer.
|
||||
// } else { // Write character to buffer
|
||||
// next_head = serial_rx_buffer_head + 1;
|
||||
// if (next_head == RX_RING_BUFFER) { next_head = 0; }
|
||||
//
|
||||
// // Write data to buffer unless it is full.
|
||||
// if (next_head != serial_rx_buffer_tail) {
|
||||
// serial_rx_buffer[serial_rx_buffer_head] = data;
|
||||
// serial_rx_buffer_head = next_head;
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
//}
|
||||
//
|
||||
|
||||
void serial_reset_read_buffer()
|
||||
{
|
||||
|
||||
167
grbl/system.c
167
grbl/system.c
@@ -23,14 +23,16 @@
|
||||
|
||||
void system_init()
|
||||
{
|
||||
CONTROL_DDR &= ~(CONTROL_MASK); // Configure as input pins
|
||||
#ifdef DISABLE_CONTROL_PIN_PULL_UP
|
||||
CONTROL_PORT &= ~(CONTROL_MASK); // Normal low operation. Requires external pull-down.
|
||||
#else
|
||||
CONTROL_PORT |= CONTROL_MASK; // Enable internal pull-up resistors. Normal high operation.
|
||||
#endif
|
||||
CONTROL_PCMSK |= CONTROL_MASK; // Enable specific pins of the Pin Change Interrupt
|
||||
PCICR |= (1 << CONTROL_INT); // Enable Pin Change Interrupt
|
||||
printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);
|
||||
|
||||
//CONTROL_DDR &= ~(CONTROL_MASK); // Configure as input pins
|
||||
//#ifdef DISABLE_CONTROL_PIN_PULL_UP
|
||||
// CONTROL_PORT &= ~(CONTROL_MASK); // Normal low operation. Requires external pull-down.
|
||||
//#else
|
||||
// CONTROL_PORT |= CONTROL_MASK; // Enable internal pull-up resistors. Normal high operation.
|
||||
//#endif
|
||||
//CONTROL_PCMSK |= CONTROL_MASK; // Enable specific pins of the Pin Change Interrupt
|
||||
//PCICR |= (1 << CONTROL_INT); // Enable Pin Change Interrupt
|
||||
}
|
||||
|
||||
|
||||
@@ -39,21 +41,23 @@ void system_init()
|
||||
// defined by the CONTROL_PIN_INDEX in the header file.
|
||||
uint8_t system_control_get_state()
|
||||
{
|
||||
uint8_t control_state = 0;
|
||||
uint8_t pin = (CONTROL_PIN & CONTROL_MASK) ^ CONTROL_MASK;
|
||||
#ifdef INVERT_CONTROL_PIN_MASK
|
||||
pin ^= INVERT_CONTROL_PIN_MASK;
|
||||
#endif
|
||||
if (pin) {
|
||||
#ifdef ENABLE_SAFETY_DOOR_INPUT_PIN
|
||||
if (bit_istrue(pin,(1<<CONTROL_SAFETY_DOOR_BIT))) { control_state |= CONTROL_PIN_INDEX_SAFETY_DOOR; }
|
||||
#else
|
||||
if (bit_istrue(pin,(1<<CONTROL_FEED_HOLD_BIT))) { control_state |= CONTROL_PIN_INDEX_FEED_HOLD; }
|
||||
#endif
|
||||
if (bit_istrue(pin,(1<<CONTROL_RESET_BIT))) { control_state |= CONTROL_PIN_INDEX_RESET; }
|
||||
if (bit_istrue(pin,(1<<CONTROL_CYCLE_START_BIT))) { control_state |= CONTROL_PIN_INDEX_CYCLE_START; }
|
||||
}
|
||||
return(control_state);
|
||||
printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);
|
||||
return 0;
|
||||
// uint8_t control_state = 0;
|
||||
// uint8_t pin = (CONTROL_PIN & CONTROL_MASK) ^ CONTROL_MASK;
|
||||
// #ifdef INVERT_CONTROL_PIN_MASK
|
||||
// pin ^= INVERT_CONTROL_PIN_MASK;
|
||||
// #endif
|
||||
// if (pin) {
|
||||
// #ifdef ENABLE_SAFETY_DOOR_INPUT_PIN
|
||||
// if (bit_istrue(pin,(1<<CONTROL_SAFETY_DOOR_BIT))) { control_state |= CONTROL_PIN_INDEX_SAFETY_DOOR; }
|
||||
// #else
|
||||
// if (bit_istrue(pin,(1<<CONTROL_FEED_HOLD_BIT))) { control_state |= CONTROL_PIN_INDEX_FEED_HOLD; }
|
||||
// #endif
|
||||
// if (bit_istrue(pin,(1<<CONTROL_RESET_BIT))) { control_state |= CONTROL_PIN_INDEX_RESET; }
|
||||
// if (bit_istrue(pin,(1<<CONTROL_CYCLE_START_BIT))) { control_state |= CONTROL_PIN_INDEX_CYCLE_START; }
|
||||
// }
|
||||
// return(control_state);
|
||||
}
|
||||
|
||||
|
||||
@@ -61,42 +65,46 @@ uint8_t system_control_get_state()
|
||||
// only the realtime command execute variable to have the main program execute these when
|
||||
// its ready. This works exactly like the character-based realtime commands when picked off
|
||||
// directly from the incoming serial data stream.
|
||||
ISR(CONTROL_INT_vect)
|
||||
{
|
||||
uint8_t pin = system_control_get_state();
|
||||
if (pin) {
|
||||
if (bit_istrue(pin,CONTROL_PIN_INDEX_RESET)) {
|
||||
mc_reset();
|
||||
}
|
||||
if (bit_istrue(pin,CONTROL_PIN_INDEX_CYCLE_START)) {
|
||||
bit_true(sys_rt_exec_state, EXEC_CYCLE_START);
|
||||
}
|
||||
#ifndef ENABLE_SAFETY_DOOR_INPUT_PIN
|
||||
if (bit_istrue(pin,CONTROL_PIN_INDEX_FEED_HOLD)) {
|
||||
bit_true(sys_rt_exec_state, EXEC_FEED_HOLD);
|
||||
#else
|
||||
if (bit_istrue(pin,CONTROL_PIN_INDEX_SAFETY_DOOR)) {
|
||||
bit_true(sys_rt_exec_state, EXEC_SAFETY_DOOR);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
}
|
||||
//ISR(CONTROL_INT_vect)
|
||||
//{
|
||||
// uint8_t pin = system_control_get_state();
|
||||
// if (pin) {
|
||||
// if (bit_istrue(pin,CONTROL_PIN_INDEX_RESET)) {
|
||||
// mc_reset();
|
||||
// }
|
||||
// if (bit_istrue(pin,CONTROL_PIN_INDEX_CYCLE_START)) {
|
||||
// bit_true(sys_rt_exec_state, EXEC_CYCLE_START);
|
||||
// }
|
||||
// #ifndef ENABLE_SAFETY_DOOR_INPUT_PIN
|
||||
// if (bit_istrue(pin,CONTROL_PIN_INDEX_FEED_HOLD)) {
|
||||
// bit_true(sys_rt_exec_state, EXEC_FEED_HOLD);
|
||||
// #else
|
||||
// if (bit_istrue(pin,CONTROL_PIN_INDEX_SAFETY_DOOR)) {
|
||||
// bit_true(sys_rt_exec_state, EXEC_SAFETY_DOOR);
|
||||
// #endif
|
||||
// }
|
||||
// }
|
||||
//}
|
||||
|
||||
|
||||
// Returns if safety door is ajar(T) or closed(F), based on pin state.
|
||||
uint8_t system_check_safety_door_ajar()
|
||||
{
|
||||
#ifdef ENABLE_SAFETY_DOOR_INPUT_PIN
|
||||
return(system_control_get_state() & CONTROL_PIN_INDEX_SAFETY_DOOR);
|
||||
#else
|
||||
return(false); // Input pin not enabled, so just return that it's closed.
|
||||
#endif
|
||||
printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);
|
||||
return 0;
|
||||
|
||||
//#ifdef ENABLE_SAFETY_DOOR_INPUT_PIN
|
||||
// return(system_control_get_state() & CONTROL_PIN_INDEX_SAFETY_DOOR);
|
||||
//#else
|
||||
// return(false); // Input pin not enabled, so just return that it's closed.
|
||||
//#endif
|
||||
}
|
||||
|
||||
|
||||
// Executes user startup script, if stored.
|
||||
void system_execute_startup(char *line)
|
||||
{
|
||||
printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);
|
||||
uint8_t n;
|
||||
for (n=0; n < N_STARTUP_LINE; n++) {
|
||||
if (!(settings_read_startup_line(n, line))) {
|
||||
@@ -121,9 +129,11 @@ void system_execute_startup(char *line)
|
||||
// since there are motions already stored in the buffer. However, this 'lag' should not
|
||||
// be an issue, since these commands are not typically used during a cycle.
|
||||
|
||||
|
||||
//Nice!!
|
||||
uint8_t system_execute_line(char *line)
|
||||
{
|
||||
printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);
|
||||
|
||||
uint8_t char_counter = 1;
|
||||
uint8_t helper_var = 0; // Helper variable
|
||||
float parameter, value;
|
||||
@@ -355,58 +365,69 @@ uint8_t system_check_travel_limits(float *target)
|
||||
|
||||
|
||||
// Special handlers for setting and clearing Grbl's real-time execution flags.
|
||||
// ecmc: set execute here (start rt thread maybe)
|
||||
void system_set_exec_state_flag(uint8_t mask) {
|
||||
uint8_t sreg = SREG;
|
||||
cli();
|
||||
printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);
|
||||
|
||||
//uint8_t sreg = SREG;
|
||||
//cli();
|
||||
sys_rt_exec_state |= (mask);
|
||||
SREG = sreg;
|
||||
//SREG = sreg;
|
||||
}
|
||||
|
||||
void system_clear_exec_state_flag(uint8_t mask) {
|
||||
uint8_t sreg = SREG;
|
||||
cli();
|
||||
printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);
|
||||
|
||||
//uint8_t sreg = SREG;
|
||||
//cli();
|
||||
sys_rt_exec_state &= ~(mask);
|
||||
SREG = sreg;
|
||||
//SREG = sreg;
|
||||
}
|
||||
|
||||
void system_set_exec_alarm(uint8_t code) {
|
||||
uint8_t sreg = SREG;
|
||||
cli();
|
||||
printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);
|
||||
//uint8_t sreg = SREG;
|
||||
//cli();
|
||||
sys_rt_exec_alarm = code;
|
||||
SREG = sreg;
|
||||
//SREG = sreg;
|
||||
}
|
||||
|
||||
void system_clear_exec_alarm() {
|
||||
uint8_t sreg = SREG;
|
||||
cli();
|
||||
printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);
|
||||
//uint8_t sreg = SREG;
|
||||
//cli();
|
||||
sys_rt_exec_alarm = 0;
|
||||
SREG = sreg;
|
||||
//SREG = sreg;
|
||||
}
|
||||
|
||||
void system_set_exec_motion_override_flag(uint8_t mask) {
|
||||
uint8_t sreg = SREG;
|
||||
cli();
|
||||
printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);
|
||||
//uint8_t sreg = SREG;
|
||||
//cli();
|
||||
sys_rt_exec_motion_override |= (mask);
|
||||
SREG = sreg;
|
||||
//SREG = sreg;
|
||||
}
|
||||
|
||||
void system_set_exec_accessory_override_flag(uint8_t mask) {
|
||||
uint8_t sreg = SREG;
|
||||
cli();
|
||||
printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);
|
||||
//uint8_t sreg = SREG;
|
||||
//cli();
|
||||
sys_rt_exec_accessory_override |= (mask);
|
||||
SREG = sreg;
|
||||
//SREG = sreg;
|
||||
}
|
||||
|
||||
void system_clear_exec_motion_overrides() {
|
||||
uint8_t sreg = SREG;
|
||||
cli();
|
||||
printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);
|
||||
//uint8_t sreg = SREG;
|
||||
//cli();
|
||||
sys_rt_exec_motion_override = 0;
|
||||
SREG = sreg;
|
||||
//SREG = sreg;
|
||||
}
|
||||
|
||||
void system_clear_exec_accessory_overrides() {
|
||||
uint8_t sreg = SREG;
|
||||
cli();
|
||||
printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);
|
||||
//uint8_t sreg = SREG;
|
||||
//cli();
|
||||
sys_rt_exec_accessory_override = 0;
|
||||
SREG = sreg;
|
||||
//SREG = sreg;
|
||||
}
|
||||
|
||||
0
iocsh/ecmc_grbl_eeprom.txt
Normal file
0
iocsh/ecmc_grbl_eeprom.txt
Normal file
37
iocsh/test.script
Normal file
37
iocsh/test.script
Normal file
@@ -0,0 +1,37 @@
|
||||
##############################################################################
|
||||
## Example: Demo of ecmc SocketCAN plugin
|
||||
## https://github.com/anderssandstrom/e3-ecmc_plugin_socketcan
|
||||
##
|
||||
## The plugin exposes:
|
||||
##
|
||||
|
||||
## Initiation:
|
||||
epicsEnvSet("IOC" ,"$(IOC="IOC_TEST")")
|
||||
epicsEnvSet("ECMCCFG_INIT" ,"") #Only run startup once (auto at PSI, need call at ESS), variable set to "#" in startup.cmd
|
||||
epicsEnvSet("SCRIPTEXEC" ,"$(SCRIPTEXEC="iocshLoad")")
|
||||
|
||||
require ecmccfg 7.0.1
|
||||
|
||||
# run module startup.cmd (only needed at ESS PSI auto call at require)
|
||||
$(ECMCCFG_INIT)$(SCRIPTEXEC) ${ecmccfg_DIR}startup.cmd, "IOC=$(IOC),ECMC_VER=7.0.1,MASTER_ID=-1"
|
||||
|
||||
##############################################################################
|
||||
## Configure hardware:
|
||||
# No EtherCAT hardware (in this example)..
|
||||
|
||||
##############################################################################
|
||||
## Load plugin:
|
||||
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
|
||||
${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)
|
||||
|
||||
##############################################################################
|
||||
## PLC 0
|
||||
# $(SCRIPTEXEC) $(ecmccfg_DIR)loadPLCFile.cmd, "PLC_ID=0, SAMPLE_RATE_MS=1000,FILE=./plc/can.plc")
|
||||
|
||||
iocInit()
|
||||
Reference in New Issue
Block a user