lin-epics-modules/motorBase
0
0
Fork 0
forked from epics_driver_modules/motorBase
Code Pull Requests Activity

New file from Nia Fong at SLAC

Browse Source
This commit is contained in:
MarkRivers
2012-09-12 17:16:54 +00:00
parent 3087c9fb50
commit 53a98f2849
4 changed files with 1092 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
motorApp/ImsSrc/ImsMDrivePlusMotorAxis.cpp
+591
View File
@@ -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 <stddef.h>
#include <stdlib.h>
#include <stdarg.h>
#include <stdio.h>
#include <string.h>
#include <exception>
#include <epicsThread.h>
#include <epicsExport.h>
#include <iocsh.h>
#include <asynOctetSyncIO.h>
#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; i<maxRetries; i++) {
status = pController->writeReadController(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);
}
motorApp/ImsSrc/ImsMDrivePlusMotorAxis.h
+71
View File
@@ -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 <epicsTime.h>
#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
motorApp/ImsSrc/ImsMDrivePlusMotorController.cpp
+356
View File
@@ -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 <stddef.h>
#include <stdlib.h>
#include <stdarg.h>
#include <stdio.h>
#include <string.h>
#include <exception>
#include <epicsThread.h>
#include <epicsExport.h>
#include <iocsh.h>
#include <asynOctetSyncIO.h>
#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);
}
motorApp/ImsSrc/ImsMDrivePlusMotorController.h
+74
View File
@@ -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