From 53a98f2849b18482f7d49010de8c50787ad5cece Mon Sep 17 00:00:00 2001 From: MarkRivers Date: Wed, 12 Sep 2012 17:16:54 +0000 Subject: [PATCH] New file from Nia Fong at SLAC --- motorApp/ImsSrc/ImsMDrivePlusMotorAxis.cpp | 591 ++++++++++++++++++ motorApp/ImsSrc/ImsMDrivePlusMotorAxis.h | 71 +++ .../ImsSrc/ImsMDrivePlusMotorController.cpp | 356 +++++++++++ .../ImsSrc/ImsMDrivePlusMotorController.h | 74 +++ 4 files changed, 1092 insertions(+) create mode 100644 motorApp/ImsSrc/ImsMDrivePlusMotorAxis.cpp create mode 100644 motorApp/ImsSrc/ImsMDrivePlusMotorAxis.h create mode 100644 motorApp/ImsSrc/ImsMDrivePlusMotorController.cpp create mode 100644 motorApp/ImsSrc/ImsMDrivePlusMotorController.h diff --git a/motorApp/ImsSrc/ImsMDrivePlusMotorAxis.cpp b/motorApp/ImsSrc/ImsMDrivePlusMotorAxis.cpp new file mode 100644 index 00000000..7a8880e7 --- /dev/null +++ b/motorApp/ImsSrc/ImsMDrivePlusMotorAxis.cpp @@ -0,0 +1,591 @@ +//! @File : ImsMDrivePlusMotorAxis.cpp +//! Motor record driver level support for Intelligent Motion Systems, Inc. +//! MDrivePlus series; M17, M23, M34. +//! Use "model 3" asyn motor, asynMotorController and asynMotorAxis classes. +//! +//! Author : Nia Fong +//! Date : 11-21-2011 +// +// Revision History +// ---------------- +// 11-21-2011 NF Initial version + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ImsMDrivePlusMotorController.h" + +//////////////////////////////////////////////////////// +//! ImsMDrivePlusMotorAxis() +// Constructor +// +//! @param[in] pC pointer to ImsMDrivePlusMotorController +//! @param[in] axisNum axis number +//////////////////////////////////////////////////////// +ImsMDrivePlusMotorAxis::ImsMDrivePlusMotorAxis(ImsMDrivePlusMotorController *pC, int axisNum) + : asynMotorAxis(pC, axisNum), pController(pC) +{ + static const char *functionName = "ImsMDrivePlusMotorAxis()"; + asynPrint(pC->pasynUserSelf, ASYN_TRACEIO_DRIVER, "%s:%s: Create Axis %d\n", DRIVER_NAME, functionName, axisNum); + + // run setup/initialize routines here + // check communication, set moving status + if (configAxis() == asynError) { + asynPrint(pC->pasynUserSelf, ASYN_TRACE_ERROR, "%s:%s: controller config failed for motor port=%s\n", DRIVER_NAME, functionName, pController->motorName); + // TODO throw exception + } + callParamCallbacks(); +} + +//////////////////////////////////////// +//! configAxis() +//! Used smarACTMCMotorDriver.cpp as reference +// +//! check communication by checking version returns a good string +//! set moving status (PR MV) +//////////////////////////////////////// +asynStatus ImsMDrivePlusMotorAxis::configAxis() +{ + asynStatus status = asynError; + char cmd[MAX_CMD_LEN]; + char resp[MAX_BUFF_LEN]; + size_t nread; + int maxRetries=3; + static const char *functionName = "configAxis()"; + // figure out what needs to be done to initialize controller + + // try getting firmware version to make sure communication works + sprintf(cmd, "PR VR"); + for (int i=0; iwriteReadController(cmd, resp, sizeof(resp), &nread, IMS_TIMEOUT); + asynPrint(pController->pasynUserSelf, ASYN_TRACEIO_DRIVER, "%s:%s: Version retry.\n", DRIVER_NAME, functionName); + if (status == asynError) { + asynPrint(pController->pasynUserSelf, ASYN_TRACE_ERROR, "%s:%s: Version inquiry FAILED.\n", DRIVER_NAME, functionName); + } else { // ok to check firmware level/format or just strlen? v3.009 + if (strlen(resp) < 2) { + asynPrint(pController->pasynUserSelf, ASYN_TRACE_ERROR, "%s:%s: Version inquiry FAILED version=%s.\n", DRIVER_NAME, functionName, resp); + setIntegerParam(pController->motorStatusProblem_, 1); + setIntegerParam(pController->motorStatusCommsError_, 1); + status = asynError; return(status); + } + break; + } + } + + // set encoder flags + sprintf(cmd, "PR EE"); + status = pController->writeReadController(cmd, resp, sizeof(resp), &nread, IMS_TIMEOUT); + if (status == asynSuccess) { + int val = atoi(resp); + setIntegerParam(pController->motorStatusHasEncoder_, val ? 1:0); + setIntegerParam(pController->motorStatusGainSupport_, val ? 1:0); + asynPrint(pController->pasynUserSelf, ASYN_TRACEIO_DRIVER, "%s:%s: set motorStatusHasEncoder_=%d, motorStatusGainSupport_=%d.\n", DRIVER_NAME, functionName, val, val); + } + + // start idle timer + //idleTimeStart = epicsTime::getCurrent(); + + return status; +} + + +//////////////////////////////////////////////////////// +//! setAxisMoveParameters() +//! set base velocity, moving velocity, and acceleration +// +//! @param[in] minVelocity +//! @param[in] maxVelocity +//! @param[in] acceleration +//////////////////////////////////////////////////////// +asynStatus ImsMDrivePlusMotorAxis::setAxisMoveParameters(double minVelocity, double maxVelocity, double acceleration) +{ + asynStatus status = asynError; + char cmd[MAX_CMD_LEN]; + static const char *functionName = "setAxisMoveParameters()"; + + // check if using base velocity (VI) + // for MDrivePlus initial velocity must be below max_velocity + if (minVelocity > 0) { // base velocity set + if (minVelocity > maxVelocity) { // illegal state + asynPrint(pController->pasynUserSelf, ASYN_TRACE_ERROR, "%s:%s: base velocity=%f cannot be greater than max velocity=%f\n", DRIVER_NAME, functionName, minVelocity, maxVelocity); + goto bail; + } + // set base velocity + sprintf(cmd, "VI=%ld", (long)minVelocity); + status = pController->writeController(cmd, IMS_TIMEOUT); + if (status) goto bail; + } + + + // set velocity + sprintf(cmd, "VM=%ld", (long)maxVelocity); + status = pController->writeController(cmd, IMS_TIMEOUT); + if (status) goto bail; + + // set accceleration + if (acceleration != 0) { + sprintf(cmd, "A=%ld", (long)maxVelocity); + status = pController->writeController(cmd, IMS_TIMEOUT); + if (status) goto bail; + } + + bail: + if (status) { + char buff[LOCAL_LINE_LEN]; + sprintf(buff, "%s:%s: ERROR setting motor velocity and acceleration", DRIVER_NAME, functionName); + handleAxisError(buff); + } + + callParamCallbacks(); + return status; +} + +//////////////////////////////////////////////////////// +//! move() +//! Override asynMotorAxis class implementation +// Based on smarActMCSMotorDriver.cpp +// +//! @param[in] position +//! @param[in] relative making absolute or relative move +//! @param[in] minVelocity +//! @param[in] maxVelocity +//! @param[in] acceleration +//////////////////////////////////////////////////////// +asynStatus ImsMDrivePlusMotorAxis::move(double position, int relative, double minVelocity, double maxVelocity, double acceleration) +{ + asynStatus status = asynError; + char cmd[MAX_CMD_LEN]; + static const char *functionName = "move()"; + + // sent commands to motor to set velocities and acceleration + status = setAxisMoveParameters(minVelocity, maxVelocity, acceleration); + if (status) goto bail; + + // move + asynPrint(pController->pasynUserSelf, ASYN_TRACEIO_DRIVER, "%s:%s: VBASE=%f, VELO=%f, ACCL=%f, position=%f, relative=%d\n", DRIVER_NAME, functionName, minVelocity, maxVelocity, acceleration, position, relative); + if (relative) { // relative move MR + sprintf(cmd, "MR %ld", (long)position); + } else { // absolute move MA + sprintf(cmd, "MA %ld", (long)position); + } + status = pController->writeController(cmd, IMS_TIMEOUT); + if (status) goto bail; + + bail: + if (status) { + char buff[LOCAL_LINE_LEN]; + sprintf(buff, "%s:%s: ERROR moving motor", DRIVER_NAME, functionName); + handleAxisError(buff); + } + + callParamCallbacks(); + return status; +} + +//////////////////////////////////////////////////////// +//! moveVelocity() +//! Override asynMotorAxis class implementation +// Based on smarActMCSMotorDriver.cpp +//! move at a fixed velocity until stop() called +// +//! @param[in] minVelocity +//! @param[in] maxVelocity +//! @param[in] acceleration +//////////////////////////////////////////////////////// +asynStatus ImsMDrivePlusMotorAxis::moveVelocity(double minVelocity, double maxVelocity, double acceleration) +{ + asynStatus status = asynError; + char cmd[MAX_CMD_LEN]; + static const char *functionName = "moveVelocity()"; + + // sent commands to motor to set velocities and acceleration + status = setAxisMoveParameters(minVelocity, maxVelocity, acceleration); + if (status) goto bail; + + // move + asynPrint(pController->pasynUserSelf, ASYN_TRACEIO_DRIVER, "%s:%s: VBASE=%f, VELO=%f, ACCL=%f\n", DRIVER_NAME, functionName, minVelocity, maxVelocity, acceleration); + sprintf(cmd, "SL %ld", (long)maxVelocity); + status = pController->writeController(cmd, IMS_TIMEOUT); + if (status) goto bail; + + bail: + if (status) { + char buff[LOCAL_LINE_LEN]; + sprintf(buff, "%s:%s: ERROR jogging motor", DRIVER_NAME, functionName); + handleAxisError(buff); + } + + callParamCallbacks(); + return status; +} + +//////////////////////////////////////////////////////// +//! stop() +//! Override asynMotorAxis class implementation +// Based on smarActMCSMotorDriver.cpp +// +//! @param[in] acceleration +//////////////////////////////////////////////////////// +asynStatus ImsMDrivePlusMotorAxis::stop(double acceleration) +{ + asynStatus status = asynError; + char cmd[MAX_CMD_LEN]; + static const char *functionName = "stop()"; + + // set accceleration + if (acceleration != 0) { + sprintf(cmd, "A=%ld", (long)acceleration); + status = pController->writeController(cmd, IMS_TIMEOUT); + if (status) goto bail; + } + + // move + sprintf(cmd, "SL 0"); + status = pController->writeController(cmd, IMS_TIMEOUT); + if (status) goto bail; + + bail: + if (status) { + char buff[LOCAL_LINE_LEN]; + sprintf(buff, "%s:%s: ERROR stopping motor", DRIVER_NAME, functionName); + handleAxisError(buff); + } + + callParamCallbacks(); + return status; +} + +//////////////////////////////////////////////////////// +//! home() +//! Override asynMotorAxis class implementation +// Based on smarActMCSMotorDriver.cpp +// +//! direction=1: slew at maxVelocity in the minus direction (until switch activates) then creep at vi in the plus direction (until switch becomes inactive again) +//! direction=3: slew at maxVelocity in the plus direction (until switch activates) then creep at vi in the minus direction (until switch becomes inactive again) +// +//! @param[in] minVelocity +//! @param[in] maxVelocity +//! @param[in] acceleration +//! @param[in] forwards direction to home, 0=minus direction, 1=plus direction +//////////////////////////////////////////////////////// +asynStatus ImsMDrivePlusMotorAxis::home(double minVelocity, double maxVelocity, double acceleration, int forwards) +{ + asynStatus status = asynError; + char cmd[MAX_CMD_LEN]; + char resp[MAX_BUFF_LEN]; + size_t nread; + int direction = 1; // direction to home, initialize homing in minus direction + double baseVelocity=0; + static const char *functionName = "home()"; + + // check if using base velocity (VI) + // for MDrivePlus initial velocity must be below max_velocity + if (minVelocity > 0) { // base velocity being configured + if (minVelocity > maxVelocity) { // illegal state + asynPrint(pController->pasynUserSelf, ASYN_TRACE_ERROR, "%s:%s: base velocity=%f cannot be greater than max velocity=%f\n", DRIVER_NAME, functionName, minVelocity, maxVelocity); + goto bail; + } + } else { // base velocity needs to be set because creeping back to home switch at base velocity, so make sure it's nonzero + sprintf(cmd, "PR VI"); // get base velocity setting + status = pController->writeReadController(cmd, resp, sizeof(resp), &nread, IMS_TIMEOUT); + if (status) goto bail; + baseVelocity = atof(resp); + if (baseVelocity == 0) { // set to factory default of 1000 + baseVelocity=1000; + } + } + + // sent commands to motor to set velocities and acceleration + if (status = setAxisMoveParameters(minVelocity, maxVelocity, acceleration)) goto bail; + + // home + if (forwards == 1) { // homing in forward direction + direction = 3; + } + asynPrint(pController->pasynUserSelf, ASYN_TRACEIO_DRIVER, "%s:%s: VBASE=%f, VELO=%f, ACCL=%f, forwards=%d\n", DRIVER_NAME, functionName, minVelocity, maxVelocity, acceleration, forwards); + sprintf(cmd, "HM %d", direction); + status = pController->writeController(cmd, IMS_TIMEOUT); + if (status) goto bail; + + bail: + if (status) { + char buff[LOCAL_LINE_LEN]; + sprintf(buff, "%s:%s: ERROR homing motor", DRIVER_NAME, functionName); + handleAxisError(buff); + } + + callParamCallbacks(); + return status; +} + +//////////////////////////////////////////////////////// +//! setPosition() +//! Override asynMotorAxis class implementation +// Based on smarActMCSMotorDriver.cpp +// +//! @param[in] position value to set motor's internal position +//////////////////////////////////////////////////////// +asynStatus ImsMDrivePlusMotorAxis::setPosition(double position) +{ + asynStatus status = asynError; + char cmd[MAX_CMD_LEN]; + static const char *functionName = "setPosition()"; + + // set position + asynPrint(pController->pasynUserSelf, ASYN_TRACEIO_DRIVER, "%s:%s: position=%f\n", DRIVER_NAME, functionName, position); + sprintf(cmd, "P=%ld", (long)position); + status = pController->writeController(cmd, IMS_TIMEOUT); + if (status) goto bail; + + bail: + if (status) { + char buff[LOCAL_LINE_LEN]; + sprintf(buff, "%s:%s: ERROR setting motor position", DRIVER_NAME, functionName); + handleAxisError(buff); + } + + callParamCallbacks(); + return status; +} + + +//////////////////////////////////////////////////////// +//! poll() +//! Override asynMotorAxis class implementation +// Based on smarActMCSMotorDriver.cpp +// +//! Set position and moving flag +// +//! @param[in] moving pointer to moving flag +//////////////////////////////////////////////////////// +asynStatus ImsMDrivePlusMotorAxis::poll(bool *moving) +{ + asynStatus status = asynError; + char cmd[MAX_CMD_LEN]; + char resp[MAX_BUFF_LEN]; + size_t nread; + int val=0; + double position; + *moving = false; + static const char *functionName = "poll()"; + //epicsTime currentTime; + + // get position + sprintf(cmd, "PR P"); + status = pController->writeReadController(cmd, resp, sizeof(resp), &nread, IMS_TIMEOUT); + if (status) goto bail; + position = atof(resp); + // update motor record position values, just update encoder's even if not using one + setDoubleParam(pController->motorEncoderPosition_, position); + setDoubleParam(pController->motorPosition_, position); + + // get moving flag + sprintf(cmd, "PR MV"); + status = pController->writeReadController(cmd, resp, sizeof(resp), &nread, IMS_TIMEOUT); + if (status) goto bail; + val = atoi(resp); + if (val == 1) *moving = true; // updating moving flag +/* else { // not moving + if (prevMovingState == 1) {// state changed, moving before, start idle timer + idleTimeStart = epicsTime::getCurrent(); + } + } + prevMovingState = val; +*/ + // update motor record status done with moving status + setIntegerParam(pController->motorStatusDone_, ! *moving ); + +/* + // if position has changed and moving stopped for over 30sec, update NVM with current position so the motor will remember + // its location just in case its power dies + currentTime = epicsTime::getCurrent(); + idleTime = currentTime - idleTimeStart; + //printf("prevPos=%f, pos=%f, moving=%d, idleTime=%f\n", prevPosition, position, *moving, idleTime); + if (prevPosition != position && *moving == false && idleTime > 30) { + sprintf(cmd, "S"); + if (status = pController->writeController(cmd, IMS_TIMEOUT)) { + asynPrint(pController->pasynUserSelf, ASYN_TRACEIO_DRIVER, "%s:poll(): ERROR saving position to NVM\n", DRIVER_NAME); + goto bail; + } + asynPrint(pController->pasynUserSelf, ASYN_TRACEIO_DRIVER, "%s:poll(): saving parameters to NVM\n", DRIVER_NAME); + idleTimeStart = epicsTime::getCurrent(); + prevPosition = position; + } +*/ + + // get home switch value + if (pController->homeSwitchInput != -1) { + sprintf(cmd, "PR I%d", pController->homeSwitchInput); + status = pController->writeReadController(cmd, resp, sizeof(resp), &nread, IMS_TIMEOUT); + if (status) goto bail; + val = atoi(resp); + setIntegerParam(pController->motorStatusHome_, val); + } + + // get positive limit switch value + if (pController->posLimitSwitchInput != -1) { + sprintf(cmd, "PR I%d", pController->posLimitSwitchInput); + status = pController->writeReadController(cmd, resp, sizeof(resp), &nread, IMS_TIMEOUT); + if (status) goto bail; + val = atoi(resp); + setIntegerParam(pController->motorStatusHighLimit_, val); + } + + // get negative limit switch value + if (pController->negLimitSwitchInput != -1) { + sprintf(cmd, "PR I%d", pController->negLimitSwitchInput); + status = pController->writeReadController(cmd, resp, sizeof(resp), &nread, IMS_TIMEOUT); + if (status) goto bail; + val = atoi(resp); + setIntegerParam(pController->motorStatusLowLimit_, val); + } + + // error polling + bail: + if (status) { + char buff[LOCAL_LINE_LEN]; + sprintf(buff, "%s:%s: ERROR polling motor", DRIVER_NAME, functionName); + handleAxisError(buff); + } + + // update motor status if polling was successful + if (status == 0) { + setIntegerParam(pController->motorStatusCommsError_, 0); // reset COMM error + setIntegerParam(pController->motorStatusProblem_, 0); // reset problem error, bit 10 + } + + // update motor record + callParamCallbacks(); + + int mstat; + pController->getIntegerParam(pController->motorStatus_, &mstat); + asynPrint(pController->pasynUserSelf, ASYN_TRACEIO_DRIVER, "%s:%s: POS=%f, MSTAT=%d\n", DRIVER_NAME, functionName, position, mstat); + + return status; + +} + +//////////////////////////////////////////////////////// +//! saveToNVM() +//! Save user variables and flags to non-volatile RAM in case of power loss +// +//////////////////////////////////////////////////////// +asynStatus ImsMDrivePlusMotorAxis::saveToNVM() +{ + asynStatus status = asynError; + char cmd[MAX_CMD_LEN]; + static const char *functionName = "saveToNVM()"; + + // send save command + sprintf(cmd, "S"); + status = pController->writeController(cmd, IMS_TIMEOUT); + if (status) goto bail; + asynPrint(pController->pasynUserSelf, ASYN_TRACEIO_DRIVER, "%s:%s: Saved to NVM\n", DRIVER_NAME, functionName); + + bail: + if (status) { + char buff[LOCAL_LINE_LEN]; + sprintf(buff, "%s:%s: ERROR saving to NVM", DRIVER_NAME, functionName); + handleAxisError(buff); + } + + callParamCallbacks(); //FIXME is this necessary?? + return status; +} + +//////////////////////////////////////////////////////// +//! handleAxisError() +//! Set motorStatusProblem_ +//! Then try to get and print error code +// +////! @param[in] errMsg pointer to error message from calling function +//////////////////////////////////////////////////////// +void ImsMDrivePlusMotorAxis::handleAxisError(char *errMsg) +{ + char cmd[MAX_CMD_LEN]; + char resp[MAX_BUFF_LEN]; + size_t nread=0; + int errCode=0; + char errCodeString[MAX_BUFF_LEN]; + static const char *functionName = "handleAxisError()"; + + // set motorStatusProblem_ bit + setIntegerParam(pController->motorStatusProblem_, 1); + + // read error code + sprintf(cmd, "PR ER"); + pController->writeReadController(cmd, resp, sizeof(resp), &nread, IMS_TIMEOUT); + errCode = atoi(resp); + + switch (errCode) { + case 0: strcpy(errCodeString, "No Error"); break; + case 6: strcpy(errCodeString, "An I/O is already set to this type. Applies to non-General Purpose I/O"); break; + case 8: strcpy(errCodeString, "Tried to set an I/O to an incorrect I/O type"); break; + case 9: strcpy(errCodeString, "Tried to write to I/O set as Input or is 'TYPED'"); break; + case 10: strcpy(errCodeString, "Illegal I/O number"); break; + case 11: strcpy(errCodeString, "Incorrect CLOCK type"); break; + case 12: strcpy(errCodeString, "Illegal Trip/Capture"); break; + case 20: strcpy(errCodeString, "Tried to set unknown variable or flag"); break; + case 21: strcpy(errCodeString, "Tried to set an incorrect value"); break; + case 22: strcpy(errCodeString, "VI is set greater than or equal to VM"); break; + case 23: strcpy(errCodeString, "VM is set less than or equal to VI"); break; + case 24: strcpy(errCodeString, "Illegal data entered"); break; + case 25: strcpy(errCodeString, "Variable or flag is read only"); break; + case 26: strcpy(errCodeString, "Variable or flag is not allowed to be incremented or decremented"); break; + case 27: strcpy(errCodeString, "Trip not defined"); break; + case 28: strcpy(errCodeString, "Trying to redefine a program label or variable"); break; + case 29: strcpy(errCodeString, "Trying to redefine a build in command, variable, or flag"); break; + case 30: strcpy(errCodeString, "Unknown label or user variable"); break; + case 31: strcpy(errCodeString, "Program label or user variable table is full"); break; + case 32: strcpy(errCodeString, "Trying to set a label"); break; + case 33: strcpy(errCodeString, "Trying to set and instruction"); break; + case 34: strcpy(errCodeString, "Trying to execute a variable or flag"); break; + case 35: strcpy(errCodeString, "Trying to print illegal variable or flag"); break; + case 36: strcpy(errCodeString, "Illegal motor count to encoder count ratio"); break; + case 37: strcpy(errCodeString, "Command, variable, or flag not available in drive"); break; + case 38: strcpy(errCodeString, "Missing parameter separator"); break; + case 39: strcpy(errCodeString, "Trip on position and trip on relative distance not allowed together"); break; + case 40: strcpy(errCodeString, "Program not running"); break; + case 41: strcpy(errCodeString, "Stack overflow"); break; + case 42: strcpy(errCodeString, "Illegal program address"); break; + case 43: strcpy(errCodeString, "Tried to overflow program stack"); break; + case 44: strcpy(errCodeString, "Program locked"); break; + case 45: strcpy(errCodeString, "Trying to overflow program space"); break; + case 46: strcpy(errCodeString, "Not in program mode"); break; + case 47: strcpy(errCodeString, "Tried to write in illegal flash address"); break; + case 48: strcpy(errCodeString, "Program execution stopped by I/O set as stop"); break; + case 61: strcpy(errCodeString, "Trying to set illegal baud rate"); break; + case 62: strcpy(errCodeString, "IV already pending or IF flag already true"); break; + case 63: strcpy(errCodeString, "Character over-run"); break; + case 64: strcpy(errCodeString, "Startup calibration failed"); break; + case 70: strcpy(errCodeString, "Flash check sum failed"); break; + case 71: strcpy(errCodeString, "Internal temperature warning, 10 C to shutdown"); break; + case 72: strcpy(errCodeString, "Internal over temp fault, disabling drive"); break; + case 73: strcpy(errCodeString, "Tried to save while moving"); break; + case 74: strcpy(errCodeString, "Tried to initialize parameters or clear program while moving"); break; + case 75: strcpy(errCodeString, "Linear over temperature error"); break; + case 80: strcpy(errCodeString, "Home switch not defined"); break; + case 81: strcpy(errCodeString, "Home type not defined"); break; + case 82: strcpy(errCodeString, "Went to both limits and did not find home"); break; + case 83: strcpy(errCodeString, "Reached plus limit switch"); break; + case 84: strcpy(errCodeString, "Reached minus limit switch"); break; + case 85: strcpy(errCodeString, "MA or MR isn't allowed during home and home isn't allowed while device is in motion"); break; + case 86: strcpy(errCodeString, "Stall detected"); break; + case 87: strcpy(errCodeString, "In clock mode"); break; + case 88: strcpy(errCodeString, "Following error"); break; + case 90: strcpy(errCodeString, "Motion variables are too low switching to EE=1"); break; + case 91: strcpy(errCodeString, "Motion stopped by I/O set as stop"); break; + case 92: strcpy(errCodeString, "Position error in closed loop"); break; + case 93: strcpy(errCodeString, "MR or MA not allowed while correcting position"); break; + } + + asynPrint(pController->pasynUserSelf, ASYN_TRACE_ERROR, "%s:%s: %s, %d:'%s'\n", DRIVER_NAME, functionName, errMsg, errCode, errCodeString); +} diff --git a/motorApp/ImsSrc/ImsMDrivePlusMotorAxis.h b/motorApp/ImsSrc/ImsMDrivePlusMotorAxis.h new file mode 100644 index 00000000..f992a3f4 --- /dev/null +++ b/motorApp/ImsSrc/ImsMDrivePlusMotorAxis.h @@ -0,0 +1,71 @@ +// Description : This is the "model 3" asyn motor driver for IMS MDrivePlus. +// Based on HytecMotorDriver.h. + +#ifndef ImsMDrivePlusMotorAxis_H +#define ImsMDrivePlusMotorAxis_H + +#include + +#include "asynMotorController.h" +#include "asynMotorAxis.h" + +#define DRIVER_NAME "ImsMDrivePlusMotorDriver" + +#define NUM_AXES 1 +#define DEFAULT_NUM_CARDS 32 +#define MAX_MESSAGES 100 +#define IMS_TIMEOUT 2 +#define MAX_BUFF_LEN 80 +#define MAX_CMD_LEN MAX_BUFF_LEN-10 // leave room for line feeds surrounding command +#define MAX_NAME_LEN 10 +#define LOCAL_LINE_LEN 256 + +class ImsMDrivePlusMotorController; + +//////////////////////////////////// +// ImsMDrivePlusMotorAxis class +// derived from asynMotorAxis class +//////////////////////////////////// +class ImsMDrivePlusMotorAxis : public asynMotorAxis +{ +public: + /////////////////////////////////// + // Override asynMotorAxis functions + /////////////////////////////////// + ImsMDrivePlusMotorAxis(ImsMDrivePlusMotorController *pC, int axis); + 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); + + //////////////////////////////////////////////////// + // IMS MDrivePlus specific functions + //////////////////////////////////////////////////// + asynStatus saveToNVM(); +protected: + + +private: + ImsMDrivePlusMotorController *pController; + + //int useEncoder; //! using encoder flag + // FIXME handle lost position in driver or ioc?? +// epicsTime idleTimeStart; //! timer used to track idle time for saving to NVM +// double prevPosition; //! previous position used to see if motor has moved and need to update position in NVM in case of power failure +// int prevMovingState; //! saves previous moving value + + //////////////////////////////////////////////////// + // IMS MDrivePlus specific functions + //////////////////////////////////////////////////// + asynStatus configAxis(); + asynStatus setAxisMoveParameters(double min_velocity, double max_velocity, double acceleration); + void handleAxisError(char *errMsg); + +friend class ImsMDrivePlusMotorController; +}; + +#endif // ImsMDrivePlusMotorAxis_H + + diff --git a/motorApp/ImsSrc/ImsMDrivePlusMotorController.cpp b/motorApp/ImsSrc/ImsMDrivePlusMotorController.cpp new file mode 100644 index 00000000..02ac536f --- /dev/null +++ b/motorApp/ImsSrc/ImsMDrivePlusMotorController.cpp @@ -0,0 +1,356 @@ +//! @File : ImsMDrivePlusController.cpp +//! Motor record driver level support for Intelligent Motion Systems, Inc. +//! MDrivePlus series; M17, M23, M34. +//! Simple implementation using "model 3" asynMotorController and asynMotorAxis base classes (derived from asynPortDriver) +//! +//! Author : Nia Fong +//! Date : 11-21-2011 +//! +//! Assumptions : +//! 1) Like all controllers, the MDrivePlus must be powered-on when EPICS is first booted up. +//! 2) Assume single controller-card-axis relationship; 1 controller = 1 axis. +//! 3) Append Line Feed (ctrl-J) to end of command string for party mode support +//! 4) If not using device name to address device, config ImsMDrivePlusCreateController() with empty string "" for deviceName +// +// Revision History +// ---------------- +// 11-21-2011 NF Initial version + + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ImsMDrivePlusMotorController.h" + +//////////////////////////////////////////////////////// +//! @ImsMDrivePlusMotorController() +//! Constructor +//! Driver assumes only 1 axis configured per controller for now... +//! +//! @param[in] motorPortName Name assigned to the port created to communicate with the motor +//! @param[in] IOPortName Name assigned to the asyn IO port, name that was assigned in drvAsynIPPortConfigure() +//! @param[in] devName Name of device (DN) assigned to motor axis in MCode, the device name is prepended to the MCode command to support Party Mode (PY) multidrop communication setup +//! set to empty string "" if no device name needed/not using Party Mode +//! @param[in] movingPollPeriod Moving polling period in milliseconds +//! @param[in] idlePollPeriod Idle polling period in milliseconds +//////////////////////////////////////////////////////// +ImsMDrivePlusMotorController::ImsMDrivePlusMotorController(const char *motorPortName, const char *IOPortName, const char *devName, double movingPollPeriod, double idlePollPeriod) + : asynMotorController(motorPortName, NUM_AXES, NUM_IMS_PARAMS, + asynInt32Mask | asynFloat64Mask | asynUInt32DigitalMask, + asynInt32Mask | asynFloat64Mask | asynUInt32DigitalMask, + ASYN_CANBLOCK | ASYN_MULTIDEVICE, + 1, // autoconnect + 0, 0), // Default priority and stack size + pAsynUserIMS(0) +{ + static const char *functionName = "ImsMDrivePlusMotorController()"; + asynStatus status; + ImsMDrivePlusMotorAxis *pAxis; + // asynMotorController constructor calloc's memory for array of axis pointers + pAxes_ = (ImsMDrivePlusMotorAxis **)(asynMotorController::pAxes_); + + // copy names + strcpy(motorName, motorPortName); + + // setup communication + status = pasynOctetSyncIO->connect(IOPortName, 0, &pAsynUserIMS, NULL); + if (status != asynSuccess) { + printf("\n\n%s:%s: ERROR connecting to Controller's IO port=%s\n\n", DRIVER_NAME, functionName, IOPortName); + // TODO would be good to implement exceptions + // TODO THROW_(SmarActMCSException(MCSConnectionError, "SmarActMCSController: unable to connect serial channel")); + } + + // write version, cannot use asynPrint() in constructor since controller (motorPortName) hasn't been created yet + printf("%s:%s: motorPortName=%s, IOPortName=%s, devName=%s \n", DRIVER_NAME, functionName, motorPortName, IOPortName, devName); + + + // init + pasynOctetSyncIO->setInputEos(pAsynUserIMS, "\n", 1); + pasynOctetSyncIO->setOutputEos(pAsynUserIMS, "\r\n", 2); + + // Create controller-specific parameters + createParam(ImsMDrivePlusSaveToNVMControlString, asynParamInt32, &ImsMDrivePlusSaveToNVM_); + createParam(ImsMDrivePlusLoadMCodeControlString, asynParamOctet, &this->ImsMDrivePlusLoadMCode_); + createParam(ImsMDrivePlusClearMCodeControlString, asynParamOctet, &this->ImsMDrivePlusClearMCode_); + + // Check the validity of the arguments and init controller object + initController(devName, movingPollPeriod, idlePollPeriod); + + // Create axis + // Assuming single axis per controller the way drvAsynIPPortConfigure( "M06", "ts-b34-nw08:2101", 0, 0 0 ) is called in st.cmd script + pAxis = new ImsMDrivePlusMotorAxis(this, 0); + pAxis = NULL; // asynMotorController constructor tracking array of axis pointers + + // read home and limit config from S1-S4 + readHomeAndLimitConfig(); + + startPoller(movingPollPeriod, idlePollPeriod, 2); +} + +//////////////////////////////////////// +//! initController() +//! config controller variables - axis, etc. +//! only support single axis per controller +// +//! @param[in] devName Name of device (DN) used to identify axis within controller for Party Mode +//! @param[in] movingPollPeriod Moving polling period in milliseconds +//! @param[in] idlePollPeriod Idle polling period in milliseconds +//////////////////////////////////////// +void ImsMDrivePlusMotorController::initController(const char *devName, double movingPollPeriod, double idlePollPeriod) +{ + strcpy(this->deviceName, devName); + + // initialize asynMotorController variables + this->numAxes_ = NUM_AXES; // only support single axis + this->movingPollPeriod_ = movingPollPeriod; + this->idlePollPeriod_ = idlePollPeriod; + + // initialize switch inputs + this->homeSwitchInput=-1; + this->posLimitSwitchInput=-1; + this->negLimitSwitchInput=-1; + + // flush io buffer + pasynOctetSyncIO->flush(pAsynUserIMS); +} + +//////////////////////////////////////// +//! readHomeAndLimitConfig +//! read home, positive limit, and neg limit switch configuration from MCode S1-S4 settings +//! S1-S4 must be set up beforehand +//! I1-I4 are used to read the status of S1-S4 +// Use logic from existing drvMDrive.cc +//////////////////////////////////////// +int ImsMDrivePlusMotorController::readHomeAndLimitConfig() +{ + asynStatus status = asynError; + char cmd[MAX_CMD_LEN]; + char resp[MAX_BUFF_LEN]; + size_t nread; + static const char *functionName = "readHomeAndLimitConfig()"; + int type; + + // iterate through S1-S4 and parse each configuration to see if home, pos, and neg limits are set + for (int i=1; i<=4; i++) { + sprintf(cmd, "PR S%d", i); // query S1-S4 setting + status = this->writeReadController(cmd, resp, sizeof(resp), &nread, IMS_TIMEOUT); + sscanf(resp, "%d", &type); + //asynPrint(pasynUserSelf, ASYN_TRACEIO_DRIVER, "%s:%s: S%d: type=%d\n", DRIVER_NAME, functionName, i, type); + if (type != 0) + //printf("%s:%s: S%d: type=%d\n", DRIVER_NAME, functionName, i, type); + switch (type) { + case 0: break; // general purpose input + case 1: // home switch input + homeSwitchInput = i; break; + case 2: // positive limit switch input + posLimitSwitchInput = i; break; + case 3: // negative limit switch input + negLimitSwitchInput = i; break; + default: + printf("%s:%s: ERROR invalid data type for S%d=%d\n", DRIVER_NAME, functionName, i, type); + } + } + + printf("homeSwitchInput=%d, posLimitSwitchInput=%d, negLimitSwitchInput=%d\n", homeSwitchInput, posLimitSwitchInput, negLimitSwitchInput); + + return status; +} + +//////////////////////////////////////// +//! getAxis() +//! Override asynMotorController function to return pointer to IMS axis object +// +//! Returns a pointer to an ImsMDrivePlusAxis object. +//! Returns NULL if the axis number encoded in pasynUser is invalid +// +//! param[in] pasynUser asynUser structure that encodes the axis index number +//////////////////////////////////////// +ImsMDrivePlusMotorAxis* ImsMDrivePlusMotorController::getAxis(asynUser *pasynUser) +{ + int axisNo; + + getAddress(pasynUser, &axisNo); + return getAxis(axisNo); +} + +//////////////////////////////////////// +//! getAxis() +//! Override asynMotorController function to return pointer to IMS axis object +// +//! Returns a pointer to an ImsMDrivePlusAxis object. +//! Returns NULL if the axis number is invalid. +// +//! param[in] axisNo Axis index number +//////////////////////////////////////// +ImsMDrivePlusMotorAxis* ImsMDrivePlusMotorController::getAxis(int axisNo) +{ + if ((axisNo < 0) || (axisNo >= numAxes_)) return NULL; + return pAxes_[axisNo]; +} + +//////////////////////////////////////// +//! writeInt32() +//! Override asynMotorController function to add hooks to IMS records +// Based on XPSController.cpp +// +//! param[in] pointer to asynUser object +//! param[in] value to pass to function +//////////////////////////////////////// +asynStatus ImsMDrivePlusMotorController::writeInt32(asynUser *pasynUser, epicsInt32 value) +{ + int reason = pasynUser->reason; + int status = asynSuccess; + ImsMDrivePlusMotorAxis *pAxis; + static const char *functionName = "writeInt32"; + + pAxis = this->getAxis(pasynUser); + if (!pAxis) return asynError; + + asynPrint(pasynUserSelf, ASYN_TRACEIO_DRIVER, "%s:%s: function=%s, val=%d\n", DRIVER_NAME, functionName, value); + + // Set the parameter and readback in the parameter library. This may be overwritten when we read back the + // status at the end, but that's OK + status = pAxis->setIntegerParam(reason, value); + + if (reason == ImsMDrivePlusSaveToNVM_) { + if (value == 1) { // save current user parameters to NVM + status = pAxis->saveToNVM(); + if (status) asynPrint(pasynUserSelf, ASYN_TRACE_ERROR, "%s:%s: ERROR saving to NVM\n", DRIVER_NAME, functionName); + else asynPrint(pasynUserSelf, ASYN_TRACE_FLOW, "%s:%s: Successfully saved to NVM\n", DRIVER_NAME, functionName); + } else { + asynPrint(pasynUserSelf, ASYN_TRACE_ERROR, "%s:%s: ERROR, value of 1 to save to NVM\n", DRIVER_NAME, functionName); + } + } else { // call base class method to continue handling + status = asynMotorController::writeInt32(pasynUser, value); + } + + callParamCallbacks(pAxis->axisNo_); + return (asynStatus)status; +} + +//////////////////////////////////////// +//! writeController() +//! reference ACRMotorDriver +// +//! Writes a string to the IMS controller. +//! Prepends deviceName to command string, if party mode not enabled, set device name to "" +//! @param[in] output the string to be written. +//! @param[in] timeout Timeout before returning an error. +//////////////////////////////////////// +asynStatus ImsMDrivePlusMotorController::writeController(const char *output, double timeout) +{ + size_t nwrite; + asynStatus status; + char outbuff[MAX_BUFF_LEN]; + static const char *functionName = "writeController()"; + + // in party-mode Line Feed must follow command string + sprintf(outbuff, "%s%s", deviceName, output); + asynPrint(pasynUserSelf, ASYN_TRACEIO_DRIVER, "%s:%s: deviceName=%s, command=%s\n", DRIVER_NAME, functionName, deviceName, outbuff); + status = pasynOctetSyncIO->write(pAsynUserIMS, outbuff, strlen(outbuff), timeout, &nwrite); + if (status) { // update comm flag + setIntegerParam(this->motorStatusCommsError_, 1); + } + return status ; +} + +//////////////////////////////////////// +//! writeReadController() +//! reference ACRMotorDriver +// +//! Writes a string to the IMS controller and reads a response. +//! Prepends deviceName to command string, if party mode not enabled, set device name to "" +//! param[in] output Pointer to the output string. +//! param[out] input Pointer to the input string location. +//! param[in] maxChars Size of the input buffer. +//! param[out] nread Number of characters read. +//! param[out] timeout Timeout before returning an error.*/ +//////////////////////////////////////// +asynStatus ImsMDrivePlusMotorController::writeReadController(const char *output, char *input, size_t maxChars, size_t *nread, double timeout) +{ + size_t nwrite; + asynStatus status; + int eomReason; + char outbuff[MAX_BUFF_LEN]; + static const char *functionName = "writeReadController()"; + + // in party-mode Line Feed must follow command string + sprintf(outbuff, "%s%s", deviceName, output); + status = pasynOctetSyncIO->writeRead(pAsynUserIMS, outbuff, strlen(outbuff), input, maxChars, timeout, &nwrite, nread, &eomReason); + if (status) { // update comm flag + setIntegerParam(this->motorStatusCommsError_, 1); + } + asynPrint(pasynUserSelf, ASYN_TRACEIO_DRIVER, "%s:%s: deviceName=%s, command=%s, response=%s\n", DRIVER_NAME, functionName, deviceName, outbuff, input); + return status; +} + +//////////////////////////////////////////////////////// +// Start code for iocsh Registration : +// Available Functions : +// ImsMDrivePlusCreateController() +//////////////////////////////////////////////////////// + +//////////////////////////////////////////////////////// +//! ImsMDrivePlusCreateController() +//! IOCSH function +//! Creates a new IMSMDrivePlusController object +// +//! Configuration command, called directly or from iocsh +//! @param[in] motorPortName User-specific name of motor port +//! @param[in] IOPortName User-specific name of port that was configured by drvAsynIPPortConfigure() +//! @param[in] deviceName Name of device, used to address motor by MCODE in party mode +// If not using party mode, config ImsMDrivePlusCreateController() with empty string "" for deviceName +//! @param[in] movingPollPeriod time in ms between polls when any axis is moving +//! @param[in] idlePollPeriod time in ms between polls when no axis is moving +//////////////////////////////////////////////////////// +extern "C" int ImsMDrivePlusCreateController(const char *motorPortName, const char *IOPortName, char *devName, double movingPollPeriod, double idlePollPeriod) +{ + ImsMDrivePlusMotorController *pImsController; + pImsController = new ImsMDrivePlusMotorController(motorPortName, IOPortName, devName, movingPollPeriod/1000., idlePollPeriod/1000.); + pImsController = NULL; + return(asynSuccess); +} + +//////////////////////////////////////////////////////// +// ImsMDrivePlus IOCSH Registration +// Copied from ACRMotorDriver.cpp +// +// Motor port name : user-specified name of port +// IO port name : user-specific name of port that was initialized with drvAsynIPPortConfigure() +// Device name : name of device, used to address motor by MCODE in party mode +// : if not using party mode, config ImsMDrivePlusCreateController() with empty string "" for deviceName +// Moving poll period : time in ms between polls when any axis is moving +// Idle poll period : time in ms between polls when no axis is moving +//////////////////////////////////////////////////////// +static const iocshArg ImsMDrivePlusCreateControllerArg0 = {"Motor port name", iocshArgString}; +static const iocshArg ImsMDrivePlusCreateControllerArg1 = {"IO port name", iocshArgString}; +static const iocshArg ImsMDrivePlusCreateControllerArg2 = {"Device name", iocshArgString}; +static const iocshArg ImsMDrivePlusCreateControllerArg3 = {"Moving poll period (ms)", iocshArgDouble}; +static const iocshArg ImsMDrivePlusCreateControllerArg4 = {"Idle poll period (ms)", iocshArgDouble}; +static const iocshArg * const ImsMDrivePlusCreateControllerArgs[] = {&ImsMDrivePlusCreateControllerArg0, + &ImsMDrivePlusCreateControllerArg1, + &ImsMDrivePlusCreateControllerArg2, + &ImsMDrivePlusCreateControllerArg3, + &ImsMDrivePlusCreateControllerArg4}; +static const iocshFuncDef ImsMDrivePlusCreateControllerDef = {"ImsMDrivePlusCreateController", 5, ImsMDrivePlusCreateControllerArgs}; +static void ImsMDrivePlusCreateControllerCallFunc(const iocshArgBuf *args) +{ + ImsMDrivePlusCreateController(args[0].sval, args[1].sval, args[2].sval, args[3].dval, args[4].dval); +} + +static void ImsMDrivePlusMotorRegister(void) +{ + iocshRegister(&ImsMDrivePlusCreateControllerDef, ImsMDrivePlusCreateControllerCallFunc); +} + +extern "C" { + epicsExportRegistrar(ImsMDrivePlusMotorRegister); +} diff --git a/motorApp/ImsSrc/ImsMDrivePlusMotorController.h b/motorApp/ImsSrc/ImsMDrivePlusMotorController.h new file mode 100644 index 00000000..f7e10b08 --- /dev/null +++ b/motorApp/ImsSrc/ImsMDrivePlusMotorController.h @@ -0,0 +1,74 @@ +//! Description : This is the "model 3" asyn motor driver for IMS MDrivePlus. +//! Based on HytecMotorDriver.h. +//! @ImsMDrivePlus.h + +#ifndef ImsMDrivePlusMotorController_H +#define ImsMDrivePlusMotorController_H + +#include "asynMotorController.h" +#include "asynMotorAxis.h" +#include "ImsMDrivePlusMotorAxis.h" + +//////////////////////////////////// +// ImsMDrivePlusMotorController class +//! derived from asynMotorController class +//////////////////////////////////// +class ImsMDrivePlusMotorController : public asynMotorController +{ +public: + ///////////////////////////////////////// + // Override asynMotorController functions + ///////////////////////////////////////// + ImsMDrivePlusMotorController(const char *motorPortName, const char *IOPortName, const char *deviceName, double movingPollPeriod, double idlePollPeriod); + ImsMDrivePlusMotorAxis* getAxis(asynUser *pasynUser); + ImsMDrivePlusMotorAxis* getAxis(int axisNo); + //void report(FILE *fp, int level); + asynStatus writeInt32(asynUser *pasynUser, epicsInt32 value); + + ///////////////////////////////////////// + // IMS specific functions + ///////////////////////////////////////// + asynStatus writeReadController(const char *output, char *input, size_t maxChars, size_t *nread, double timeout); + asynStatus writeController(const char *output, double timeout); + + + +protected: + ImsMDrivePlusMotorAxis **pAxes_; // Array of pointers to axis objects + + /////////////////////////////////////////// + // IMS MDrivePlus controller function codes + /////////////////////////////////////////// + + //! extra parameters that the ImsMDrivePlus controller supports + int ImsMDrivePlusLoadMCode_; //! Load MCode string, NOT SUPPORTED YET + int ImsMDrivePlusClearMCode_; //! Clear program buffer, NOT SUPPORTED YET + int ImsMDrivePlusSaveToNVM_; //! Store current user variables and flags to nonvolatile ram +#define FIRST_IMS_PARAM ImsMDrivePlusLoadMCode_ +#define LAST_IMS_PARAM ImsMDrivePlusSaveToNVM_ +#define NUM_IMS_PARAMS (&LAST_IMS_PARAM - &FIRST_IMS_PARAM + 1) + +private: +// drvInfo strings for extra parameters that the ImsMDrivePlus controller supports +#define ImsMDrivePlusLoadMCodeControlString "IMS_LOADMCODE" // NOT SUPPORTED YET +#define ImsMDrivePlusClearMCodeControlString "IMS_CLEARMCODE" // NOT SUPPORTED YET +#define ImsMDrivePlusSaveToNVMControlString "IMS_SAVETONVM" + + asynUser *pAsynUserIMS; + char motorName[MAX_NAME_LEN]; + char deviceName[MAX_NAME_LEN]; + int homeSwitchInput; + int posLimitSwitchInput; + int negLimitSwitchInput; + + void initController(const char *devName, double movingPollPeriod, double idlePollPeriod); + int readHomeAndLimitConfig(); // read home, positive limit, and neg limit switch configuration from controller (S1-S4 settings) + + friend class ImsMDrivePlusMotorAxis; +}; +//! iocsh function to create controller object +//! NOTE: drvAsynIPPortConfigure() must be called first +//extern "C" int ImsMDrivePlusCreateController(const char *motorPortName, const char *IOPortName, char *devName, double movingPollPeriod, double idlePollPeriod); + +#endif // ImsMDrivePlusMotorController_H +