773 lines
24 KiB
C++
773 lines
24 KiB
C++
/*
|
|
FILENAME... PhytronDriver.cpp
|
|
USAGE... Motor driver support for the Phytron MCC-2 motor controller.
|
|
|
|
This is a motor driver for the Phytron MCC-2 motor controller as used at
|
|
PSI. The MCC-2 can do two motors per controller. At PSI, X and Y. The MCC-2 can
|
|
be chained. We do not do this at PSI, thus it is not supported in this driver. The
|
|
driver assumes the controller to be at address 0.
|
|
|
|
The ASCII table actually holds two separate characters, STX and ETX for annotating
|
|
the start and end of messages. I have heard of no one using this feature. But the
|
|
Phytron people use it. The ASCII table also holds two special characters ACK and NACK for
|
|
acknowledge and not acknowledged. Again, no one except the phytron people use this. And for NACK, they
|
|
use 0x05 instead of 0x15 as in the ASCII table. Thus I have to do some translation in this driver.
|
|
|
|
At PSI we talk to the MCC-2 thing via RS-232 and a terminal server. The MCC-2 principally
|
|
supports more means of communications. If this driver works with any other then the RS-232
|
|
option is not known. The likelihood of NO is very high.
|
|
|
|
Though this driver has been written in 2016, the MCC-2 version used is probably of
|
|
2008-2009 vintage. This is the date when the non-EPICS drivers were written.
|
|
|
|
Mark Koennecke
|
|
September 2016
|
|
|
|
Updated to use the new MsgTxt field through SINQAxis
|
|
Added a selector to support multiple phytrons on a connection
|
|
|
|
Mark Koennecke
|
|
January 2019
|
|
|
|
*/
|
|
|
|
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
#include <stdlib.h>
|
|
#include <math.h>
|
|
#include <ctype.h>
|
|
#include <time.h>
|
|
|
|
#include <iocsh.h>
|
|
#include <epicsThread.h>
|
|
#include <errlog.h>
|
|
|
|
#include <asynOctetSyncIO.h>
|
|
|
|
#include "PhytronDriver.h"
|
|
#include <epicsExport.h>
|
|
|
|
#define IDLEPOLL 60
|
|
|
|
/** Creates a new PhytronController object.
|
|
* \param[in] portName The name of the asyn port that will be created for this driver
|
|
* \param[in] PhytronPortName The name of the drvAsynSerialPort that was created previously to connect to the Phytron controller
|
|
* \param[in] numAxes The number of axes that this controller supports
|
|
*/
|
|
PhytronController::PhytronController(const char *portName, const char *PhytronPortName, const char *sel ,
|
|
int encX, int encY)
|
|
: SINQController(portName, PhytronPortName,2)
|
|
{
|
|
asynStatus status;
|
|
static const char *functionName = "PhytronController::PhytronController";
|
|
char etx[2];
|
|
|
|
selector = strdup(sel);
|
|
|
|
/* Connect to Phytron controller */
|
|
status = pasynOctetSyncIO->connect(PhytronPortName, 0, &pasynUserController_, NULL);
|
|
if (status) {
|
|
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"%s: cannot connect to Phytron controller\n",
|
|
functionName);
|
|
}
|
|
etx[0] = 0x03;
|
|
etx[1] = '\0';
|
|
pasynOctetSyncIO->setOutputEos(pasynUserController_,etx,strlen(etx));
|
|
pasynOctetSyncIO->setInputEos(pasynUserController_,etx,strlen(etx));
|
|
|
|
/* The special selector D selects the dose controlled axis version */
|
|
if(strcmp(sel, (const char *)"D") == 0) {
|
|
new PhytronDoseAxis(this, 1, encX);
|
|
new PhytronDoseAxis(this, 2, encY);
|
|
} else {
|
|
new PhytronAxis(this, 1, encX);
|
|
new PhytronAxis(this, 2, encY);
|
|
}
|
|
|
|
|
|
startPoller(1000./1000., IDLEPOLL, 2);
|
|
}
|
|
|
|
PhytronDoseController::PhytronDoseController(const char *portName, const char *PhytronPortName, const char *sel ,
|
|
int encX, int encY)
|
|
: PhytronController(portName, PhytronPortName, sel, encX, encY)
|
|
{
|
|
new PhytronDoseAxis(this, 1, encX);
|
|
new PhytronDoseAxis(this, 2, encY);
|
|
}
|
|
|
|
|
|
/** Creates a new PhytronController object.
|
|
* Configuration command, called directly or from iocsh
|
|
* \param[in] portName The name of the asyn port that will be created for this driver
|
|
* \param[in] PhytronPortName The name of the drvAsynIPPPort that was created previously to connect to the Phytron controller
|
|
* \param[in] numAxes The number of axes that this controller supports
|
|
*/
|
|
extern "C" int PhytronCreateController(const char *portName, const char *PhytronPortName, const char *selector,
|
|
int encX, int encY)
|
|
{
|
|
new PhytronController(portName, PhytronPortName,selector, encX, encY);
|
|
return(asynSuccess);
|
|
}
|
|
|
|
extern "C" int PhytronDoseCreateController(const char *portName, const char *PhytronPortName, const char *selector,
|
|
int encX, int encY)
|
|
{
|
|
new PhytronDoseController(portName, PhytronPortName,selector, encX, encY);
|
|
return(asynSuccess);
|
|
}
|
|
|
|
|
|
/** Reports on status of the driver
|
|
* \param[in] fp The file pointer on which report information will be written
|
|
* \param[in] level The level of report detail desired
|
|
*
|
|
* If details > 0 then information is printed about each axis.
|
|
* After printing controller-specific information it calls asynMotorController::report()
|
|
*/
|
|
void PhytronController::report(FILE *fp, int level)
|
|
{
|
|
fprintf(fp, "Phytron motor driver %s, numAxes=%d\n",
|
|
this->portName, numAxes_);
|
|
|
|
// Call the base class method
|
|
asynMotorController::report(fp, level);
|
|
}
|
|
|
|
/** Returns a pointer to an PhytronAxis object.
|
|
* Returns NULL if the axis number encoded in pasynUser is invalid.
|
|
* \param[in] pasynUser asynUser structure that encodes the axis index number. */
|
|
PhytronAxis* PhytronController::getAxis(asynUser *pasynUser)
|
|
{
|
|
return static_cast<PhytronAxis*>(asynMotorController::getAxis(pasynUser));
|
|
}
|
|
|
|
/** Returns a pointer to an PhytronAxis object.
|
|
* Returns NULL if the axis number encoded in pasynUser is invalid.
|
|
* \param[in] axisNo Axis index number. */
|
|
PhytronAxis* PhytronController::getAxis(int axisNo)
|
|
{
|
|
return static_cast<PhytronAxis*>(asynMotorController::getAxis(axisNo));
|
|
}
|
|
|
|
/** Returns a pointer to an PhytronDoseAxis object.
|
|
* Returns NULL if the axis number encoded in pasynUser is invalid.
|
|
* \param[in] pasynUser asynUser structure that encodes the axis index number. */
|
|
PhytronDoseAxis* PhytronDoseController::getAxis(asynUser *pasynUser)
|
|
{
|
|
return static_cast<PhytronDoseAxis*>(asynMotorController::getAxis(pasynUser));
|
|
}
|
|
|
|
/** Returns a pointer to an PhytronDoseAxis object.
|
|
* Returns NULL if the axis number encoded in pasynUser is invalid.
|
|
* \param[in] axisNo Axis index number. */
|
|
PhytronDoseAxis* PhytronDoseController::getAxis(int axisNo)
|
|
{
|
|
return static_cast<PhytronDoseAxis*>(asynMotorController::getAxis(axisNo));
|
|
}
|
|
|
|
|
|
/**
|
|
* send a command to the Phytron and read the reply. Do some error and controller
|
|
* issue fixing on the way
|
|
* \param[in] command The command to send
|
|
* \param[out] reply The controllers reply
|
|
*/
|
|
|
|
asynStatus PhytronController::transactController(int axisNo,char command[COMLEN], char reply[COMLEN])
|
|
{
|
|
asynStatus status;
|
|
size_t in, out;
|
|
int reason;
|
|
char myReply[COMLEN+10], myCommand[COMLEN+10], *pPtr;
|
|
SINQAxis *axis = getAxis(axisNo);
|
|
|
|
|
|
/* pasynOctetSyncIO->flush(pasynUserController_); */
|
|
|
|
|
|
memset(myCommand,0,sizeof(myCommand));
|
|
memset(myReply,0,sizeof(myReply));
|
|
/*
|
|
add the stx
|
|
*/
|
|
myCommand[0] = 0x02;
|
|
pPtr = myCommand+1;
|
|
strcpy(pPtr,command);
|
|
status = pasynOctetSyncIO->writeRead(pasynUserController_, myCommand, strlen(myCommand),
|
|
myReply,sizeof(myReply), 1.,&out,&in,&reason);
|
|
if(status != asynSuccess){
|
|
if(axis!= NULL){
|
|
axis->updateMsgTxtFromDriver("Lost connection to motor controller");
|
|
}
|
|
return status;
|
|
}
|
|
|
|
/*
|
|
skip over the stx
|
|
*/
|
|
pPtr = myReply+1;
|
|
/*
|
|
handle ACK, NACK
|
|
*/
|
|
if(*pPtr == 0x05) {
|
|
strcpy(reply,"NACK");
|
|
pPtr++;
|
|
} else if(*pPtr == 0x06){
|
|
strcpy(reply,"ACK");
|
|
pPtr++;
|
|
}
|
|
/*
|
|
I may need to replace the ETX. But I am not sure if asyn did
|
|
not remove it for me.
|
|
*/
|
|
strncat(reply,pPtr,COMLEN-1);
|
|
|
|
|
|
return status;
|
|
}
|
|
|
|
// These are the PhytronAxis methods
|
|
|
|
/** Creates a new PhytronAxis object.
|
|
* \param[in] pC Pointer to the PhytronController to which this axis belongs.
|
|
* \param[in] axisNo Index number of this axis, range 0 to pC->numAxes_-1.
|
|
*
|
|
* Initializes register numbers, etc.
|
|
*/
|
|
PhytronAxis::PhytronAxis(PhytronController *pC, int axisNo, int enc)
|
|
: SINQAxis(pC, axisNo),
|
|
pC_(pC)
|
|
{
|
|
encoder = enc;
|
|
if(axisNo == 1){
|
|
phytronChar = 'X';
|
|
} else {
|
|
phytronChar = 'Y';
|
|
}
|
|
haveBrake = 0;
|
|
brakeIO = -1;
|
|
next_poll = -1;
|
|
}
|
|
|
|
int PhytronAxis::setBrake(int brakeNO)
|
|
{
|
|
if(brakeNO < 1 || brakeNO > 8) {
|
|
return 0;
|
|
}
|
|
|
|
haveBrake = 1;
|
|
brakeIO = brakeNO;
|
|
return 1;
|
|
}
|
|
|
|
/** Reports on status of the axis
|
|
* \param[in] fp The file pointer on which report information will be written
|
|
* \param[in] level The level of report detail desired
|
|
*
|
|
* After printing device-specific information calls asynMotorAxis::report()
|
|
*/
|
|
void PhytronAxis::report(FILE *fp, int level)
|
|
{
|
|
if (level > 0) {
|
|
fprintf(fp, " axis %d\n",
|
|
axisNo_);
|
|
}
|
|
|
|
// Call the base class method
|
|
//asynMotorAxis::report(fp, level);
|
|
}
|
|
|
|
|
|
asynStatus PhytronAxis::move(double position, int relative, double minVelocity, double maxVelocity, double acceleration)
|
|
{
|
|
asynStatus status;
|
|
//static const char *functionName = "PhytronAxis::move";
|
|
char command[COMLEN], reply[COMLEN];
|
|
|
|
updateMsgTxtFromDriver("");
|
|
|
|
/*
|
|
deal with brake
|
|
*/
|
|
if(haveBrake) {
|
|
sprintf(command,"%sA%dS", pC_->selector, brakeIO);
|
|
status = pC_->transactController(axisNo_,command,reply);
|
|
if(strstr(reply,"NACK") != NULL){
|
|
errlogSevPrintf(errlogMajor, "Failed to release brake on %d", axisNo_);
|
|
updateMsgTxtFromDriver("Failed to release brake");
|
|
setIntegerParam(pC_->motorStatusProblem_, true);
|
|
return asynError;
|
|
}
|
|
}
|
|
|
|
/*
|
|
set speed
|
|
*/
|
|
sprintf(command, "%s%cP14S%f", pC_->selector, phytronChar, maxVelocity);
|
|
status = pC_->transactController(axisNo_,command,reply);
|
|
if(strstr(reply,"NACK") != NULL){
|
|
errlogSevPrintf(errlogMajor, "Speed not accepted on %d", axisNo_);
|
|
updateMsgTxtFromDriver("Invalid speed");
|
|
setIntegerParam(pC_->motorStatusProblem_, true);
|
|
return asynError;
|
|
}
|
|
|
|
/*
|
|
actually send a move command
|
|
*/
|
|
if (relative) {
|
|
position += this->position;
|
|
}
|
|
homing = 0;
|
|
sprintf(command, "%s%cA%f", pC_->selector,phytronChar,position/1000.);
|
|
status = pC_->transactController(axisNo_,command,reply);
|
|
if(strstr(reply,"NACK") != NULL){
|
|
errlogSevPrintf(errlogMajor, "Drive command not acknowledged on %d", axisNo_);
|
|
updateMsgTxtFromDriver("Drive command not acknowledged");
|
|
setIntegerParam(pC_->motorStatusProblem_, true);
|
|
return asynError;
|
|
}
|
|
next_poll = -1;
|
|
return status;
|
|
}
|
|
|
|
asynStatus PhytronAxis::home(double minVelocity, double maxVelocity, double acceleration, int forwards)
|
|
{
|
|
asynStatus status;
|
|
//static const char *functionName = "PhytronAxis::home";
|
|
char command[COMLEN], reply[COMLEN];
|
|
|
|
updateMsgTxtFromDriver("");
|
|
|
|
/*
|
|
handle the fucking brake
|
|
*/
|
|
if(haveBrake) {
|
|
sprintf(command,"%sA%dS", pC_->selector, brakeIO);
|
|
status = pC_->transactController(axisNo_,command,reply);
|
|
if(strstr(reply,"NACK") != NULL){
|
|
errlogSevPrintf(errlogMajor, "Failed to release brake on %d", axisNo_);
|
|
updateMsgTxtFromDriver("Failed to release brake");
|
|
setIntegerParam(pC_->motorStatusProblem_, true);
|
|
return asynError;
|
|
}
|
|
}
|
|
|
|
/*
|
|
set speed
|
|
*/
|
|
sprintf(command, "%s%cP14S%f", pC_->selector, phytronChar, maxVelocity);
|
|
status = pC_->transactController(axisNo_,command,reply);
|
|
if(strstr(reply,"NACK") != NULL){
|
|
errlogSevPrintf(errlogMajor, "Speed not accepted on %d", axisNo_);
|
|
updateMsgTxtFromDriver("Invalid speed");
|
|
setIntegerParam(pC_->motorStatusProblem_, true);
|
|
return asynError;
|
|
}
|
|
|
|
homing_direction = forwards;
|
|
if(forwards){
|
|
sprintf(command, "%s%c0+",pC_->selector,phytronChar);
|
|
} else {
|
|
sprintf(command, "%s%c0-",pC_->selector,phytronChar);
|
|
}
|
|
homing = 1;
|
|
next_poll= -1;
|
|
status = pC_->transactController(axisNo_,command,reply);
|
|
if(strstr(reply,"NACK") != NULL){
|
|
errlogSevPrintf(errlogMajor, "Home command not acknowledged on %d", axisNo_);
|
|
setIntegerParam(pC_->motorStatusProblem_, true);
|
|
updateMsgTxtFromDriver("Home command not acknowledged");
|
|
return asynError;
|
|
}
|
|
return status;
|
|
}
|
|
|
|
asynStatus PhytronAxis::moveVelocity(double minVelocity, double maxVelocity, double acceleration)
|
|
{
|
|
asynStatus status;
|
|
double target;
|
|
|
|
//static const char *functionName = "PhytronAxis::moveVelocity";
|
|
|
|
// asynPrint(pasynUser_, ASYN_TRACE_FLOW,
|
|
// "%s: minVelocity=%f, maxVelocity=%f, acceleration=%f\n",
|
|
// functionName, minVelocity, maxVelocity, acceleration);
|
|
|
|
updateMsgTxtFromDriver("");
|
|
|
|
|
|
if (maxVelocity > 0.) {
|
|
/* This is a positive move */
|
|
pC_->getDoubleParam(axisNo_,pC_->motorHighLimit_,&target);
|
|
} else {
|
|
/* This is a negative move */
|
|
pC_->getDoubleParam(axisNo_,pC_->motorLowLimit_,&target);
|
|
}
|
|
status = move(target,0,0,0,0);
|
|
next_poll = -1;
|
|
return status;
|
|
}
|
|
|
|
asynStatus PhytronAxis::stop(double acceleration )
|
|
{
|
|
asynStatus status;
|
|
//static const char *functionName = "PhytronAxis::stop";
|
|
char command[COMLEN], reply[COMLEN];
|
|
|
|
sprintf(command, "%s%cSN", pC_->selector,phytronChar);
|
|
status = pC_->transactController(axisNo_,command,reply);
|
|
errlogPrintf("Sent STOP on Axis %d\n", axisNo_);
|
|
updateMsgTxtFromDriver("Axis interrupted");
|
|
|
|
return status;
|
|
}
|
|
|
|
asynStatus PhytronAxis::setPosition(double position)
|
|
{
|
|
asynStatus status;
|
|
//static const char *functionName = "PhytronAxis::setPosition";
|
|
char command[COMLEN], reply[COMLEN];
|
|
|
|
errlogPrintf("PhytronAxis::setPosition called with %lf\n", position);
|
|
|
|
sprintf(command, "%s%cP22S%f", pC_->selector,phytronChar, position/1000.);
|
|
status = pC_->transactController(axisNo_,command,reply);
|
|
sprintf(command, "%s%cP20S%f", pC_->selector,phytronChar, position/1000.);
|
|
status = pC_->transactController(axisNo_,command,reply);
|
|
next_poll = -1;
|
|
|
|
return status;
|
|
}
|
|
|
|
asynStatus PhytronAxis::setClosedLoop(bool closedLoop)
|
|
{
|
|
//static const char *functionName = "PhytronAxis::setClosedLoop";
|
|
|
|
/*
|
|
This belongs into the Kingdom of Electronics.
|
|
We do not do this.
|
|
*/
|
|
|
|
return asynError;
|
|
}
|
|
|
|
/** Polls the axis.
|
|
* This function reads the motor position, the limit status, the home status, the moving status,
|
|
* and the drive power-on status.
|
|
* It calls setIntegerParam() and setDoubleParam() for each item that it polls,
|
|
* and then calls callParamCallbacks() at the end.
|
|
* \param[out] moving A flag that is set indicating that the axis is moving (true) or done (false). */
|
|
asynStatus PhytronAxis::poll(bool *moving)
|
|
{
|
|
asynStatus comStatus = asynSuccess;
|
|
char command[COMLEN], reply[COMLEN];
|
|
double lowlim;
|
|
|
|
|
|
// protect against excessive polling
|
|
if(time(NULL) < next_poll){
|
|
*moving = false;
|
|
return asynSuccess;
|
|
}
|
|
|
|
setIntegerParam(pC_->motorStatusProblem_, false);
|
|
// Read the current motor position
|
|
if(encoder) {
|
|
sprintf(command,"%s%cP22R",pC_->selector,phytronChar);
|
|
} else {
|
|
sprintf(command,"%s%cP20R",pC_->selector,phytronChar);
|
|
}
|
|
|
|
comStatus = pC_->transactController(axisNo_,command,reply);
|
|
if(comStatus == asynError) {
|
|
setIntegerParam(pC_->motorStatusProblem_, true);
|
|
updateMsgTxtFromDriver("No connection to phytron controller");
|
|
goto skip;
|
|
}
|
|
if(strstr(reply,"NACK") != NULL){
|
|
setIntegerParam(pC_->motorStatusProblem_, true);
|
|
errlogSevPrintf(errlogMajor, "Bad reply for position on %d", axisNo_);
|
|
updateMsgTxtFromDriver("Bad reply reading position");
|
|
goto skip;
|
|
}
|
|
/*
|
|
read over the ACK
|
|
*/
|
|
sscanf(reply+3,"%lf", &position);
|
|
/* errlogPrintf("Axis %d, reply %s, position %lf\n", axisNo_, reply, position); */
|
|
setDoubleParam(pC_->motorPosition_, position*1000);
|
|
setDoubleParam(pC_->motorEncoderPosition_, position);
|
|
|
|
|
|
// Read the moving status of this motor
|
|
sprintf(command,"%s%c=H",pC_->selector,phytronChar);
|
|
comStatus = pC_->transactController(axisNo_,command,reply);
|
|
if(comStatus == asynError){
|
|
setIntegerParam(pC_->motorStatusProblem_, true);
|
|
updateMsgTxtFromDriver("No connection to phytron controller");
|
|
goto skip;
|
|
}
|
|
/* errlogPrintf("Axis %d, status reply %s, position %lf\n", axisNo_, reply, position); */
|
|
if(strstr(reply,"ACKN") != NULL){
|
|
*moving = true;
|
|
next_poll = -1;
|
|
setIntegerParam(pC_->motorStatusDone_, false);
|
|
} else if(strstr(reply,"ACKE") != NULL){
|
|
*moving = false;
|
|
next_poll = time(NULL)+IDLEPOLL;
|
|
setIntegerParam(pC_->motorStatusDone_, true);
|
|
|
|
if(haveBrake) {
|
|
sprintf(command,"%sA%dR", pC_->selector, brakeIO);
|
|
comStatus = pC_->transactController(axisNo_,command,reply);
|
|
if(strstr(reply,"NACK") != NULL){
|
|
errlogSevPrintf(errlogMajor, "Failed to set brake on %d", axisNo_);
|
|
updateMsgTxtFromDriver("Failed to set brake");
|
|
setIntegerParam(pC_->motorStatusProblem_, true);
|
|
return asynError;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
if(!*moving) {
|
|
if(homing){
|
|
if(homing_direction) {
|
|
pC_->getDoubleParam(axisNo_,pC_->motorHighLimit_,&lowlim);
|
|
} else {
|
|
pC_->getDoubleParam(axisNo_,pC_->motorLowLimit_,&lowlim);
|
|
}
|
|
setPosition(lowlim);
|
|
setIntegerParam(pC_->motorStatusAtHome_, true);
|
|
} else {
|
|
/*
|
|
check limits and errors, upper
|
|
*/
|
|
sprintf(command,"%s%c=I+",pC_->selector,phytronChar);
|
|
comStatus = pC_->transactController(axisNo_,command,reply);
|
|
if(comStatus == asynError) {
|
|
setIntegerParam(pC_->motorStatusProblem_, true);
|
|
updateMsgTxtFromDriver("No connection to phytron controller");
|
|
goto skip;
|
|
}
|
|
if(strstr(reply,"ACKE") != NULL){
|
|
setIntegerParam(pC_->motorStatusHighLimit_, true);
|
|
updateMsgTxtFromDriver("Hit High Limit");
|
|
comStatus = asynError;
|
|
goto skip;
|
|
} else {
|
|
setIntegerParam(pC_->motorStatusHighLimit_, false);
|
|
}
|
|
|
|
/*
|
|
lower limit
|
|
*/
|
|
sprintf(command,"%s%c=I-",pC_->selector,phytronChar);
|
|
comStatus = pC_->transactController(axisNo_,command,reply);
|
|
if(comStatus == asynError){
|
|
setIntegerParam(pC_->motorStatusProblem_, true);
|
|
updateMsgTxtFromDriver("No connection to phytron controller");
|
|
goto skip;
|
|
}
|
|
if(strstr(reply,"ACKE") != NULL){
|
|
setIntegerParam(pC_->motorStatusLowLimit_, true);
|
|
updateMsgTxtFromDriver("Low Limit Hit");
|
|
comStatus = asynError;
|
|
goto skip;
|
|
} else {
|
|
setIntegerParam(pC_->motorStatusLowLimit_, false);
|
|
}
|
|
|
|
/*
|
|
error
|
|
*/
|
|
sprintf(command,"%s%c=E",pC_->selector,phytronChar);
|
|
comStatus = pC_->transactController(axisNo_,command,reply);
|
|
if(comStatus == asynError) {
|
|
setIntegerParam(pC_->motorStatusProblem_, true);
|
|
updateMsgTxtFromDriver("No connection to phytron controller");
|
|
goto skip;
|
|
}
|
|
if(strstr(reply,"ACKE") != NULL){
|
|
setIntegerParam(pC_->motorStatusProblem_, true);
|
|
errlogSevPrintf(errlogMajor, "Electronics on %d", axisNo_);
|
|
updateMsgTxtFromDriver("Electronics error");
|
|
comStatus = asynError;
|
|
goto skip;
|
|
} else {
|
|
setIntegerParam(pC_->motorStatusProblem_, false);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
skip:
|
|
callParamCallbacks();
|
|
return comStatus;
|
|
}
|
|
|
|
/* The special PhytronDoseAxis code used when the speed is controlled through the neutron flux
|
|
*/
|
|
|
|
PhytronDoseAxis::PhytronDoseAxis(PhytronController *pC, int axisNo, int enc)
|
|
: PhytronAxis(pC, axisNo, enc)
|
|
{
|
|
if(axisNo == 1){
|
|
doseChar = '3';
|
|
} else {
|
|
doseChar = '4';
|
|
}
|
|
}
|
|
|
|
|
|
asynStatus PhytronDoseAxis::move(double position, int relative, double minVelocity, double maxVelocity, double acceleration)
|
|
{
|
|
asynStatus status;
|
|
//static const char *functionName = "PhytronDoseAxis::move";
|
|
char command[COMLEN], reply[COMLEN];
|
|
float realTarget;
|
|
|
|
updateMsgTxtFromDriver("");
|
|
|
|
/*
|
|
set conversion factor from neutron rate to speed
|
|
*/
|
|
sprintf(command, "R%c3S%f", doseChar, maxVelocity);
|
|
status = pC_->transactController(axisNo_,command,reply);
|
|
if(strstr(reply,"NACK") != NULL){
|
|
errlogSevPrintf(errlogMajor, "Speed not accepted on %d", axisNo_);
|
|
updateMsgTxtFromDriver("Invalid speed");
|
|
setIntegerParam(pC_->motorStatusProblem_, true);
|
|
return asynError;
|
|
}
|
|
|
|
/*
|
|
actually send a move command
|
|
*/
|
|
if (relative) {
|
|
position += this->position;
|
|
}
|
|
homing = 0;
|
|
|
|
/* set target */
|
|
realTarget = position/1000.;
|
|
if(realTarget > this->position) {
|
|
sprintf(command, "R%c1S%f", doseChar,realTarget);
|
|
} else {
|
|
sprintf(command, "R%c2S%f", doseChar,realTarget);
|
|
}
|
|
status = pC_->transactController(axisNo_,command,reply);
|
|
if(strstr(reply,"NACK") != NULL){
|
|
errlogSevPrintf(errlogMajor, "Drive command not acknowledged on %d", axisNo_);
|
|
updateMsgTxtFromDriver("Drive command not acknowledged");
|
|
setIntegerParam(pC_->motorStatusProblem_, true);
|
|
return asynError;
|
|
}
|
|
|
|
/* really start the move */
|
|
sprintf(command, "R%c01S", doseChar);
|
|
status = pC_->transactController(axisNo_,command,reply);
|
|
if(strstr(reply,"NACK") != NULL){
|
|
errlogSevPrintf(errlogMajor, "Drive start command not acknowledged on %d", axisNo_);
|
|
updateMsgTxtFromDriver("Drive command not acknowledged");
|
|
setIntegerParam(pC_->motorStatusProblem_, true);
|
|
return asynError;
|
|
}
|
|
|
|
next_poll = -1;
|
|
return status;
|
|
}
|
|
|
|
|
|
extern "C" asynStatus PhytronConfBrake(const char *port, /* specify which controller by port name */
|
|
int axis, /* axis: 0, 1 */
|
|
int brakeNO) /* brakeIO No, 1-8 */
|
|
{
|
|
PhytronController *pC;
|
|
PhytronAxis *pAxis;
|
|
int status;
|
|
|
|
static const char *functionName = "PhytronConfBrake";
|
|
|
|
pC = (PhytronController*) findAsynPortDriver(port);
|
|
if (!pC) {
|
|
printf("%s:%s: Error port %s not found\n",
|
|
"PhytronDriver", functionName, port);
|
|
return asynError;
|
|
}
|
|
|
|
pC->lock();
|
|
pAxis = pC->getAxis(axis);
|
|
status = pAxis->setBrake(brakeNO);
|
|
pC->unlock();
|
|
if(!status) {
|
|
printf("%s:%s:%s requested brake IO out of range\n",
|
|
"PhytronDriver", functionName, port);
|
|
return asynError;
|
|
}
|
|
return asynSuccess;
|
|
}
|
|
|
|
/** Code for iocsh registration */
|
|
static const iocshArg PhytronCreateControllerArg0 = {"Port name", iocshArgString};
|
|
static const iocshArg PhytronCreateControllerArg1 = {"Phytron port name", iocshArgString};
|
|
static const iocshArg PhytronCreateControllerArg2 = {"Phytron Selector", iocshArgString};
|
|
static const iocshArg PhytronCreateControllerArg3 = {"EncoderX", iocshArgInt};
|
|
static const iocshArg PhytronCreateControllerArg4 = {"EncoderY", iocshArgInt};
|
|
static const iocshArg * const PhytronCreateControllerArgs[] = {&PhytronCreateControllerArg0,
|
|
&PhytronCreateControllerArg1,
|
|
&PhytronCreateControllerArg2,
|
|
&PhytronCreateControllerArg3,
|
|
&PhytronCreateControllerArg4};
|
|
static const iocshFuncDef PhytronCreateControllerDef = {"PhytronCreateController", 5, PhytronCreateControllerArgs};
|
|
static void PhytronCreateContollerCallFunc(const iocshArgBuf *args)
|
|
{
|
|
PhytronCreateController(args[0].sval, args[1].sval, args[2].sval, args[3].ival,args[4].ival);
|
|
}
|
|
|
|
static const iocshArg PhytronDoseCreateControllerArg0 = {"Port name", iocshArgString};
|
|
static const iocshArg PhytronDoseCreateControllerArg1 = {"Phytron port name", iocshArgString};
|
|
static const iocshArg PhytronDoseCreateControllerArg2 = {"Phytron Selector", iocshArgString};
|
|
static const iocshArg PhytronDoseCreateControllerArg3 = {"EncoderX", iocshArgInt};
|
|
static const iocshArg PhytronDoseCreateControllerArg4 = {"EncoderY", iocshArgInt};
|
|
static const iocshArg * const PhytronDoseCreateControllerArgs[] = {&PhytronDoseCreateControllerArg0,
|
|
&PhytronDoseCreateControllerArg1,
|
|
&PhytronDoseCreateControllerArg2,
|
|
&PhytronDoseCreateControllerArg3,
|
|
&PhytronDoseCreateControllerArg4};
|
|
static const iocshFuncDef PhytronDoseCreateControllerDef = {"PhytronDoseCreateController", 5, PhytronDoseCreateControllerArgs};
|
|
static void PhytronDoseCreateContollerCallFunc(const iocshArgBuf *args)
|
|
{
|
|
PhytronDoseCreateController(args[0].sval, args[1].sval, args[2].sval, args[3].ival,args[4].ival);
|
|
}
|
|
|
|
/* PhytronConfigureBrake */
|
|
static const iocshArg phytronBrakeArg0 = {"Controller port name", iocshArgString};
|
|
static const iocshArg phytronBrakeArg1 = {"Axis number", iocshArgInt};
|
|
static const iocshArg phytronBrakeArg2 = {"Brake IO number", iocshArgInt};
|
|
static const iocshArg * const phytronBrakeArgs[] = {&phytronBrakeArg0,
|
|
&phytronBrakeArg1,
|
|
&phytronBrakeArg2};
|
|
static const iocshFuncDef phytronBrakeDef = {"PhytronConfigureBrake", 3, phytronBrakeArgs};
|
|
|
|
static void phytronBrakeCallFunc(const iocshArgBuf *args)
|
|
{
|
|
PhytronConfBrake(args[0].sval, args[1].ival, args[2].ival);
|
|
}
|
|
|
|
|
|
static void PhytronRegister(void)
|
|
{
|
|
iocshRegister(&PhytronCreateControllerDef, PhytronCreateContollerCallFunc);
|
|
iocshRegister(&PhytronDoseCreateControllerDef, PhytronDoseCreateContollerCallFunc);
|
|
iocshRegister(&phytronBrakeDef, phytronBrakeCallFunc);
|
|
}
|
|
|
|
extern "C" {
|
|
epicsExportRegistrar(PhytronRegister);
|
|
}
|