lin-epics-modules/sinqepicsapp
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
f1a17bc2950fedbec90eeebc63885688e92a1303
sinqepicsapp/sinqEPICSApp/src/pmacController.cpp

1060 lines
38 KiB
C++
Raw Blame History

/********************************************
* pmacController.cpp
*
* PMAC Asyn motor based on the
* asynMotorController class.
*
* Matthew Pearson
* 23 May 2012
*
*
* Substantially modified for use at SINQ at PSI.
* The thing with the PMACS is that they can be programmed.
* This affects also the commands they understand.
*
* Mark Koennecke, February 2013
*
* Updated to use the MsgTxt field for error messages as
* used at ESS and SINQ
*
* Mark Koennecke, January 2019
********************************************/
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <errno.h>
#include <iostream>
using std::cout;
using std::endl;
#include "epicsTime.h"
#include <epicsThread.h>
#include <epicsExport.h>
#include <epicsString.h>
#include <iocsh.h>
#include <drvSup.h>
#include <registryFunction.h>
#include <errlog.h>
#include "asynOctetSyncIO.h"
#include "pmacController.h"
#include "pmacAxis.h"
#define MULT 1000.
static const char *driverName = "pmacController";
const epicsUInt32 pmacController::PMAC_MAXBUF_ = 1024;
const epicsFloat64 pmacController::PMAC_TIMEOUT_ = 5.0;
const epicsUInt32 pmacController::PMAC_STATUS1_MAXRAPID_SPEED = (0x1<<0);
const epicsUInt32 pmacController::PMAC_STATUS1_ALT_CMNDOUT_MODE = (0x1<<1);
const epicsUInt32 pmacController::PMAC_STATUS1_SOFT_POS_CAPTURE = (0x1<<2);
const epicsUInt32 pmacController::PMAC_STATUS1_ERROR_TRIGGER = (0x1<<3);
const epicsUInt32 pmacController::PMAC_STATUS1_FOLLOW_ENABLE = (0x1<<4);
const epicsUInt32 pmacController::PMAC_STATUS1_FOLLOW_OFFSET = (0x1<<5);
const epicsUInt32 pmacController::PMAC_STATUS1_PHASED_MOTOR = (0x1<<6);
const epicsUInt32 pmacController::PMAC_STATUS1_ALT_SRC_DEST = (0x1<<7);
const epicsUInt32 pmacController::PMAC_STATUS1_USER_SERVO = (0x1<<8);
const epicsUInt32 pmacController::PMAC_STATUS1_USER_PHASE = (0x1<<9);
const epicsUInt32 pmacController::PMAC_STATUS1_HOMING = (0x1<<10);
const epicsUInt32 pmacController::PMAC_STATUS1_BLOCK_REQUEST = (0x1<<11);
const epicsUInt32 pmacController::PMAC_STATUS1_DECEL_ABORT = (0x1<<12);
const epicsUInt32 pmacController::PMAC_STATUS1_DESIRED_VELOCITY_ZERO = (0x1<<13);
const epicsUInt32 pmacController::PMAC_STATUS1_DATABLKERR = (0x1<<14);
const epicsUInt32 pmacController::PMAC_STATUS1_DWELL = (0x1<<15);
const epicsUInt32 pmacController::PMAC_STATUS1_INTEGRATE_MODE = (0x1<<16);
const epicsUInt32 pmacController::PMAC_STATUS1_MOVE_TIME_ON = (0x1<<17);
const epicsUInt32 pmacController::PMAC_STATUS1_OPEN_LOOP = (0x1<<18);
const epicsUInt32 pmacController::PMAC_STATUS1_AMP_ENABLED = (0x1<<19);
const epicsUInt32 pmacController::PMAC_STATUS1_X_SERVO_ON = (0x1<<20);
const epicsUInt32 pmacController::PMAC_STATUS1_POS_LIMIT_SET = (0x1<<21);
const epicsUInt32 pmacController::PMAC_STATUS1_NEG_LIMIT_SET = (0x1<<22);
const epicsUInt32 pmacController::PMAC_STATUS1_MOTOR_ON = (0x1<<23);
const epicsUInt32 pmacController::PMAC_STATUS2_IN_POSITION = (0x1<<0);
const epicsUInt32 pmacController::PMAC_STATUS2_WARN_FOLLOW_ERR = (0x1<<1);
const epicsUInt32 pmacController::PMAC_STATUS2_ERR_FOLLOW_ERR = (0x1<<2);
const epicsUInt32 pmacController::PMAC_STATUS2_AMP_FAULT = (0x1<<3);
const epicsUInt32 pmacController::PMAC_STATUS2_NEG_BACKLASH = (0x1<<4);
const epicsUInt32 pmacController::PMAC_STATUS2_I2T_AMP_FAULT = (0x1<<5);
const epicsUInt32 pmacController::PMAC_STATUS2_I2_FOLLOW_ERR = (0x1<<6);
const epicsUInt32 pmacController::PMAC_STATUS2_TRIGGER_MOVE = (0x1<<7);
const epicsUInt32 pmacController::PMAC_STATUS2_PHASE_REF_ERR = (0x1<<8);
const epicsUInt32 pmacController::PMAC_STATUS2_PHASE_SEARCH = (0x1<<9);
const epicsUInt32 pmacController::PMAC_STATUS2_HOME_COMPLETE = (0x1<<10);
const epicsUInt32 pmacController::PMAC_STATUS2_POS_LIMIT_STOP = (0x1<<11);
const epicsUInt32 pmacController::PMAC_STATUS2_DESIRED_STOP = (0x1<<12);
const epicsUInt32 pmacController::PMAC_STATUS2_FORE_IN_POS = (0x1<<13);
const epicsUInt32 pmacController::PMAC_STATUS2_NA14 = (0x1<<14);
const epicsUInt32 pmacController::PMAC_STATUS2_ASSIGNED_CS = (0x1<<15);
/*Global status ???*/
const epicsUInt32 pmacController::PMAC_GSTATUS_CARD_ADDR = (0x1<<0);
const epicsUInt32 pmacController::PMAC_GSTATUS_ALL_CARD_ADDR = (0x1<<1);
const epicsUInt32 pmacController::PMAC_GSTATUS_RESERVED = (0x1<<2);
const epicsUInt32 pmacController::PMAC_GSTATUS_PHASE_CLK_MISS = (0x1<<3);
const epicsUInt32 pmacController::PMAC_GSTATUS_MACRO_RING_ERRORCHECK = (0x1<<4);
const epicsUInt32 pmacController::PMAC_GSTATUS_MACRO_RING_COMMS = (0x1<<5);
const epicsUInt32 pmacController::PMAC_GSTATUS_TWS_PARITY_ERROR = (0x1<<6);
const epicsUInt32 pmacController::PMAC_GSTATUS_CONFIG_ERROR = (0x1<<7);
const epicsUInt32 pmacController::PMAC_GSTATUS_ILLEGAL_LVAR = (0x1<<8);
const epicsUInt32 pmacController::PMAC_GSTATUS_REALTIME_INTR = (0x1<<9);
const epicsUInt32 pmacController::PMAC_GSTATUS_FLASH_ERROR = (0x1<<10);
const epicsUInt32 pmacController::PMAC_GSTATUS_DPRAM_ERROR = (0x1<<11);
const epicsUInt32 pmacController::PMAC_GSTATUS_CKSUM_ACTIVE = (0x1<<12);
const epicsUInt32 pmacController::PMAC_GSTATUS_CKSUM_ERROR = (0x1<<13);
const epicsUInt32 pmacController::PMAC_GSTATUS_LEADSCREW_COMP = (0x1<<14);
const epicsUInt32 pmacController::PMAC_GSTATUS_WATCHDOG = (0x1<<15);
const epicsUInt32 pmacController::PMAC_GSTATUS_SERVO_REQ = (0x1<<16);
const epicsUInt32 pmacController::PMAC_GSTATUS_DATA_GATHER_START = (0x1<<17);
const epicsUInt32 pmacController::PMAC_GSTATUS_RESERVED2 = (0x1<<18);
const epicsUInt32 pmacController::PMAC_GSTATUS_DATA_GATHER_ON = (0x1<<19);
const epicsUInt32 pmacController::PMAC_GSTATUS_SERVO_ERROR = (0x1<<20);
const epicsUInt32 pmacController::PMAC_GSTATUS_CPUTYPE = (0x1<<21);
const epicsUInt32 pmacController::PMAC_GSTATUS_REALTIME_INTR_RE = (0x1<<22);
const epicsUInt32 pmacController::PMAC_GSTATUS_RESERVED3 = (0x1<<23);
const epicsUInt32 pmacController::PMAC_HARDWARE_PROB = (PMAC_GSTATUS_MACRO_RING_ERRORCHECK | PMAC_GSTATUS_MACRO_RING_COMMS | PMAC_GSTATUS_REALTIME_INTR | PMAC_GSTATUS_FLASH_ERROR | PMAC_GSTATUS_DPRAM_ERROR | PMAC_GSTATUS_CKSUM_ERROR | PMAC_GSTATUS_WATCHDOG | PMAC_GSTATUS_SERVO_ERROR);
const epicsUInt32 pmacController::PMAX_AXIS_GENERAL_PROB1 = 0;
const epicsUInt32 pmacController::PMAX_AXIS_GENERAL_PROB2 = (PMAC_STATUS2_DESIRED_STOP | PMAC_STATUS2_AMP_FAULT);
//C function prototypes, for the functions that can be called on IOC shell
extern "C" {
asynStatus pmacCreateController(const char *portName, const char *lowLevelPortName, int lowLevelPortAddress,
int numAxes, int movingPollPeriod, int idlePollPeriod);
asynStatus SeleneCreateController(const char *portName, const char *lowLevelPortName, int lowLevelPortAddress,
int numAxes, int movingPollPeriod, int idlePollPeriod);
asynStatus pmacV3CreateController(const char *portName,
const char *lowLevelPortName,
int lowLevelPortAddress, int numAxes,
int movingPollPeriod, int idlePollPeriod);
asynStatus pmacCreateAxis(const char *pmacName, int axis);
asynStatus pmacCreateAxis(const char *pmacName, int numAxis);
}
pmacController::pmacController(const char *portName, const char *lowLevelPortName, int lowLevelPortAddress,
int numAxes, double movingPollPeriod, double idlePollPeriod, const int& extraParams)
: SINQController(portName, lowLevelPortName, numAxes+1, extraParams)
{
static const char *functionName = "pmacController::pmacController";
//Initialize non static data members
lowLevelPortUser_ = NULL;
debugFlag_ = 0;
pAxes_ = (pmacAxis **)(asynMotorController::pAxes_);
// Create controller-specific parameters
createParam(PMAC_C_CommsErrorString, asynParamInt32, &PMAC_C_CommsError_);
// Connect our Asyn user to the low level port that is a parameter to this constructor
if (lowLevelPortConnect(lowLevelPortName, lowLevelPortAddress, &lowLevelPortUser_, "\006", (char *)"\r") != asynSuccess) {
printf("%s: Failed to connect to low level asynOctetSyncIO port %s\n", functionName, lowLevelPortName);
setIntegerParam(PMAC_C_CommsError_, 1);
} else {
/* Create the poller thread for this controller
* NOTE: at this point the axis objects don't yet exist, but the poller tolerates this */
setIntegerParam(PMAC_C_CommsError_, 0);
}
startPoller(movingPollPeriod, idlePollPeriod, 10);
callParamCallbacks();
}
pmacController::~pmacController(void)
{
//Destructor
}
/**
* Connect to the underlying low level Asyn port that is used for comms.
* This uses the asynOctetSyncIO interface, and also sets the input and output terminators.
*/
int pmacController::lowLevelPortConnect(const char *port, int addr, asynUser **ppasynUser, char *inputEos, char *outputEos)
{
asynStatus status = asynSuccess;
static const char *functionName = "pmacController::lowLevelPortConnect";
debugFlow(functionName);
status = pasynOctetSyncIO->connect( port, addr, ppasynUser, NULL);
if (status) {
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
"pmacController::motorAxisAsynConnect: unable to connect to port %s\n",
port);
return status;
}
//Do I want to disconnect below? If the IP address comes up, will the driver recover
//if the poller functions are running? Might have to use asynManager->isConnected to
//test connection status of low level port (in the pollers). But then autosave
//restore doesn't work (and we would save wrong positions). So I need to
//have a seperate function(s) to deal with connecting after IOC init.
status = pasynOctetSyncIO->setInputEos(*ppasynUser, inputEos, strlen(inputEos) );
if (status) {
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
"pmacController: unable to set input EOS on %s: %s\n",
port, (*ppasynUser)->errorMessage);
pasynOctetSyncIO->disconnect(*ppasynUser);
//Set my low level pasynUser pointer to NULL
*ppasynUser = NULL;
return status;
}
status = pasynOctetSyncIO->setOutputEos(*ppasynUser, outputEos, strlen(outputEos));
if (status) {
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
"pmacController: unable to set output EOS on %s: %s\n",
port, (*ppasynUser)->errorMessage);
pasynOctetSyncIO->disconnect(*ppasynUser);
//Set my low level pasynUser pointer to NULL
*ppasynUser = NULL;
return status;
}
return status;
}
/**
* Utilty function to print the connected status of the low level asyn port.
*/
asynStatus pmacController::printConnectedStatus()
{
asynStatus status = asynSuccess;
int asynManagerConnected = 0;
if (lowLevelPortUser_) {
status = pasynManager->isConnected(lowLevelPortUser_, &asynManagerConnected);
if (status) {
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
"pmacController: Error calling pasynManager::isConnected.\n");
return asynError;
} else {
printf("pmacController::printConnectedStatus: isConnected: %d\n", asynManagerConnected);
}
}
return asynSuccess;
}
/**
* Wrapper for asynOctetSyncIO write/read functions.
* @param command - String command to send.
* @response response - String response back.
*/
asynStatus pmacController::lowLevelWriteRead(int axisNo, const char *command, char *response)
{
asynStatus status = asynSuccess;
int eomReason;
size_t nwrite = 0;
size_t nread = 0;
int commsError = 0;
static const char *functionName = "pmacController::lowLevelWriteRead";
pmacAxis *axis = getAxis(axisNo);
debugFlow(functionName);
if (!lowLevelPortUser_) {
setIntegerParam(this->motorStatusCommsError_, 1);
return asynError;
}
asynPrint(lowLevelPortUser_, ASYN_TRACEIO_DRIVER, "%s: command: %s\n", functionName, command);
debugFlow("Sending: ");
debugFlow(command);
//Make sure the low level port is connected before we attempt comms
//Use the controller-wide param PMAC_C_CommsError_
getIntegerParam(PMAC_C_CommsError_, &commsError);
if (!commsError) {
status = pasynOctetSyncIO->writeRead(lowLevelPortUser_ ,
command, strlen(command),
response, PMAC_MAXBUF_,
PMAC_TIMEOUT_,
&nwrite, &nread, &eomReason );
if (status) {
asynPrint(lowLevelPortUser_, ASYN_TRACE_ERROR, "%s: Error from pasynOctetSyncIO->writeRead. command: %s\n", functionName, command);
setIntegerParam(this->motorStatusCommsError_, 1);
if(axis!= NULL){
axis->updateMsgTxtFromDriver("Lost connection to motor controller");
}
} else {
setIntegerParam(this->motorStatusCommsError_, 0);
}
}
asynPrint(lowLevelPortUser_, ASYN_TRACEIO_DRIVER, "%s: response: %s\n", functionName, response);
debugFlow("Received: ");
debugFlow(response);
return status;
}
void pmacController::debugFlow(const char *message)
{
if (debugFlag_ == 1) {
printf(" %s\n", message);
}
asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW, "%s\n", message);
}
void pmacController::report(FILE *fp, int level)
{
int axis = 0;
pmacAxis *pAxis = NULL;
fprintf(fp, "pmac motor driver %s, numAxes=%d, moving poll period=%f, idle poll period=%f\n",
this->portName, numAxes_, movingPollPeriod_, idlePollPeriod_);
if (level > 0) {
for (axis=0; axis<numAxes_; axis++) {
pAxis = getAxis(axis);
fprintf(fp, " axis %d\n"
" scale = %lf\n",
pAxis->axisNo_,
pAxis->scale_);
}
}
// Call the base class method
asynMotorController::report(fp, level);
}
asynStatus pmacController::writeFloat64(asynUser *pasynUser, epicsFloat64 value)
{
int function = pasynUser->reason;
asynStatus status = asynError;
pmacAxis *pAxis = NULL;
char command[64] = {0};
char response[64] = {0};
char message[132];
static const char *functionName = "pmacController::writeFloat64";
sprintf(message,"%s, reason %d", functionName, function);
debugFlow(message);
//debugFlow(functionName);
pAxis = this->getAxis(pasynUser);
if (!pAxis) {
return asynError;
}
/* Set the parameter and readback in the parameter library. */
status = pAxis->setDoubleParam(function, value);
// if (function == motorPosition_) {
// /*Set position on motor axis.*/
// epicsInt32 position = (epicsInt32) floor(value*32/pAxis->scale_ + 0.5);
// sprintf(command, "#%dK M%d61=%d*I%d08 M%d62=%d*I%d08",
// pAxis->axisNo_,
// pAxis->axisNo_, position, pAxis->axisNo_,
// pAxis->axisNo_, position, pAxis->axisNo_ );
// asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW,
// "%s: Set axis %d on controller %s to position %f\n",
// functionName, pAxis->axisNo_, portName, value);
// if ( command[0] != 0 && status == asynSuccess) {
// status = lowLevelWriteRead(command, response);
// }
// sprintf(command, "#%dJ/", pAxis->axisNo_);
// if (command[0] != 0 && status == asynSuccess) {
// status = lowLevelWriteRead(command, response);
// }
// /*Now set position on encoder axis, if one is in use.*/
// if (pAxis->encoder_axis_) {
// getDoubleParam(motorEncRatio_, &encRatio);
// encposition = (epicsInt32) floor((position*encRatio) + 0.5);
// sprintf(command, "#%dK M%d61=%d*I%d08 M%d62=%d*I%d08",
// pAxis->encoder_axis_,
// pAxis->encoder_axis_, encposition, pAxis->encoder_axis_,
// pAxis->encoder_axis_, encposition, pAxis->encoder_axis_ );
// asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW,
// "%s: Set encoder axis %d on controller %s to position %f\n",
// functionName, pAxis->axisNo_, portName, value);
// if (command[0] != 0 && status == asynSuccess) {
// status = lowLevelWriteRead(command, response);
// }
// sprintf(command, "#%dJ/", pAxis->encoder_axis_);
// //The lowLevelWriteRead will be done at the end of this function.
// }
// /*Now do an update, to get the new position from the controller.*/
// bool moving = true;
// pAxis->getAxisStatus(&moving);
// } else
/*
if (function == motorLowLimit_) {
sprintf(command, "I%d14=%d", pAxis->axisNo_, (int)(value/pAxis->scale_/MULT));
asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW,
"%s: Setting low limit on controller %s, axis %d to %f\n",
functionName, portName, pAxis->axisNo_, value);
} else if (function == motorHighLimit_) {
sprintf(command, "I%d13=%d", pAxis->axisNo_, (int)(value/pAxis->scale_/MULT));
asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW,
"%s: Setting high limit on controller %s, axis %d to %f\n",
functionName, portName, pAxis->axisNo_, value);
}
//Execute the command.
if (command[0] != 0 && status == asynSuccess) {
status = lowLevelWriteRead(pAxis->axisNo_,command, response);
}
*/
//Call base class method
//This will handle callCallbacks even if the function was handled here.
status = asynMotorController::writeFloat64(pasynUser, value);
return status;
}
asynStatus pmacController::writeInt32(asynUser *pasynUser, epicsInt32 value)
{
int function = pasynUser->reason;
asynStatus status = asynError;
pmacAxis *pAxis = NULL;
static const char *functionName = "pmacController::writeInt32";
debugFlow(functionName);
pAxis = this->getAxis(pasynUser);
if (!pAxis) {
return asynError;
}
/* 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(function, value);
//Call base class method
//This will handle callCallbacks even if the function was handled here.
status = asynMotorController::writeInt32(pasynUser, value);
return status;
}
/** Returns a pointer to an pmacAxis object.
* Returns NULL if the axis number encoded in pasynUser is invalid.
* \param[in] pasynUser asynUser structure that encodes the axis index number. */
pmacAxis* pmacController::getAxis(asynUser *pasynUser)
{
int axisNo = 0;
getAddress(pasynUser, &axisNo);
return getAxis(axisNo);
}
/** Returns a pointer to an pmacAxis object.
* Returns NULL if the axis number is invalid.
* \param[in] axisNo Axis index number. */
pmacAxis* pmacController::getAxis(int axisNo)
{
if ((axisNo < 0) || (axisNo >= numAxes_)) return NULL;
return pAxes_[axisNo];
}
/** Polls the controller, rather than individual axis.*/
asynStatus pmacController::poll()
{
static const char *functionName = "pmacController::poll";
debugFlow(functionName);
if (!lowLevelPortUser_) {
setIntegerParam(this->motorStatusCommsError_, 1);
return asynError;
}
asynMotorController::poll();
callParamCallbacks();
return asynSuccess;
}
/*************************************************************************************/
/** The following functions have C linkage, and can be called directly or from iocsh */
extern "C" {
/**
* C wrapper for the pmacController constructor.
* See pmacController::pmacController.
*
*/
asynStatus pmacCreateController(const char *portName, const char *lowLevelPortName, int lowLevelPortAddress,
int numAxes, int movingPollPeriod, int idlePollPeriod)
{
pmacController *ppmacController
= new pmacController(portName, lowLevelPortName, lowLevelPortAddress, numAxes, movingPollPeriod/1000., idlePollPeriod/1000.);
ppmacController = NULL;
return asynSuccess;
}
asynStatus SeleneCreateController(const char *portName, const char *lowLevelPortName, int lowLevelPortAddress,
int numAxes, int movingPollPeriod, int idlePollPeriod)
{
SeleneController *ppmacController
= new SeleneController(portName, lowLevelPortName, lowLevelPortAddress, numAxes, movingPollPeriod/1000., idlePollPeriod/1000.);
ppmacController = NULL;
return asynSuccess;
}
asynStatus pmacV3CreateController(const char *portName, const char *lowLevelPortName, int lowLevelPortAddress,
int numAxes, int movingPollPeriod, int idlePollPeriod)
{
pmacController *ppmacController = new pmacV3Controller(portName, lowLevelPortName, lowLevelPortAddress, numAxes, movingPollPeriod/1000., idlePollPeriod/1000.);
ppmacController = NULL;
return asynSuccess;
}
/*---------------------------------------------------------------------------------------------------------------------*/
SeleneController::SeleneController(const char *portName, const char *lowLevelPortName, int lowLevelPortAddress,
int numAxes, double movingPollPeriod, double idlePollPeriod) : pmacController(portName,
lowLevelPortName,
lowLevelPortAddress,
numAxes,
movingPollPeriod,
idlePollPeriod, 3) {
static const char *functionName = "seleneController::seleneController";
createParam(MotorSetPositionString, asynParamFloat64, &setMotorPosition_);
callParamCallbacks();
}
pmacV3Controller::pmacV3Controller(const char *portName,
const char *lowLevelPortName,
int lowLevelPortAddress, int numAxes,
double movingPollPeriod,
double idlePollPeriod,
const int &extraParams) :
pmacController(portName, lowLevelPortName, lowLevelPortAddress, numAxes, movingPollPeriod, idlePollPeriod, extraParams) {
static const char *functionName = "pmacV3Controller::pmacV3Controller";
createParam(EnableAxisString, asynParamInt32, &enableAxis_);
createParam(AxisEnabledString, asynParamInt32, &axisEnabled_);
callParamCallbacks();
}
/**
* C wrapper for the pmacAxis constructor.
* See pmacAxis::pmacAxis.
*
*/
asynStatus pmacCreateAxis(
const char *pmacName, /* specify which controller by port name */
int axis) /* axis number (start from 1). */
{
pmacController *pC;
pmacAxis *pAxis;
static const char *functionName = "pmacCreateAxis";
pC = (pmacController *)findAsynPortDriver(pmacName);
if (!pC) {
printf("%s:%s: Error port %s not found\n", driverName, functionName,
pmacName);
return asynError;
}
pC->lock();
pAxis = new pmacAxis(pC, axis);
pAxis = NULL;
pC->unlock();
return asynSuccess;
}
/**
* C wrapper for the pmacHRPTAxis constructor.
* See pmacHRPTAxis::pmacHRPTAxis.
*
*/
asynStatus pmacCreateHRPTAxis(const char *pmacName, /* specify which controller by port name */
int axis) /* axis number (start from 1). */
{
pmacController *pC;
pmacAxis *pAxis;
static const char *functionName = "pmacCreateHRPTAxis";
pC = (pmacController*) findAsynPortDriver(pmacName);
if (!pC) {
printf("%s:%s: Error port %s not found\n",
driverName, functionName, pmacName);
return asynError;
}
pC->lock();
pAxis = new pmacHRPTAxis(pC, axis);
pAxis = NULL;
pC->unlock();
return asynSuccess;
}
/**
* C wrapper for the SeleneAxis constructor.
* See SeleneAxis::SeleneAxis.
*
*/
asynStatus SeleneCreateAxis(const char *pmacName, /* specify which controller by port name */
int axis, /* axis number (start from 1). */
double limitTarget)
{
SeleneController *pC;
SeleneAxis *pAxis;
static const char *functionName = "SeleneCreateAxis";
pC = (SeleneController*) findAsynPortDriver(pmacName);
if (!pC) {
printf("%s:%s: Error port %s not found\n",
driverName, functionName, pmacName);
return asynError;
}
pC->lock();
pAxis = new SeleneAxis(pC, axis, limitTarget);
pAxis = NULL;
pC->unlock();
return asynSuccess;
}
/**
* C wrapper for the Selene LiftAxis constructor.
* See LiftAxis::LiftAxis.
*
*/
asynStatus LiftCreateAxis(const char *pmacName, /* specify which controller by port name */
int axis) /* axis number (start from 1). */
{
pmacController *pC;
LiftAxis *pAxis;
static const char *functionName = "LiftCreateAxis";
pC = (pmacController*) findAsynPortDriver(pmacName);
if (!pC) {
printf("%s:%s: Error port %s not found\n",
driverName, functionName, pmacName);
return asynError;
}
pC->lock();
pAxis = new LiftAxis(pC, axis);
pAxis = NULL;
pC->unlock();
return asynSuccess;
}
/**
* C wrapper for the pmacV3Axis constructor.
* See pmacAxis::pmacV3Axis.
*
*/
asynStatus pmacV3CreateAxis(
const char *pmacName, /* specify which controller by port name */
int axis) /* axis number (start from 1). */
{
pmacController *pC;
pmacAxis *pAxis;
static const char *functionName = "pmacV3CreateAxis";
pC = (pmacController *)findAsynPortDriver(pmacName);
if (!pC) {
printf("%s:%s: Error port %s not found\n", driverName, functionName,
pmacName);
return asynError;
}
pC->lock();
pAxis = new pmacV3Axis(pC, axis);
pAxis = NULL;
pC->unlock();
return asynSuccess;
}
/**
* C Wrapper function for pmacHRPTAxis constructor.
* See pmacAxis::pmacAxis.
* This function allows creation of multiple pmacAxis objects with axis numbers 1 to numAxes.
* @param pmacName Asyn port name for the controller (const char *)
* @param numAxes The number of axes to create, starting at 1.
*
*/
asynStatus pmacCreateAxes(const char *pmacName,
int numAxes)
{
pmacController *pC;
pmacAxis *pAxis;
static const char *functionName = "pmacCreateAxis";
pC = (pmacController*) findAsynPortDriver(pmacName);
if (!pC) {
printf("%s:%s: Error port %s not found\n",
driverName, functionName, pmacName);
return asynError;
}
pC->lock();
for (int axis=1; axis<=numAxes; axis++) {
pAxis = new pmacAxis(pC, axis);
pAxis = NULL;
}
pC->unlock();
return asynSuccess;
}
/**
* C wrapper for the pmacAmorDetectorAxis constructor.
* See pmacAmorDetectorAxis::pmacAmorDetectorAxis.
*
*/
asynStatus pmacCreateAmorDetectorAxis(const char *pmacName, /* specify which controller by port name */
int axis, /* axis number (start from 1). */
int function_code) /* AMOR detector axis function */
{
pmacController *pC;
pmacAxis *pAxis;
static const char *functionName = "pmacCreateHRPTAxis";
pC = (pmacController*) findAsynPortDriver(pmacName);
if (!pC) {
printf("%s:%s: Error port %s not found\n",
driverName, functionName, pmacName);
return asynError;
}
pC->lock();
pAxis = new AmorDetectorAxis(pC, axis, function_code);
pAxis = NULL;
pC->unlock();
return asynSuccess;
}
/*================================ SeleneController ===============================================*/
asynStatus SeleneController::writeFloat64(asynUser *pasynUser, epicsFloat64 value)
{
int function = pasynUser->reason;
asynStatus status = asynError;
SeleneAxis *pAxis = NULL;
char command[64] = {0};
char response[64] = {0};
char message[132];
static const char *functionName = "SeleneController::writeFloat64";
sprintf(message,"%s, reason %d", functionName, function);
pAxis = (SeleneAxis *)this->getAxis(pasynUser);
if (!pAxis) {
return asynError;
}
/* Set the parameter and readback in the parameter library. */
status = pAxis->setDoubleParam(function, value);
if (function == motorLowLimit_) {
sprintf(command, "Q%d54=%f", pAxis->axisNo_, (float)value/(float)MULT);
// sprintf(command, "Q%d54=%d", pAxis->axisNo_, (int)(value/pAxis->scale_/MULT));
asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW,
"%s: Setting low limit on controller %s, axis %d to %f\n",
functionName, portName, pAxis->axisNo_, value);
errlogPrintf("Setting low limit of axis %d to %f, command = %s\n", pAxis->axisNo_, value, command);
} else if (function == motorHighLimit_) {
sprintf(command, "Q%d53=%f", pAxis->axisNo_, (float)value/(float)MULT);
asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW,
"%s: Setting high limit on controller %s, axis %d to %f\n",
functionName, portName, pAxis->axisNo_, value);
errlogPrintf("Setting high limit of axis %d to %f, command = %s\n", pAxis->axisNo_, value, command);
} else if(function == setMotorPosition_){
snprintf(command,sizeof(command),"Q%d59=%f", pAxis->axisNo_, value);
asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW,
"%s: Defining position of axis%d to %f\n",
functionName, pAxis->axisNo_, value);
errlogPrintf("Defining position of axis %d to %f, command = %s\n", pAxis->axisNo_, value, command);
}
//Execute the command.
if (command[0] != 0 && status == asynSuccess) {
status = lowLevelWriteRead(pAxis->axisNo_,command, response);
}
// What if now status != asynSuccess
//Call base class method
//This will handle callCallbacks even if the function was handled here.
status = asynMotorController::writeFloat64(pasynUser, value);
return status;
}
asynStatus pmacV3Controller::writeInt32(asynUser *pasynUser, epicsInt32 value) {
int function = pasynUser->reason;
asynStatus status = asynSuccess;
pmacV3Axis *pAxis = NULL;
char command[64] = {0};
char response[64] = {0};
int isOn;
static const char *functionName = "pmacV3Controller::writeInt32";
debugFlow(functionName);
pAxis = (pmacV3Axis*)this->getAxis(pasynUser);
if (!pAxis) {
return asynError;
}
/* 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 */
pAxis->setIntegerParam(function, value);
if (function == enableAxis_) {
// only send commands when a state change is necessary
snprintf(command, sizeof(command), "P%2.2d00", pAxis->axisNo_);
this->lowLevelWriteRead(pAxis->axisNo_, command, response);
isOn = strtol(response, NULL, 10);
if(value == 1 && isOn == 0) {
sprintf(command, "M%2.2d=15\n", pAxis->axisNo_);
lowLevelWriteRead(pAxis->axisNo_, command, response);
}
sprintf(command, "M%2.2d14=%d\n", pAxis->axisNo_, value);
pAxis->updateMsgTxtFromDriver("");
if(isOn != value) {
asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW,
"%s: Enable axis on controller %s, axis %d enable=%d\n",
functionName, portName, pAxis->axisNo_, value);
lowLevelWriteRead(pAxis->axisNo_, command, response);
}
}
return pmacController::writeInt32(pasynUser, value);
}
asynStatus pmacV3Controller::readInt32(asynUser *pasynUser, epicsInt32 *value) {
int function = pasynUser->reason;
asynStatus status = asynError;
pmacV3Axis *pAxis = NULL;
static const char *functionName = "pmacV3Controller::readInt32";
char command[128];
char response[128];
debugFlow(functionName);
pAxis = (pmacV3Axis*)(this->getAxis(pasynUser));
if (!pAxis) {
return asynError;
}
if (function == axisEnabled_) {
snprintf(command, sizeof(command), "P%2.2d00", pAxis->axisNo_);
status = this->lowLevelWriteRead(pAxis->axisNo_, command, response);
*value = strtol(response, NULL, 10);
int st = setIntegerParam(axisEnabled_, *value);
callParamCallbacks();
}
return pmacController::readInt32(pasynUser, value);
}
/* Code for iocsh registration */
#ifdef vxWorks
#else
/* pmacCreateController */
static const iocshArg pmacCreateControllerArg0 = {"Controller port name", iocshArgString};
static const iocshArg pmacCreateControllerArg1 = {"Low level port name", iocshArgString};
static const iocshArg pmacCreateControllerArg2 = {"Low level port address", iocshArgInt};
static const iocshArg pmacCreateControllerArg3 = {"Number of axes", iocshArgInt};
static const iocshArg pmacCreateControllerArg4 = {"Moving poll rate (ms)", iocshArgInt};
static const iocshArg pmacCreateControllerArg5 = {"Idle poll rate (ms)", iocshArgInt};
static const iocshArg * const pmacCreateControllerArgs[] = {&pmacCreateControllerArg0,
&pmacCreateControllerArg1,
&pmacCreateControllerArg2,
&pmacCreateControllerArg3,
&pmacCreateControllerArg4,
&pmacCreateControllerArg5};
static const iocshFuncDef configpmacCreateController = {"pmacCreateController", 6, pmacCreateControllerArgs};
static void configpmacCreateControllerCallFunc(const iocshArgBuf *args)
{
pmacCreateController(args[0].sval, args[1].sval, args[2].ival, args[3].ival, args[4].ival, args[5].ival);
}
static const iocshFuncDef configSeleneCreateController = {"SeleneCreateController", 6, pmacCreateControllerArgs};
static void configSeleneCreateControllerCallFunc(const iocshArgBuf *args)
{
SeleneCreateController(args[0].sval, args[1].sval, args[2].ival, args[3].ival, args[4].ival, args[5].ival);
}
static const iocshFuncDef configpmacV3CreateController = {
"pmacV3CreateController", 6, pmacCreateControllerArgs
};
static void configpmacV3CreateControllerCallFunc(const iocshArgBuf *args) {
pmacV3CreateController(args[0].sval, args[1].sval, args[2].ival, args[3].ival,
args[4].ival, args[5].ival);
}
/* pmacCreateAxis */
static const iocshArg pmacCreateAxisArg0 = {"Controller port name", iocshArgString};
static const iocshArg pmacCreateAxisArg1 = {"Axis number", iocshArgInt};
static const iocshArg * const pmacCreateAxisArgs[] = {&pmacCreateAxisArg0,
&pmacCreateAxisArg1};
static const iocshFuncDef configpmacAxis = {"pmacCreateAxis", 2, pmacCreateAxisArgs};
static void configpmacAxisCallFunc(const iocshArgBuf *args)
{
pmacCreateAxis(args[0].sval, args[1].ival);
}
/* pmacCreateHRPTAxis */
static const iocshArg pmacCreateHRPTAxisArg0 = {"Controller port name", iocshArgString};
static const iocshArg pmacCreateHRPTAxisArg1 = {"Axis number", iocshArgInt};
static const iocshArg * const pmacCreateHRPTAxisArgs[] = {&pmacCreateAxisArg0,
&pmacCreateAxisArg1};
static const iocshFuncDef configpmacHRPTAxis = {"pmacCreateHRPTAxis", 2, pmacCreateHRPTAxisArgs};
static void configpmacHRPTAxisCallFunc(const iocshArgBuf *args)
{
pmacCreateHRPTAxis(args[0].sval, args[1].ival);
}
/* SeleneCreateAxis */
static const iocshArg SeleneCreateAxisArg0 = {"Controller port name", iocshArgString};
static const iocshArg SeleneCreateAxisArg1 = {"Axis number", iocshArgInt};
static const iocshArg SeleneCreateAxisArg2 = {"limitTraget", iocshArgDouble};
static const iocshArg * const SeleneCreateAxisArgs[] = {&SeleneCreateAxisArg0,
&SeleneCreateAxisArg1,
&SeleneCreateAxisArg2};
static const iocshFuncDef configSeleneCreateAxis = {"SeleneCreateAxis", 3, SeleneCreateAxisArgs};
static void configSeleneCreateAxisCallFunc(const iocshArgBuf *args)
{
SeleneCreateAxis(args[0].sval, args[1].ival,args[2].dval);
}
/* LiftCreateAxis */
static const iocshArg LiftCreateAxisArg0 = {"Controller port name", iocshArgString};
static const iocshArg LiftCreateAxisArg1 = {"Axis number", iocshArgInt};
static const iocshArg * const LiftCreateAxisArgs[] = {&LiftCreateAxisArg0,
&LiftCreateAxisArg1};
static const iocshFuncDef configLiftAxis = {"LiftCreateAxis", 2, LiftCreateAxisArgs};
static void configLiftAxisCallFunc(const iocshArgBuf *args)
{
LiftCreateAxis(args[0].sval, args[1].ival);
}
/* pmacV3CreateAxis */
static const iocshArg pmacV3CreateAxisArg0 = {"Controller port name",
iocshArgString};
static const iocshArg pmacV3CreateAxisArg1 = {"Axis number", iocshArgInt};
static const iocshArg *const pmacV3CreateAxisArgs[] = {&pmacV3CreateAxisArg0,
&pmacV3CreateAxisArg1};
static const iocshFuncDef configpmacV3Axis = {"pmacV3CreateAxis", 2,
pmacV3CreateAxisArgs};
static void configpmacV3AxisCallFunc(const iocshArgBuf *args) {
pmacV3CreateAxis(args[0].sval, args[1].ival);
}
/* pmacCreateAxes */
static const iocshArg pmacCreateAxesArg0 = {"Controller port name", iocshArgString};
static const iocshArg pmacCreateAxesArg1 = {"Num Axes", iocshArgInt};
static const iocshArg * const pmacCreateAxesArgs[] = {&pmacCreateAxesArg0,
&pmacCreateAxesArg1};
static const iocshFuncDef configpmacAxes = {"pmacCreateAxes", 2, pmacCreateAxesArgs};
static void configpmacAxesCallFunc(const iocshArgBuf *args)
{
pmacCreateAxes(args[0].sval, args[1].ival);
}
/* pmacCraeteAmorDetectorAxis */
static const iocshArg pmacCreateAmorDetectorAxisArg0 = {"Controller port name", iocshArgString};
static const iocshArg pmacCreateAmorDetectorAxisArg1 = {"Axis number", iocshArgInt};
static const iocshArg pmacCreateAmorDetectorAxisArg2 = {"Function code", iocshArgInt};
static const iocshArg * const pmacCreateAmorDetectorAxisArgs[] = {&pmacCreateAmorDetectorAxisArg0,
&pmacCreateAmorDetectorAxisArg1, &pmacCreateAmorDetectorAxisArg2};
static const iocshFuncDef configpmacAmorDetectorAxis = {"pmacCreateAmorDetectorAxis", 3, pmacCreateAmorDetectorAxisArgs};
static void configpmacAmorDetectorAxisCallFunc(const iocshArgBuf *args)
{
pmacCreateAmorDetectorAxis(args[0].sval, args[1].ival, args[2].ival);
}
static void pmacControllerRegister(void)
{
iocshRegister(&configpmacCreateController, configpmacCreateControllerCallFunc);
iocshRegister(&configSeleneCreateController, configSeleneCreateControllerCallFunc);
iocshRegister(&configpmacV3CreateController, configpmacV3CreateControllerCallFunc);
iocshRegister(&configpmacAxis, configpmacAxisCallFunc);
iocshRegister(&configpmacHRPTAxis, configpmacHRPTAxisCallFunc);
iocshRegister(&configpmacAmorDetectorAxis, configpmacAmorDetectorAxisCallFunc);
iocshRegister(&configSeleneCreateAxis, configSeleneCreateAxisCallFunc);
iocshRegister(&configLiftAxis, configLiftAxisCallFunc);
iocshRegister(&configpmacV3Axis, configpmacV3AxisCallFunc);
iocshRegister(&configpmacAxes, configpmacAxesCallFunc);
}
epicsExportRegistrar(pmacControllerRegister);
#endif
} // extern "C"
Reference in New Issue View Git Blame Copy Permalink