diff --git a/dsmApp/src/MD90Driver.cpp b/dsmApp/src/MD90Driver.cpp index afdeeb6..2405fe9 100644 --- a/dsmApp/src/MD90Driver.cpp +++ b/dsmApp/src/MD90Driver.cpp @@ -140,6 +140,37 @@ void MD90Axis::report(FILE *fp, int level) asynMotorAxis::report(fp, level); } +/** Print out message if the motor controller returns a non-zero error code + * \param[in] functionName The function originating the call + * \param[in] reply Reply message returned from motor controller + */ +asynStatus MD90Axis::parseReply(const char *functionName, const char *reply) +{ + int replyStatus; + char replyString[256]; + int replyValue; + asynStatus comStatus; + + comStatus = asynSuccess; + + if (reply[0] == '\0') { + comStatus = asynError; + replyStatus = -1; + } else if ( strcmp(reply, "Unrecognized command.") == 0 ) { + replyStatus = 6; + } else { + sscanf(reply, "%d: %[^:]: %d", &replyStatus, replyString, &replyValue); + } + + if (replyStatus != 0) { + asynPrint(pasynUser_, ASYN_TRACE_ERROR, + "%s: %s\n", + functionName, reply); + } + + return comStatus; +} + /** Acceleration currently unsupported with MD-90 controller * \param[in] acceleration The accelerations to ramp up to max velocity * \param[in] velocity Motor velocity in steps / sec @@ -148,7 +179,7 @@ asynStatus MD90Axis::sendAccelAndVelocity(double acceleration, double velocity) { asynStatus status; int freq; - // static const char *functionName = "MD90::sendAccelAndVelocity"; + static const char *functionName = "MD90::sendAccelAndVelocity"; // Send the velocity // Velocity provided in steps/sec @@ -157,6 +188,9 @@ asynStatus MD90Axis::sendAccelAndVelocity(double acceleration, double velocity) freq = NINT(fabs(velocity / 500.)); sprintf(pC_->outString_, "SSF %d", freq); status = pC_->writeReadController(); + if (!status) { + status = parseReply(functionName, pC_->inString_); + } return status; } @@ -165,7 +199,7 @@ asynStatus MD90Axis::sendAccelAndVelocity(double acceleration, double velocity) asynStatus MD90Axis::move(double position, int relative, double minVelocity, double maxVelocity, double acceleration) { asynStatus status; - // static const char *functionName = "MD90Axis::move"; + static const char *functionName = "MD90Axis::move"; status = sendAccelAndVelocity(acceleration, maxVelocity); @@ -177,18 +211,24 @@ asynStatus MD90Axis::move(double position, int relative, double minVelocity, dou sprintf(pC_->outString_, "CLM %d", NINT(position)); } status = pC_->writeReadController(); + if (!status) { + status = parseReply(functionName, pC_->inString_); + } return status; } asynStatus MD90Axis::home(double minVelocity, double maxVelocity, double acceleration, int forwards) { asynStatus status; - // static const char *functionName = "MD90Axis::home"; + static const char *functionName = "MD90Axis::home"; status = sendAccelAndVelocity(acceleration, maxVelocity); sprintf(pC_->outString_, "HOM"); status = pC_->writeReadController(); + if (!status) { + status = parseReply(functionName, pC_->inString_); + } return status; } @@ -214,16 +254,22 @@ asynStatus MD90Axis::moveVelocity(double minVelocity, double maxVelocity, double sprintf(pC_->outString_, "ESB"); } status = pC_->writeReadController(); + if (!status) { + status = parseReply(functionName, pC_->inString_); + } return status; } asynStatus MD90Axis::stop(double acceleration ) { asynStatus status; - //static const char *functionName = "MD90Axis::stop"; + static const char *functionName = "MD90Axis::stop"; sprintf(pC_->outString_, "STP"); status = pC_->writeReadController(); + if (!status) { + status = parseReply(functionName, pC_->inString_); + } return status; } @@ -233,7 +279,7 @@ asynStatus MD90Axis::stop(double acceleration ) asynStatus MD90Axis::setClosedLoop(bool closedLoop) { asynStatus status; - //static const char *functionName = "MD90Axis::setClosedLoop"; + static const char *functionName = "MD90Axis::setClosedLoop"; if (closedLoop == 1) { sprintf(pC_->outString_, "EPM"); @@ -241,6 +287,9 @@ asynStatus MD90Axis::setClosedLoop(bool closedLoop) sprintf(pC_->outString_, "DPM"); } status = pC_->writeReadController(); + if (!status) { + status = parseReply(functionName, pC_->inString_); + } return status; } @@ -251,23 +300,29 @@ asynStatus MD90Axis::setClosedLoop(bool closedLoop) asynStatus MD90Axis::setIGain(double iGain) { asynStatus status; - //static const char *functionName = "MD90Axis::setIGain"; + static const char *functionName = "MD90Axis::setIGain"; iGain = iGain * 1000; if (iGain < 1) iGain = 1.0; if (iGain > 1000) iGain = 1000.0; sprintf(pC_->outString_, "SGN %d", NINT(iGain)); status = pC_->writeReadController(); + if (!status) { + status = parseReply(functionName, pC_->inString_); + } return status; } asynStatus MD90Axis::doMoveToHome() { asynStatus status; - //static const char *functionName = "MD90Axis::doMoveToHome"; + static const char *functionName = "MD90Axis::doMoveToHome"; sprintf(pC_->outString_, "CLM 0"); status = pC_->writeReadController(); + if (!status) { + status = parseReply(functionName, pC_->inString_); + } return status; } @@ -298,7 +353,7 @@ asynStatus MD90Axis::poll(bool *moving) comStatus = pC_->writeReadController(); if (comStatus) goto skip; // The response string is of the form "0: Power supply enabled state: 1" - sscanf (pC_->inString_, "%d: %[^:]: %d", &replyStatus, replyString, &replyValue); + sscanf(pC_->inString_, "%d: %[^:]: %d", &replyStatus, replyString, &replyValue); driveOn = (replyValue == '1') ? 1:0; setIntegerParam(pC_->motorStatusPowerOn_, driveOn); @@ -307,7 +362,7 @@ asynStatus MD90Axis::poll(bool *moving) comStatus = pC_->writeReadController(); if (comStatus) goto skip; // The response string is of the form "0: Home status: 1" - sscanf (pC_->inString_, "%d: %[^:]: %d", &replyStatus, replyString, &replyValue); + sscanf(pC_->inString_, "%d: %[^:]: %d", &replyStatus, replyString, &replyValue); homed = (replyValue == '1') ? 1:0; setIntegerParam(pC_->motorStatusHomed_, homed); @@ -316,7 +371,7 @@ asynStatus MD90Axis::poll(bool *moving) comStatus = pC_->writeReadController(); if (comStatus) goto skip; // The response string is of the form "0: Current status value: 0" - sscanf (pC_->inString_, "%d: %[^:]: %d", &replyStatus, replyString, &replyValue); + sscanf(pC_->inString_, "%d: %[^:]: %d", &replyStatus, replyString, &replyValue); done = (replyValue == '2') ? 0:1; setIntegerParam(pC_->motorStatusDone_, done); *moving = done ? false:true; @@ -367,7 +422,7 @@ asynStatus MD90Axis::poll(bool *moving) comStatus = pC_->writeReadController(); if (comStatus) goto skip; // The response string is of the form "0: Current position in encoder counts: 1000" - sscanf (pC_->inString_, "%d: %[^:]: %lf", &replyStatus, replyString, &position); + sscanf(pC_->inString_, "%d: %[^:]: %lf", &replyStatus, replyString, &position); setDoubleParam(pC_->motorPosition_, position); setIntegerParam(pC_->motorStatusAtHome_, (position == 0) ? 1:0); // home limit switch setIntegerParam(pC_->motorStatusHome_, (position == 0) ? 1:0); // at home position @@ -377,7 +432,7 @@ asynStatus MD90Axis::poll(bool *moving) comStatus = pC_->writeReadController(); if (comStatus) goto skip; // The response string is of the form "0: Gain: 1000" - sscanf (pC_->inString_, "%d: %[^:]: %d", &replyStatus, replyString, &replyValue); + sscanf(pC_->inString_, "%d: %[^:]: %d", &replyStatus, replyString, &replyValue); setDoubleParam(pC_->motorIGain_, replyValue); // set some default params diff --git a/dsmApp/src/MD90Driver.h b/dsmApp/src/MD90Driver.h index 45715d1..0b1e056 100644 --- a/dsmApp/src/MD90Driver.h +++ b/dsmApp/src/MD90Driver.h @@ -34,6 +34,7 @@ private: MD90Controller *pC_; /**< Pointer to the asynMotorController to which this axis belongs. * Abbreviated because it is used very frequently */ asynStatus sendAccelAndVelocity(double accel, double velocity); + asynStatus parseReply(const char *functionName, const char *reply); friend class MD90Controller; };