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

Changes to support CONEX-CC servo controller; it has KD; supports limits, velocity, acceleration; fixed homing problems

Browse Source
This commit is contained in:
MarkRivers
2014-04-08 17:10:28 +00:00
parent c389eb7574
commit d0816ef7f0
1 changed files with 106 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files
motorApp/NewportSrc/AG_CONEX.cpp
+106 -42
View File
@@ -1,6 +1,6 @@
/*
FILENAME... AG_CONEX.cpp
USAGE... Motor driver support for the Newport Agilis UC series controllers.
USAGE... Motor driver support for the Newport CONEX-AGP and CONEX-CC series controllers.
Mark Rivers
April 11, 2013
@@ -23,15 +23,12 @@ April 11, 2013
#define NINT(f) (int)((f)>0 ? (f)+0.5 : (f)-0.5)
#define AGILIS_TIMEOUT 2.0
#define CONEX_TIMEOUT 2.0
#define LINUX_WRITE_DELAY 0.1
#define MAX_CONEX_KP 3000. // Proportional gain
#define MAX_CONEX_KI 3000. // Integral gain
#define MAX_CONEX_LF 1000. // Low-pass filter frequency
/** Creates a new AG_CONEXController object.
* \param[in] portName The name of the asyn port that will be created for this driver
* \param[in] serialPortName The name of the drvAsynSerialPort that was created previously to connect to the Agilis controller
* \param[in] serialPortName The name of the drvAsynSerialPort that was created previously to connect to the CONEX controller
* \param[in] numAxes The number of axes that this controller supports
* \param[in] movingPollPeriod The time between polls when any axis is moving
* \param[in] idlePollPeriod The time between polls when no axis is moving
@@ -50,11 +47,11 @@ AG_CONEXController::AG_CONEXController(const char *portName, const char *serialP
asynStatus status;
static const char *functionName = "AG_CONEXController::AG_CONEXController";
/* Connect to Agilis controller */
/* Connect to CONEX controller */
status = pasynOctetSyncIO->connect(serialPortName, 0, &pasynUserController_, NULL);
if (status) {
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
"%s: cannot connect to Agilis controller\n",
asynPrint(pasynUserSelf, ASYN_TRACE_ERROR,
"%s: cannot connect to CONEX controller\n",
functionName);
return;
}
@@ -63,7 +60,9 @@ AG_CONEXController::AG_CONEXController(const char *portName, const char *serialP
sprintf(outString_, "%dVE", controllerID_);
status = writeReadController();
if (status) {
printf("ERROR: cannot read version information from AG_CONEX controller\n");
asynPrint(pasynUserSelf, ASYN_TRACE_ERROR,
"%s: cannot read version information from AG_CONEX controller\n",
functionName);
return;
}
strcpy(controllerVersion_, &inString_[4]);
@@ -78,7 +77,7 @@ AG_CONEXController::AG_CONEXController(const char *portName, const char *serialP
/** Creates a new AG_CONEXController 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] serialPortName The name of the drvAsynIPPPort that was created previously to connect to the Agilis controller
* \param[in] serialPortName The name of the drvAsynIPPPort that was created previously to connect to the CONEX controller
* \param[in] numAxes The number of axes that this controller supports
* \param[in] movingPollPeriod The time in ms between polls when any axis is moving
* \param[in] idlePollPeriod The time in ms between polls when no axis is moving
@@ -96,20 +95,20 @@ int AG_CONEXCreateController(const char *portName, const char *serialPortName, i
/** Writes a string to the controller.
* Calls writeAgilis() with a default location of the string to write and a default timeout. */
asynStatus AG_CONEXController::writeAgilis()
* Calls writeCONEX() with a default location of the string to write and a default timeout. */
asynStatus AG_CONEXController::writeCONEX()
{
return writeAgilis(outString_, AGILIS_TIMEOUT);
return writeCONEX(outString_, CONEX_TIMEOUT);
}
/** Writes a string to the controller.
* \param[in] output The string to be written.
* \param[in] timeout Timeout before returning an error.*/
asynStatus AG_CONEXController::writeAgilis(const char *output, double timeout)
asynStatus AG_CONEXController::writeCONEX(const char *output, double timeout)
{
size_t nwrite;
asynStatus status;
// const char *functionName="writeAgilis";
// const char *functionName="writeCONEX";
status = pasynOctetSyncIO->write(pasynUserController_, output,
strlen(output), timeout, &nwrite);
@@ -131,7 +130,7 @@ asynStatus AG_CONEXController::writeAgilis(const char *output, double timeout)
*/
void AG_CONEXController::report(FILE *fp, int level)
{
fprintf(fp, "Agilis CONEX motor driver %s, controllerID=%d, version=\"%s\n"
fprintf(fp, "CONEX motor driver %s, controllerID=%d, version=\"%s\n"
" moving poll period=%f, idle poll period=%f\n",
this->portName, controllerID_, controllerVersion_, movingPollPeriod_, idlePollPeriod_);
@@ -169,6 +168,28 @@ AG_CONEXAxis::AG_CONEXAxis(AG_CONEXController *pC)
pC_(pC),
currentPosition_(0.), positionOffset_(0.)
{
static const char *functionName = "AG_CONEXAxis::AG_CONEXAxis";
// Figure out what model this is
if (strstr(pC->controllerVersion_, "CONEX-AGP")) {
conexModel_ = ModelConexAGP;
KPMax_ = 3000.;
KIMax_ = 3000.;
LFMax_ = 1000.;
}
else if (strstr(pC->controllerVersion_, "CONEX-CC")) {
conexModel_ = ModelConexCC;
KPMax_ = 1.e6;
KIMax_ = 1.e6;
KDMax_ = 1.e6;
}
else {
asynPrint(pC->pasynUserSelf, ASYN_TRACE_ERROR,
"%s: unknown model, firmware string=%s\n",
functionName, pC->controllerVersion_);
return;
}
// Read the stage ID
sprintf(pC_->outString_, "%dID?", pC->controllerID_);
pC_->writeReadController();
@@ -179,10 +200,14 @@ AG_CONEXAxis::AG_CONEXAxis(AG_CONEXController *pC)
pC_->writeReadController();
encoderIncrement_ = atof(&pC_->inString_[3]);
// Read the interpolation factor
sprintf(pC_->outString_, "%dIF?", pC->controllerID_);
pC_->writeReadController();
interpolationFactor_ = atof(&pC_->inString_[3]);
// Read the interpolation factor (AGP only)
if (conexModel_ == ModelConexAGP) {
sprintf(pC_->outString_, "%dIF?", pC->controllerID_);
pC_->writeReadController();
interpolationFactor_ = atof(&pC_->inString_[3]);
} else {
interpolationFactor_ = 1.;
}
// Compute the minimum step size
stepSize_ = encoderIncrement_ / interpolationFactor_;
@@ -215,14 +240,21 @@ void AG_CONEXAxis::report(FILE *fp, int level)
sprintf(pC_->outString_, "%dKI?", pC_->controllerID_);
pC_->writeReadController();
KI_ = atof(&pC_->inString_[3]);
sprintf(pC_->outString_, "%dLF?", pC_->controllerID_);
pC_->writeReadController();
LF_ = atof(&pC_->inString_[3]);
if (conexModel_ == ModelConexAGP) {
sprintf(pC_->outString_, "%dLF?", pC_->controllerID_);
pC_->writeReadController();
LF_ = atof(&pC_->inString_[3]);
} else if (conexModel_ == ModelConexCC) {
sprintf(pC_->outString_, "%dKD?", pC_->controllerID_);
pC_->writeReadController();
KD_ = atof(&pC_->inString_[3]);
LF_ = KD_; // For printout below
}
fprintf(fp, " stageID=%s\n"
" currentPosition=%f, positionOffset=%f, encoderIncrement=%f\n"
" interpolationFactor=%f, stepSize=%f, lowLimit=%f, highLimit=%f\n"
" KP=%f, KI=%f, LF=%f\n",
" KP=%f, KI=%f, KD/LF=%f\n",
stageID_,
currentPosition_, positionOffset_, encoderIncrement_,
interpolationFactor_, stepSize_, lowLimit_, highLimit_,
@@ -237,13 +269,21 @@ asynStatus AG_CONEXAxis::move(double position, int relative, double minVelocity,
{
asynStatus status;
// static const char *functionName = "AG_CONEXAxis::move";
// The CONEX-CC supports velocity and acceleration, the CONEX-AGP does not
if (conexModel_ == ModelConexCC) {
sprintf(pC_->outString_, "%dAC%f", pC_->controllerID_, acceleration*stepSize_);
status = pC_->writeCONEX();
sprintf(pC_->outString_, "%dVA%f", pC_->controllerID_, maxVelocity*stepSize_);
status = pC_->writeCONEX();
}
if (relative) {
sprintf(pC_->outString_, "%dPR%f", pC_->controllerID_, position*stepSize_);
} else {
sprintf(pC_->outString_, "%dPA%f", pC_->controllerID_, (position-positionOffset_)*stepSize_);
}
status = pC_->writeAgilis();
status = pC_->writeCONEX();
return status;
}
@@ -252,8 +292,17 @@ asynStatus AG_CONEXAxis::home(double minVelocity, double maxVelocity, double acc
asynStatus status;
//static const char *functionName = "AG_CONEXAxis::home";
// Must go to unreferenced state to home
sprintf(pC_->outString_, "%dRS", pC_->controllerID_);
status = pC_->writeCONEX();
epicsThreadSleep(1.0);
// The CONEX-CC supports home velocity, but only by going to Configuration state (PW1)
// and writing to non-volatile memory with the OH command.
// This is time-consuming and can only be done a limited number of times so we don't do it here.
sprintf(pC_->outString_, "%dOR", pC_->controllerID_);
status = pC_->writeAgilis();
status = pC_->writeCONEX();
return status;
}
@@ -268,7 +317,7 @@ asynStatus AG_CONEXAxis::moveVelocity(double minVelocity, double maxVelocity, do
else position = lowLimit_ + stepSize_;
sprintf(pC_->outString_, "%dPA%f", pC_->controllerID_, position);
status = pC_->writeAgilis();
status = pC_->writeCONEX();
return status;
}
@@ -278,7 +327,7 @@ asynStatus AG_CONEXAxis::stop(double acceleration )
//static const char *functionName = "AG_CONEXAxis::stop";
sprintf(pC_->outString_, "%dST", pC_->controllerID_);
status = pC_->writeAgilis();
status = pC_->writeCONEX();
return status;
}
@@ -296,7 +345,7 @@ asynStatus AG_CONEXAxis::setClosedLoop(bool closedLoop)
//static const char *functionName = "AG_CONEXAxis::setClosedLoop";
sprintf(pC_->outString_, "%dMM%d", pC_->controllerID_, closedLoop ? 1 : 0);
status = pC_->writeAgilis();
status = pC_->writeCONEX();
return status;
}
@@ -323,8 +372,8 @@ asynStatus AG_CONEXAxis::setPGain(double pGain)
getClosedLoop(&closedLoop);
setClosedLoop(false);
// The pGain value from the motor record is between 0 and 1.
sprintf(pC_->outString_, "%dKP%f", pC_->controllerID_, pGain*MAX_CONEX_KP);
status = pC_->writeAgilis();
sprintf(pC_->outString_, "%dKP%f", pC_->controllerID_, pGain*KPMax_);
status = pC_->writeCONEX();
if (closedLoop) setClosedLoop(true);
return status;
}
@@ -338,8 +387,8 @@ asynStatus AG_CONEXAxis::setIGain(double iGain)
getClosedLoop(&closedLoop);
setClosedLoop(false);
// The iGain value from the motor record is between 0 and 1.
sprintf(pC_->outString_, "%dKI%f", pC_->controllerID_, iGain*MAX_CONEX_KI);
status = pC_->writeAgilis();
sprintf(pC_->outString_, "%dKI%f", pC_->controllerID_, iGain*KIMax_);
status = pC_->writeCONEX();
if (closedLoop) setClosedLoop(true);
return status;
}
@@ -352,10 +401,15 @@ asynStatus AG_CONEXAxis::setDGain(double dGain)
getClosedLoop(&closedLoop);
setClosedLoop(false);
// We are using the DGain for the Low pass filter frequency.
// DGain value is between 0 and 1
sprintf(pC_->outString_, "%dLF%f", pC_->controllerID_, dGain*MAX_CONEX_LF);
status = pC_->writeAgilis();
if (conexModel_ == ModelConexCC) {
// The dGain value from the motor record is between 0 and 1.
sprintf(pC_->outString_, "%dKI%f", pC_->controllerID_, dGain*KDMax_);
} else if (conexModel_ == ModelConexAGP) {
// We are using the DGain for the Low pass filter frequency.
// DGain value is between 0 and 1
sprintf(pC_->outString_, "%dLF%f", pC_->controllerID_, dGain*LFMax_);
}
status = pC_->writeCONEX();
if (closedLoop) setClosedLoop(true);
return status;
}
@@ -371,6 +425,8 @@ asynStatus AG_CONEXAxis::poll(bool *moving)
int done=1;
double position;
unsigned int status;
unsigned int state;
int highLimit=0, lowLimit=0;
int count;
bool closedLoop;
asynStatus comStatus;
@@ -391,18 +447,26 @@ asynStatus AG_CONEXAxis::poll(bool *moving)
// The response string is of the form "1TSabcdef"
count = sscanf(pC_->inString_, "%*dTS%*4c%x", &status);
if (count != 1) goto skip;
if ((status == 0x1e) || (status == 0x28)) done = 0;
state = status & 0xff;
if ((state == 0x1e) || (state == 0x28)) done = 0;
setIntegerParam(pC_->motorStatusDone_, done);
*moving = done ? false:true;
// The meaning of the error bits is different for the CC and AGP
if (conexModel_ == ModelConexCC) {
if (status & 0x100) lowLimit = 1;
if (status & 0x200) highLimit = 1;
}
setIntegerParam(pC_->motorStatusLowLimit_, lowLimit);
setIntegerParam(pC_->motorStatusHighLimit_, highLimit);
// Set the power-on (closed loop) status of the motor
comStatus = getClosedLoop(&closedLoop);
if (comStatus) goto skip;
setIntegerParam(pC_->motorStatusPowerOn_, closedLoop ? 1:0);
setIntegerParam(pC_->motorStatusLowLimit_, 0);
setIntegerParam(pC_->motorStatusHighLimit_, 0);
skip:
setIntegerParam(pC_->motorStatusProblem_, comStatus ? 1:0);
callParamCallbacks();