From b633281f692a1be23a9cd0ee104f1f86dad30b2f Mon Sep 17 00:00:00 2001 From: Edward Wall Date: Mon, 27 Apr 2026 14:06:05 +0200 Subject: [PATCH] format with clang-format --- sinqEPICSApp/src/NanotecDriver.cpp | 800 ++++++++++++++--------------- sinqEPICSApp/src/NanotecDriver.h | 71 +-- 2 files changed, 436 insertions(+), 435 deletions(-) diff --git a/sinqEPICSApp/src/NanotecDriver.cpp b/sinqEPICSApp/src/NanotecDriver.cpp index fdb02ed..3c851c6 100644 --- a/sinqEPICSApp/src/NanotecDriver.cpp +++ b/sinqEPICSApp/src/NanotecDriver.cpp @@ -1,15 +1,15 @@ /* FILENAME... NanotecDriver.cpp -USAGE... Motor driver support for the Nanotec SMCI controllers +USAGE... Motor driver support for the Nanotec SMCI controllers connected to a RS-485 bus. -These motors sit on a bus. The address on the bus is a random number, not -necessarily the motor number. These asyn motor drivers do not have access to -the motor record. Thus it was impossible to pass the bus address through. -The option was to create an axiliary record but this sucked too because the -bus address a motor is an initialisation parameter and should not be visible -in the EPICS database. Thus I choose to pass the bus address as a comma separated -list to the motor controller constructor. +These motors sit on a bus. The address on the bus is a random number, not +necessarily the motor number. These asyn motor drivers do not have access to +the motor record. Thus it was impossible to pass the bus address through. +The option was to create an axiliary record but this sucked too because the +bus address a motor is an initialisation parameter and should not be visible +in the EPICS database. Thus I choose to pass the bus address as a comma +separated list to the motor controller constructor. Mark Koennecke July 2015 @@ -19,22 +19,21 @@ Modified to use the MsgTxt field for SINQ Mark Koennecke, January 2019 */ - -#include -#include -#include -#include #include +#include +#include +#include +#include #include -#include #include #include +#include #include -#include "stptok.h" #include "NanotecDriver.h" +#include "stptok.h" #include #define IDLEPOLL 60 @@ -42,464 +41,463 @@ Mark Koennecke, January 2019 #define ABS(x) (x < 0 ? -(x) : (x)) /** Creates a new NanotecController object. - * \param[in] portName The name of the asyn port that will be created for this driver - * \param[in] NanotecPortName The name of the drvAsynSerialPort that was created previously to connect to the Nanotec controller - */ -NanotecController::NanotecController(const char *portName, const char *NanotecPortName, int motCount, const char *bus) - : SINQController(portName, NanotecPortName, motCount+1) -{ - int axis, busAddress; - asynStatus status; - NanotecAxis *pAxis; - static const char *functionName = "NanotecController::NanotecController"; - char *pPtr, busNoString[20]; + * \param[in] portName The name of the asyn port that will be created + * for this driver + * \param[in] NanotecPortName The name of the drvAsynSerialPort that was + * created previously to connect to the Nanotec controller + */ +NanotecController::NanotecController(const char *portName, + const char *NanotecPortName, int motCount, + const char *bus) + : SINQController(portName, NanotecPortName, motCount + 1) { + int axis, busAddress; + asynStatus status; + NanotecAxis *pAxis; + static const char *functionName = "NanotecController::NanotecController"; + char *pPtr, busNoString[20]; + /* Connect to Nanotec controller */ + status = pasynOctetSyncIO->connect(NanotecPortName, 0, + &pasynUserController_, NULL); + if (status) { + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: cannot connect to Nanotec controller\n", functionName); + } + pasynOctetSyncIO->setOutputEos(pasynUserController_, "\r", strlen("\r")); + pasynOctetSyncIO->setInputEos(pasynUserController_, "\r", strlen("\r")); - /* Connect to Nanotec controller */ - status = pasynOctetSyncIO->connect(NanotecPortName, 0, &pasynUserController_, NULL); - if (status) { - asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, - "%s: cannot connect to Nanotec controller\n", - functionName); - } - pasynOctetSyncIO->setOutputEos(pasynUserController_,"\r",strlen("\r")); - pasynOctetSyncIO->setInputEos(pasynUserController_,"\r",strlen("\r")); + axis = 1; + pPtr = (char *)bus; + while ((pPtr = stptok(pPtr, busNoString, sizeof(busNoString), ",")) != + NULL) { + busAddress = atoi(busNoString); + pAxis = new NanotecAxis(this, axis, busAddress); + errlogPrintf("Axis %d, busAddress = %d\n", axis, busAddress); + axis++; + } - axis = 1; - pPtr = (char *)bus; - while((pPtr = stptok(pPtr,busNoString,sizeof(busNoString),",")) != NULL){ - busAddress = atoi(busNoString); - pAxis = new NanotecAxis(this, axis,busAddress); - errlogPrintf("Axis %d, busAddress = %d\n", axis, busAddress); - axis++; - } - - startPoller(1000./1000., IDLEPOLL, 2); + startPoller(1000. / 1000., IDLEPOLL, 2); } - /** Creates a new NanotecController object. - * Configuration command, called directly or from iocsh - * \param[in] portName The name of the asyn port that will be created for this driver - * \param[in] NanotecPortName The name of the drvAsynIPPPort that was created previously to connect to the Nanotec controller - */ -extern "C" int NanotecCreateController(const char *portName, const char *NanotecPortName, int numMot, const char *busAddresses) -{ - NanotecController *pNanotecController - = new NanotecController(portName, NanotecPortName,numMot,busAddresses); - pNanotecController = NULL; - return(asynSuccess); + * Configuration command, called directly or from iocsh + * \param[in] portName The name of the asyn port that will be created + * for this driver + * \param[in] NanotecPortName The name of the drvAsynIPPPort that was + * created previously to connect to the Nanotec controller + */ +extern "C" int NanotecCreateController(const char *portName, + const char *NanotecPortName, int numMot, + const char *busAddresses) { + NanotecController *pNanotecController = + new NanotecController(portName, NanotecPortName, numMot, busAddresses); + pNanotecController = NULL; + return (asynSuccess); } /** Reports on status of the driver - * \param[in] fp The file pointer on which report information will be written - * \param[in] level The level of report detail desired - * - * If details > 0 then information is printed about each axis. - * After printing controller-specific information it calls asynMotorController::report() - */ -void NanotecController::report(FILE *fp, int level) -{ - fprintf(fp, "Nanotec motor driver %s, numAxes=%d\n", - this->portName, numAxes_); + * \param[in] fp The file pointer on which report information will be written + * \param[in] level The level of report detail desired + * + * If details > 0 then information is printed about each axis. + * After printing controller-specific information it calls + * asynMotorController::report() + */ +void NanotecController::report(FILE *fp, int level) { + fprintf(fp, "Nanotec motor driver %s, numAxes=%d\n", this->portName, + numAxes_); - // Call the base class method - asynMotorController::report(fp, level); + // Call the base class method + asynMotorController::report(fp, level); } /** Returns a pointer to an NanotecAxis object. - * Returns NULL if the axis number encoded in pasynUser is invalid. - * \param[in] pasynUser asynUser structure that encodes the axis index number. */ -NanotecAxis* NanotecController::getAxis(asynUser *pasynUser) -{ - return static_cast(asynMotorController::getAxis(pasynUser)); + * Returns NULL if the axis number encoded in pasynUser is invalid. + * \param[in] pasynUser asynUser structure that encodes the axis index number. + */ +NanotecAxis *NanotecController::getAxis(asynUser *pasynUser) { + return static_cast(asynMotorController::getAxis(pasynUser)); } /** Returns a pointer to an NanotecAxis object. - * Returns NULL if the axis number encoded in pasynUser is invalid. - * \param[in] axisNo Axis index number. */ -NanotecAxis* NanotecController::getAxis(int axisNo) -{ - return static_cast(asynMotorController::getAxis(axisNo)); + * Returns NULL if the axis number encoded in pasynUser is invalid. + * \param[in] axisNo Axis index number. */ +NanotecAxis *NanotecController::getAxis(int axisNo) { + return static_cast(asynMotorController::getAxis(axisNo)); } - - // These are the NanotecAxis methods /** Creates a new NanotecAxis object. - * \param[in] pC Pointer to the NanotecController to which this axis belongs. - * \param[in] axisNo Index number of this axis, range 0 to pC->numAxes_-1. - * - * Initializes register numbers, etc. - */ + * \param[in] pC Pointer to the NanotecController to which this axis belongs. + * \param[in] axisNo Index number of this axis, range 0 to pC->numAxes_-1. + * + * Initializes register numbers, etc. + */ NanotecAxis::NanotecAxis(NanotecController *pC, int axisNo, int busAddress) - : SINQAxis(pC, axisNo), - pC_(pC) -{ - this->busAddress = busAddress; - next_poll = -1; + : SINQAxis(pC, axisNo), pC_(pC) { + this->busAddress = busAddress; + next_poll = -1; } - - /** Reports on status of the axis - * \param[in] fp The file pointer on which report information will be written - * \param[in] level The level of report detail desired - * - * After printing device-specific information calls asynMotorAxis::report() - */ -void NanotecAxis::report(FILE *fp, int level) -{ - if (level > 0) { - fprintf(fp, " axis %d\n", - axisNo_); - } - - // Call the base class method - //asynMotorAxis::report(fp, level); -} - -asynStatus NanotecController::transactController(int axisNo, char command[COMLEN], char reply[COMLEN]) -{ - asynStatus status; - size_t in, out; - int reason; - SINQAxis *axis = getAxis(axisNo); - - pasynOctetSyncIO->flush(pasynUserController_); - - status = pasynOctetSyncIO->writeRead(pasynUserController_, command, strlen(command), - reply,COMLEN, 1.,&out,&in,&reason); - if(status != asynSuccess){ - if(axis != NULL){ - axis->updateMsgTxtFromDriver("Lost connection to motor controller"); + * \param[in] fp The file pointer on which report information will be written + * \param[in] level The level of report detail desired + * + * After printing device-specific information calls asynMotorAxis::report() + */ +void NanotecAxis::report(FILE *fp, int level) { + if (level > 0) { + fprintf(fp, " axis %d\n", axisNo_); } - return status; - } - /* - check for Nanotec errors - */ - if(strstr(reply,"?") != NULL){ - errlogSevPrintf(errlogMajor, "Bad command %s", command); - return asynError; - } - - if(strlen(reply) < 1) { - errlogSevPrintf(errlogMajor, "No reply received for %s", command); - return asynError; - } - - return status; + // Call the base class method + // asynMotorAxis::report(fp, level); } -asynStatus NanotecAxis::move(double position, int relative, double minVelocity, double maxVelocity, double acceleration) -{ - asynStatus status; - //static const char *functionName = "NanotecAxis::move"; - char command[COMLEN], reply[COMLEN]; - size_t in, out; - int reason; +asynStatus NanotecController::transactController(int axisNo, + char command[COMLEN], + char reply[COMLEN]) { + asynStatus status; + size_t in, out; + int reason; + SINQAxis *axis = getAxis(axisNo); - updateMsgTxtFromDriver(""); + pasynOctetSyncIO->flush(pasynUserController_); - // status = sendAccelAndVelocity(acceleration, maxVelocity); - - if (relative) { - position += this->position; - } + status = pasynOctetSyncIO->writeRead(pasynUserController_, command, + strlen(command), reply, COMLEN, 1., + &out, &in, &reason); + if (status != asynSuccess) { + if (axis != NULL) { + axis->updateMsgTxtFromDriver("Lost connection to motor controller"); + } + return status; + } - homing = 0; - setIntegerParam(pC_->motorStatusAtHome_, false); - setIntegerParam(pC_->motorStatusHighLimit_, false); - setIntegerParam(pC_->motorStatusLowLimit_, false); + /* + check for Nanotec errors + */ + if (strstr(reply, "?") != NULL) { + errlogSevPrintf(errlogMajor, "Bad command %s", command); + return asynError; + } + + if (strlen(reply) < 1) { + errlogSevPrintf(errlogMajor, "No reply received for %s", command); + return asynError; + } - /* - set mode - */ - snprintf(command,sizeof(command),"#%dp2",busAddress); - status = pC_->transactController(axisNo_,command,reply); - if(status != asynSuccess){ return status; - } - - /* - set target - */ - snprintf(command,sizeof(command),"#%ds%d",busAddress, (int)position); - status = pC_->transactController(axisNo_,command,reply); - if(status != asynSuccess){ - return status; - } - - /* - and start.. - */ - snprintf(command,sizeof(command),"#%dA",busAddress); - status = pC_->transactController(axisNo_,command,reply); - if(status != asynSuccess){ - return status; - } - - next_poll = -1; - return status; } -asynStatus NanotecAxis::home(double minVelocity, double maxVelocity, double acceleration, int forwards) -{ - asynStatus status; - //static const char *functionName = "NanotecAxis::home"; - char command[COMLEN], reply[COMLEN]; +asynStatus NanotecAxis::move(double position, int relative, double minVelocity, + double maxVelocity, double acceleration) { + asynStatus status; + // static const char *functionName = "NanotecAxis::move"; + char command[COMLEN], reply[COMLEN]; + size_t in, out; + int reason; - setIntegerParam(pC_->motorStatusAtHome_, false); + updateMsgTxtFromDriver(""); - updateMsgTxtFromDriver(""); + // status = sendAccelAndVelocity(acceleration, maxVelocity); - /* - reset positioning errors - */ - snprintf(command,sizeof(command),"#%dD",busAddress); - status = pC_->transactController(axisNo_,command,reply); - if(status != asynSuccess){ + if (relative) { + position += this->position; + } + + homing = 0; + setIntegerParam(pC_->motorStatusAtHome_, false); + setIntegerParam(pC_->motorStatusHighLimit_, false); + setIntegerParam(pC_->motorStatusLowLimit_, false); + + /* + set mode + */ + snprintf(command, sizeof(command), "#%dp2", busAddress); + status = pC_->transactController(axisNo_, command, reply); + if (status != asynSuccess) { + return status; + } + + /* + set target + */ + snprintf(command, sizeof(command), "#%ds%d", busAddress, (int)position); + status = pC_->transactController(axisNo_, command, reply); + if (status != asynSuccess) { + return status; + } + + /* + and start.. + */ + snprintf(command, sizeof(command), "#%dA", busAddress); + status = pC_->transactController(axisNo_, command, reply); + if (status != asynSuccess) { + return status; + } + + next_poll = -1; return status; - } - - /* - set mode - */ - snprintf(command,sizeof(command),"#%dp4",busAddress); - status = pC_->transactController(axisNo_,command,reply); - if(status != asynSuccess){ - return status; - } - - - /* - set direction - */ - snprintf(command,sizeof(command),"#%dd0",busAddress); - status = pC_->transactController(axisNo_,command,reply); - if(status != asynSuccess){ - return status; - } - - - /* - and start.. - */ - snprintf(command,sizeof(command),"#%dA",busAddress); - status = pC_->transactController(axisNo_,command,reply); - if(status != asynSuccess){ - return status; - } - - - homing = 1; - next_poll= -1; - return status; } -asynStatus NanotecAxis::moveVelocity(double minVelocity, double maxVelocity, double acceleration) -{ - asynStatus status; - //static const char *functionName = "NanotecAxis::moveVelocity"; +asynStatus NanotecAxis::home(double minVelocity, double maxVelocity, + double acceleration, int forwards) { + asynStatus status; + // static const char *functionName = "NanotecAxis::home"; + char command[COMLEN], reply[COMLEN]; + + setIntegerParam(pC_->motorStatusAtHome_, false); + + updateMsgTxtFromDriver(""); + + /* + reset positioning errors + */ + snprintf(command, sizeof(command), "#%dD", busAddress); + status = pC_->transactController(axisNo_, command, reply); + if (status != asynSuccess) { + return status; + } + + /* + set mode + */ + snprintf(command, sizeof(command), "#%dp4", busAddress); + status = pC_->transactController(axisNo_, command, reply); + if (status != asynSuccess) { + return status; + } + + /* + set direction + */ + snprintf(command, sizeof(command), "#%dd0", busAddress); + status = pC_->transactController(axisNo_, command, reply); + if (status != asynSuccess) { + return status; + } + + /* + and start.. + */ + snprintf(command, sizeof(command), "#%dA", busAddress); + status = pC_->transactController(axisNo_, command, reply); + if (status != asynSuccess) { + return status; + } + + homing = 1; + next_poll = -1; + return status; +} + +asynStatus NanotecAxis::moveVelocity(double minVelocity, double maxVelocity, + double acceleration) { + asynStatus status; + // static const char *functionName = "NanotecAxis::moveVelocity"; double target; - // asynPrint(pasynUser_, ASYN_TRACE_FLOW, - // "%s: minVelocity=%f, maxVelocity=%f, acceleration=%f\n", - // functionName, minVelocity, maxVelocity, acceleration); - - updateMsgTxtFromDriver(""); + // asynPrint(pasynUser_, ASYN_TRACE_FLOW, + // "%s: minVelocity=%f, maxVelocity=%f, acceleration=%f\n", + // functionName, minVelocity, maxVelocity, acceleration); + updateMsgTxtFromDriver(""); - if (maxVelocity > 0.) { - /* This is a positive move */ - pC_->getDoubleParam(axisNo_,pC_->motorHighLimit_,&target); - } else { - /* This is a negative move */ - pC_->getDoubleParam(axisNo_,pC_->motorLowLimit_,&target); - } + if (maxVelocity > 0.) { + /* This is a positive move */ + pC_->getDoubleParam(axisNo_, pC_->motorHighLimit_, &target); + } else { + /* This is a negative move */ + pC_->getDoubleParam(axisNo_, pC_->motorLowLimit_, &target); + } - status = move(target,0,0,0,0); + status = move(target, 0, 0, 0, 0); - return status; + return status; } -asynStatus NanotecAxis::stop(double acceleration ) -{ - asynStatus status; - //static const char *functionName = "NanotecAxis::stop"; - char command[COMLEN], reply[COMLEN]; +asynStatus NanotecAxis::stop(double acceleration) { + asynStatus status; + // static const char *functionName = "NanotecAxis::stop"; + char command[COMLEN], reply[COMLEN]; - sprintf(command, "#%dS1", busAddress); - status = pC_->transactController(axisNo_,command,reply); - errlogPrintf("Sent STOP on Axis %d\n", axisNo_); + sprintf(command, "#%dS1", busAddress); + status = pC_->transactController(axisNo_, command, reply); + errlogPrintf("Sent STOP on Axis %d\n", axisNo_); - return status; + return status; } -asynStatus NanotecAxis::setPosition(double position) -{ - asynStatus status; - //static const char *functionName = "NanotecAxis::setPosition"; - char command[COMLEN], reply[COMLEN]; +asynStatus NanotecAxis::setPosition(double position) { + asynStatus status; + // static const char *functionName = "NanotecAxis::setPosition"; + char command[COMLEN], reply[COMLEN]; - updateMsgTxtFromDriver(""); + updateMsgTxtFromDriver(""); - sprintf(command, "#%dD%d", busAddress, (int)position); - status = pC_->transactController(axisNo_,command,reply); - next_poll = -1; + sprintf(command, "#%dD%d", busAddress, (int)position); + status = pC_->transactController(axisNo_, command, reply); + next_poll = -1; - return status; + return status; } -asynStatus NanotecAxis::setClosedLoop(bool closedLoop) -{ - //static const char *functionName = "NanotecAxis::setClosedLoop"; - - /* - This belongs into the Kingdom of Electronics. - We do not do this. - */ +asynStatus NanotecAxis::setClosedLoop(bool closedLoop) { + // static const char *functionName = "NanotecAxis::setClosedLoop"; - return asynError; + /* + This belongs into the Kingdom of Electronics. + We do not do this. + */ + + return asynError; } /** Polls the axis. - * This function reads the motor position, the limit status, the home status, the moving status, - * and the drive power-on status. - * It calls setIntegerParam() and setDoubleParam() for each item that it polls, - * and then calls callParamCallbacks() at the end. - * \param[out] moving A flag that is set indicating that the axis is moving (true) or done (false). */ -asynStatus NanotecAxis::poll(bool *moving) -{ - asynStatus comStatus; - char command[COMLEN], reply[COMLEN]; - char *pPtr; - int posVal, statVal, count = 0; - double lowLim, highLim; + * This function reads the motor position, the limit status, the home status, + * the moving status, and the drive power-on status. It calls setIntegerParam() + * and setDoubleParam() for each item that it polls, and then calls + * callParamCallbacks() at the end. + * \param[out] moving A flag that is set indicating that the axis is moving + * (true) or done (false). */ +asynStatus NanotecAxis::poll(bool *moving) { + asynStatus comStatus; + char command[COMLEN], reply[COMLEN]; + char *pPtr; + int posVal, statVal, count = 0; + double lowLim, highLim; - - // protect against excessive polling - if(time(NULL) < next_poll){ - *moving = false; - return asynSuccess; - } - - // Read the current motor position - sprintf(command,"#%dC", busAddress); - comStatus = pC_->transactController(axisNo_,command,reply); - if(comStatus) goto skip; - - pPtr = strchr(reply,'C'); - if(pPtr){ - pPtr++; - count = sscanf(pPtr,"%d", &posVal); - } - if(pPtr == NULL || count < 1) { - errlogPrintf("Invalid response %s for #C received for axis %d, address %d\n", reply, axisNo_, busAddress); - return asynError; - } - - //errlogPrintf("Axis %d, reply %s, position %d\n", axisNo_, reply, posVal); - setDoubleParam(pC_->motorPosition_, (double)posVal); - //setDoubleParam(pC_->motorEncoderPosition_, position); - - - // Read the moving status of this motor - sprintf(command,"#%d$",busAddress); - comStatus = pC_->transactController(axisNo_,command,reply); - if(comStatus) goto skip; - - pPtr = strchr(reply,'$'); - if(pPtr) { - pPtr++; - count = sscanf(pPtr, "%d", &statVal); - } - if(pPtr == NULL || count < 1) { - errlogPrintf("Invalid response %s for #$ received for axis %d busAddress %d\n", reply, axisNo_, busAddress); - return asynError; - } - //errlogPrintf("Axis %d, reply %s, statVal = %d\n", - // axisNo_, reply, statVal); - - setIntegerParam(pC_->motorStatusDone_, false); - *moving = true; - pC_->getDoubleParam(axisNo_,pC_->motorLowLimit_,&lowLim); - pC_->getDoubleParam(axisNo_,pC_->motorHighLimit_,&highLim); - - if(homing){ - /* - code for homing - */ - switch(statVal) { - case 163: - setPosition(lowLim); - *moving = false; - setIntegerParam(pC_->motorStatusAtHome_, true); - setIntegerParam(pC_->motorStatusDone_, true); - break; - default : - if(statVal & 1) { - *moving = false; - setIntegerParam(pC_->motorStatusAtHome_, true); - setIntegerParam(pC_->motorStatusDone_, true); - } - break; + // protect against excessive polling + if (time(NULL) < next_poll) { + *moving = false; + return asynSuccess; } - } else { - /* - code for normal movement - */ - if(statVal & 1) { - *moving = false; - setIntegerParam(pC_->motorStatusDone_, true); - } else if (statVal & 4) { - setIntegerParam(pC_->motorStatusDone_, true); - setIntegerParam(pC_->motorStatusProblem_, true); - errlogSevPrintf(errlogMajor, "Limit or other positioning problem at %d", axisNo_); - updateMsgTxtFromDriver("Positioning problem"); - if(ABS(posVal - lowLim) < ABS(posVal - highLim)){ - setIntegerParam(pC_->motorStatusLowLimit_, true); - updateMsgTxtFromDriver("Low Limit Hit"); - } else { - setIntegerParam(pC_->motorStatusHighLimit_, true); - updateMsgTxtFromDriver("High Limit Hit"); - } - *moving = false; - } - } - if(*moving == true){ - next_poll = -1; - } + // Read the current motor position + sprintf(command, "#%dC", busAddress); + comStatus = pC_->transactController(axisNo_, command, reply); + if (comStatus) + goto skip; - skip: - setIntegerParam(pC_->motorStatusProblem_, comStatus ? 1:0); - callParamCallbacks(); - return comStatus ? asynError : asynSuccess; + pPtr = strchr(reply, 'C'); + if (pPtr) { + pPtr++; + count = sscanf(pPtr, "%d", &posVal); + } + if (pPtr == NULL || count < 1) { + errlogPrintf( + "Invalid response %s for #C received for axis %d, address %d\n", + reply, axisNo_, busAddress); + return asynError; + } + + // errlogPrintf("Axis %d, reply %s, position %d\n", axisNo_, reply, posVal); + setDoubleParam(pC_->motorPosition_, (double)posVal); + // setDoubleParam(pC_->motorEncoderPosition_, position); + + // Read the moving status of this motor + sprintf(command, "#%d$", busAddress); + comStatus = pC_->transactController(axisNo_, command, reply); + if (comStatus) + goto skip; + + pPtr = strchr(reply, '$'); + if (pPtr) { + pPtr++; + count = sscanf(pPtr, "%d", &statVal); + } + if (pPtr == NULL || count < 1) { + errlogPrintf( + "Invalid response %s for #$ received for axis %d busAddress %d\n", + reply, axisNo_, busAddress); + return asynError; + } + // errlogPrintf("Axis %d, reply %s, statVal = %d\n", + // axisNo_, reply, statVal); + + setIntegerParam(pC_->motorStatusDone_, false); + *moving = true; + pC_->getDoubleParam(axisNo_, pC_->motorLowLimit_, &lowLim); + pC_->getDoubleParam(axisNo_, pC_->motorHighLimit_, &highLim); + + if (homing) { + /* + code for homing + */ + switch (statVal) { + case 163: + setPosition(lowLim); + *moving = false; + setIntegerParam(pC_->motorStatusAtHome_, true); + setIntegerParam(pC_->motorStatusDone_, true); + break; + default: + if (statVal & 1) { + *moving = false; + setIntegerParam(pC_->motorStatusAtHome_, true); + setIntegerParam(pC_->motorStatusDone_, true); + } + break; + } + } else { + /* + code for normal movement + */ + if (statVal & 1) { + *moving = false; + setIntegerParam(pC_->motorStatusDone_, true); + } else if (statVal & 4) { + setIntegerParam(pC_->motorStatusDone_, true); + setIntegerParam(pC_->motorStatusProblem_, true); + errlogSevPrintf(errlogMajor, + "Limit or other positioning problem at %d", + axisNo_); + updateMsgTxtFromDriver("Positioning problem"); + if (ABS(posVal - lowLim) < ABS(posVal - highLim)) { + setIntegerParam(pC_->motorStatusLowLimit_, true); + updateMsgTxtFromDriver("Low Limit Hit"); + } else { + setIntegerParam(pC_->motorStatusHighLimit_, true); + updateMsgTxtFromDriver("High Limit Hit"); + } + *moving = false; + } + } + + if (*moving == true) { + next_poll = -1; + } + +skip: + setIntegerParam(pC_->motorStatusProblem_, comStatus ? 1 : 0); + callParamCallbacks(); + return comStatus ? asynError : asynSuccess; } /** Code for iocsh registration */ -static const iocshArg NanotecCreateControllerArg0 = {"Port name", iocshArgString}; -static const iocshArg NanotecCreateControllerArg1 = {"Nanotec port name", iocshArgString}; -static const iocshArg NanotecCreateControllerArg2 = {"Number of axes", iocshArgInt}; -static const iocshArg NanotecCreateControllerArg3 = {"Komma separated list of bus addresses", iocshArgString}; -static const iocshArg * const NanotecCreateControllerArgs[] = {&NanotecCreateControllerArg0, - &NanotecCreateControllerArg1, - &NanotecCreateControllerArg2, - &NanotecCreateControllerArg3 - }; -static const iocshFuncDef NanotecCreateControllerDef = {"NanotecCreateController", 4, NanotecCreateControllerArgs}; -static void NanotecCreateContollerCallFunc(const iocshArgBuf *args) -{ - NanotecCreateController(args[0].sval, args[1].sval, args[2].ival,args[3].sval); +static const iocshArg NanotecCreateControllerArg0 = {"Port name", + iocshArgString}; +static const iocshArg NanotecCreateControllerArg1 = {"Nanotec port name", + iocshArgString}; +static const iocshArg NanotecCreateControllerArg2 = {"Number of axes", + iocshArgInt}; +static const iocshArg NanotecCreateControllerArg3 = { + "Komma separated list of bus addresses", iocshArgString}; +static const iocshArg *const NanotecCreateControllerArgs[] = { + &NanotecCreateControllerArg0, &NanotecCreateControllerArg1, + &NanotecCreateControllerArg2, &NanotecCreateControllerArg3}; +static const iocshFuncDef NanotecCreateControllerDef = { + "NanotecCreateController", 4, NanotecCreateControllerArgs}; +static void NanotecCreateContollerCallFunc(const iocshArgBuf *args) { + NanotecCreateController(args[0].sval, args[1].sval, args[2].ival, + args[3].sval); } -static void NanotecRegister(void) -{ - iocshRegister(&NanotecCreateControllerDef, NanotecCreateContollerCallFunc); +static void NanotecRegister(void) { + iocshRegister(&NanotecCreateControllerDef, NanotecCreateContollerCallFunc); } extern "C" { diff --git a/sinqEPICSApp/src/NanotecDriver.h b/sinqEPICSApp/src/NanotecDriver.h index c2915f1..ece5222 100644 --- a/sinqEPICSApp/src/NanotecDriver.h +++ b/sinqEPICSApp/src/NanotecDriver.h @@ -10,51 +10,54 @@ Modified to use the MsgTxt field for SINQ Mark Koennecke, January 2019 */ -#include "SINQController.h" #include "SINQAxis.h" +#include "SINQController.h" #define COMLEN 80 #define MAXMOT 99 -class NanotecAxis : public SINQAxis -{ -public: - /* These are the methods we override from the base class */ - NanotecAxis(class NanotecController *pC, int axis, int busAddress); - void report(FILE *fp, int level); - asynStatus move(double position, int relative, double min_velocity, double max_velocity, double acceleration); - asynStatus moveVelocity(double min_velocity, double max_velocity, double acceleration); - asynStatus home(double min_velocity, double max_velocity, double acceleration, int forwards); - asynStatus stop(double acceleration); - asynStatus poll(bool *moving); - asynStatus setPosition(double position); - asynStatus setClosedLoop(bool closedLoop); +class NanotecAxis : public SINQAxis { + public: + /* These are the methods we override from the base class */ + NanotecAxis(class NanotecController *pC, int axis, int busAddress); + void report(FILE *fp, int level); + asynStatus move(double position, int relative, double min_velocity, + double max_velocity, double acceleration); + asynStatus moveVelocity(double min_velocity, double max_velocity, + double acceleration); + asynStatus home(double min_velocity, double max_velocity, + double acceleration, int forwards); + asynStatus stop(double acceleration); + asynStatus poll(bool *moving); + asynStatus setPosition(double position); + asynStatus setClosedLoop(bool closedLoop); -private: - NanotecController *pC_; /**< Pointer to the asynMotorController to which this axis belongs. - * Abbreviated because it is used very frequently */ - double position; - int homing; - time_t next_poll; - int busAddress; - -friend class NanotecController; + private: + NanotecController + *pC_; /**< Pointer to the asynMotorController to which this axis + * belongs. Abbreviated because it is used very frequently */ + double position; + int homing; + time_t next_poll; + int busAddress; + + friend class NanotecController; }; class NanotecController : public SINQController { -public: - NanotecController(const char *portName, const char *NanotecPortName, int numMot, const char *busAddresses); + public: + NanotecController(const char *portName, const char *NanotecPortName, + int numMot, const char *busAddresses); - void report(FILE *fp, int level); - NanotecAxis* getAxis(asynUser *pasynUser); - NanotecAxis* getAxis(int axisNo); - -friend class NanotecAxis; - private: - asynUser *pasynUserController_; - - asynStatus transactController(int axisNo, char command[COMLEN], char reply[COMLEN]); + void report(FILE *fp, int level); + NanotecAxis *getAxis(asynUser *pasynUser); + NanotecAxis *getAxis(int axisNo); + friend class NanotecAxis; + private: + asynUser *pasynUserController_; + asynStatus transactController(int axisNo, char command[COMLEN], + char reply[COMLEN]); };