From d0816ef7f0dfeb94eaade62191b6e82572941e6b Mon Sep 17 00:00:00 2001 From: MarkRivers Date: Tue, 8 Apr 2014 17:10:28 +0000 Subject: [PATCH] Changes to support CONEX-CC servo controller; it has KD; supports limits, velocity, acceleration; fixed homing problems --- motorApp/NewportSrc/AG_CONEX.cpp | 148 ++++++++++++++++++++++--------- 1 file changed, 106 insertions(+), 42 deletions(-) diff --git a/motorApp/NewportSrc/AG_CONEX.cpp b/motorApp/NewportSrc/AG_CONEX.cpp index 19a0f03e..f53bbb10 100644 --- a/motorApp/NewportSrc/AG_CONEX.cpp +++ b/motorApp/NewportSrc/AG_CONEX.cpp @@ -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();