diff --git a/sinqEPICSApp/src/PhytronDriver.cpp b/sinqEPICSApp/src/PhytronDriver.cpp index 3d45b3a..17645b1 100644 --- a/sinqEPICSApp/src/PhytronDriver.cpp +++ b/sinqEPICSApp/src/PhytronDriver.cpp @@ -211,7 +211,7 @@ PhytronAxis::PhytronAxis(PhytronController *pC, int axisNo, int enc) brakeIO = -1; } -PhytronAxis::setBrake(int brakeNO) +int PhytronAxis::setBrake(int brakeNO) { if(brakeNO < 1 || brakeNO > 8) { return 0; @@ -248,6 +248,9 @@ asynStatus PhytronAxis::move(double position, int relative, double minVelocity, updateMsgTxtFromDriver(""); + /* + deal with brake + */ if(haveBrake) { sprintf(command,"%sA%dS", pC_->selector, brakeIO); status = pC_->transactController(axisNo_,command,reply); @@ -259,6 +262,21 @@ asynStatus PhytronAxis::move(double position, int relative, double minVelocity, } } + /* + 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; } @@ -283,6 +301,9 @@ asynStatus PhytronAxis::home(double minVelocity, double maxVelocity, double acce updateMsgTxtFromDriver(""); + /* + handle the fucking brake + */ if(haveBrake) { sprintf(command,"%sA%dS", pC_->selector, brakeIO); status = pC_->transactController(axisNo_,command,reply); @@ -294,10 +315,23 @@ asynStatus PhytronAxis::home(double minVelocity, double maxVelocity, double acce } } + /* + 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%cO+",pC_->selector,phytronChar); + sprintf(command, "%s%c0+",pC_->selector,phytronChar); } else { - sprintf(command, "%s%cO-",pC_->selector,phytronChar); + sprintf(command, "%s%c0-",pC_->selector,phytronChar); } homing = 1; next_poll= -1; @@ -448,7 +482,7 @@ asynStatus PhytronAxis::poll(bool *moving) if(haveBrake) { sprintf(command,"%sA%dR", pC_->selector, brakeIO); - status = pC_->transactController(axisNo_,command,reply); + 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"); @@ -461,7 +495,11 @@ asynStatus PhytronAxis::poll(bool *moving) if(!*moving) { if(homing){ - pC_->getDoubleParam(axisNo_,pC_->motorLowLimit_,&lowlim); + if(homing_direction) { + pC_->getDoubleParam(axisNo_,pC_->motorHighLimit_,&lowlim); + } else { + pC_->getDoubleParam(axisNo_,pC_->motorLowLimit_,&lowlim); + } setPosition(lowlim); setIntegerParam(pC_->motorStatusAtHome_, true); } else { @@ -544,7 +582,7 @@ extern "C" asynStatus PhytronConfBrake(const char *port, /* specify whic pC = (PhytronController*) findAsynPortDriver(port); if (!pC) { printf("%s:%s: Error port %s not found\n", - driverName, functionName, port); + "PhytronDriver", functionName, port); return asynError; } @@ -553,8 +591,8 @@ extern "C" asynStatus PhytronConfBrake(const char *port, /* specify whic status = pAxis->setBrake(brakeNO); pC->unlock(); if(!status) { - printf("%s:%s: requested brake IO out of range\n", - driverName, functionName, port); + printf("%s:%s:%s requested brake IO out of range\n", + "PhytronDriver", functionName, port); return asynError; } return asynSuccess; @@ -579,7 +617,7 @@ static void PhytronCreateContollerCallFunc(const iocshArgBuf *args) /* PhytronConfigureBrake */ -static const iocshArg phytronBrake0 = {"Controller port name", iocshArgString}; +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, diff --git a/sinqEPICSApp/src/PhytronDriver.h b/sinqEPICSApp/src/PhytronDriver.h index 8586fb0..dc6bb4a 100644 --- a/sinqEPICSApp/src/PhytronDriver.h +++ b/sinqEPICSApp/src/PhytronDriver.h @@ -38,6 +38,7 @@ private: * Abbreviated because it is used very frequently */ double position; int homing; + int homing_direction; /*1 forward, 0 backwards */ time_t next_poll; int encoder; int haveBrake; diff --git a/sinqEPICSApp/src/pmacAxis.cpp b/sinqEPICSApp/src/pmacAxis.cpp index 02d7a33..1dd82a2 100644 --- a/sinqEPICSApp/src/pmacAxis.cpp +++ b/sinqEPICSApp/src/pmacAxis.cpp @@ -567,6 +567,112 @@ asynStatus pmacHRPTAxis::getAxisStatus(bool *moving) } /*================================= SeleneAxis code ======================================================*/ +SeleneAxis::SeleneAxis(SeleneController *pC, int axisNo, double limitTarget) + : pmacAxis(pC, axisNo) +{ + static const char *functionName = "pmacAxis::pmacAxis"; + + pC_->debugFlow(functionName); + + //Initialize non-static data members + setpointPosition_ = 0.0; + encoderPosition_ = 0.0; + currentVelocity_ = 0.0; + velocity_ = 0.0; + accel_ = 0.0; + highLimit_ = 0.0; + lowLimit_ = 0.0; + scale_ = 1; + previous_position_ = 0.0; + previous_direction_ = 0; + axisErrorCount = 0; + startTime = 0; + status6Time = 0; + starting = 0; + homing = 0; + + /* Set an EPICS exit handler that will shut down polling before asyn kills the IP sockets */ + epicsAtExit(shutdownCallback, pC_); + + //Do an initial poll to get some values from the PMAC + if (getSeleneAxisInitialStatus() != asynSuccess) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: getAxisInitialStatus failed to return asynSuccess. Controller: %s, Axis: %d.\n", functionName, pC_->portName, axisNo_); + } + this->limitTarget = limitTarget; + + + callParamCallbacks(); + + /* Wake up the poller task which will make it do a poll, + * updating values for this axis to use the new resolution (stepSize_) */ + pC_->wakeupPoller(); + +} +/*-------------------------------------------------------------------------------------------*/ +asynStatus SeleneAxis::getSeleneAxisInitialStatus(void) +{ + char command[pC_->PMAC_MAXBUF_]; + char response[pC_->PMAC_MAXBUF_]; + int cmdStatus = 0; + double low_limit = 0.0; + double high_limit = 0.0; + int nvals = 0; + int limVal; + + static const char *functionName = "pmacAxis::getAxisInitialStatus"; + + sprintf(command, "Q%2.2d00", axisNo_); + cmdStatus = pC_->lowLevelWriteRead(axisNo_,command, response); + nvals = sscanf(response, "%lf", &scale_); + if (cmdStatus || nvals != 1) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "%s: Error: failed to read scale_factor on axis %d.\n", functionName, axisNo_); + return asynError; + } + + sprintf(command, "I%d13", axisNo_); + cmdStatus = pC_->lowLevelWriteRead(axisNo_,command, response); + nvals = sscanf(response, "%d", &limVal); + if (cmdStatus || nvals != 1) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "%s: Error: failed to read high limit on axis %d.\n", functionName, axisNo_); + return asynError; + } + high_limit = ((double)limVal)*scale_; + + sprintf(command, "I%d14", axisNo_); + cmdStatus = pC_->lowLevelWriteRead(axisNo_,command, response); + nvals = sscanf(response, "%d", &limVal); + if (cmdStatus || nvals != 1) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "%s: Error: failed to read low limit on axis %d.\n", functionName, axisNo_); + return asynError; + } + low_limit = ((double)limVal)*scale_; + + char message[132]; + sprintf(message,"scale_factor = %lf, high = %lf, low = %lf", scale_,high_limit, low_limit); + pC_->debugFlow(message); + + + setIntegerParam(pC_->motorStatusHasEncoder_, 1); + /* + In some configurations at SINQ, the high and low limits are interchanged in the motor controller. + This messy bit of code takes care of this messy electronics configuration. + */ + if(high_limit > low_limit){ + setDoubleParam(pC_->motorLowLimit_, low_limit); + setDoubleParam(pC_->motorHighLimit_, high_limit); + } else { + setDoubleParam(pC_->motorLowLimit_, high_limit); + setDoubleParam(pC_->motorHighLimit_, low_limit); + } + + callParamCallbacks(); + + return asynSuccess; +} + + +/*----------------------------------------------------------------------------------------------------------*/ asynStatus SeleneAxis::home(double min_velocity, double max_velocity, double acceleration, int forwards) { asynStatus status = asynError; @@ -634,7 +740,10 @@ asynStatus SeleneAxis::setPosition(double position) status = pC_->lowLevelWriteRead(axisNo_,command, response); errlogPrintf("Sending set position : %s\n", command); - + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, + "Setting Position on %d to value %f, command: %s", + axisNo_, position/MULT, command); + return status; } /*======================================= Selene Lift Axis Code ===========================================*/ diff --git a/sinqEPICSApp/src/pmacAxis.h b/sinqEPICSApp/src/pmacAxis.h index 824f91d..9aea827 100644 --- a/sinqEPICSApp/src/pmacAxis.h +++ b/sinqEPICSApp/src/pmacAxis.h @@ -64,7 +64,7 @@ class pmacAxis : public SINQAxis friend class pmacController; }; - +/*----------------------------------------------------------------------------------------------*/ class pmacHRPTAxis : public pmacAxis { public: @@ -80,14 +80,12 @@ class pmacHRPTAxis : public pmacAxis }; +/*--------------------------------------------------------------------------------------------*/ class SeleneAxis : public pmacAxis { public: - SeleneAxis(SeleneController *pController, int axisNo, double limitTarget) : pmacAxis((pmacController *)pController,axisNo) - { - this->limitTarget = limitTarget; - }; + SeleneAxis(SeleneController *pController, int axisNo, double limitTarget); asynStatus move(double position, int relative, double min_velocity, double max_velocity, double acceleration); asynStatus setPosition(double position); asynStatus home(double min_velocity, double max_velocity, double acceleration, int forwards); @@ -98,6 +96,7 @@ class SeleneAxis : public pmacAxis private: double limitTarget; + asynStatus getSeleneAxisInitialStatus(void); }; diff --git a/sinqEPICSApp/src/pmacController.cpp b/sinqEPICSApp/src/pmacController.cpp index ba45829..d444514 100644 --- a/sinqEPICSApp/src/pmacController.cpp +++ b/sinqEPICSApp/src/pmacController.cpp @@ -713,13 +713,13 @@ asynStatus SeleneController::writeFloat64(asynUser *pasynUser, epicsFloat64 valu // TODO: somethign is really shitty here: lowLimit and highLimit cannot be on the command if (function == motorLowLimit_) { - sprintf(command, "Q%d54=%d", pAxis->axisNo_, (int)(value/pAxis->scale_/MULT)); + sprintf(command, "Q%d54=%f", pAxis->axisNo_, (float)value/(float)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=%d", pAxis->axisNo_, (int)(value/pAxis->scale_/MULT)); + 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);