diff --git a/sinqEPICSApp/src/PhytronDriver.cpp b/sinqEPICSApp/src/PhytronDriver.cpp index f9443d4..3d45b3a 100644 --- a/sinqEPICSApp/src/PhytronDriver.cpp +++ b/sinqEPICSApp/src/PhytronDriver.cpp @@ -207,9 +207,20 @@ PhytronAxis::PhytronAxis(PhytronController *pC, int axisNo, int enc) } else { phytronChar = 'Y'; } + haveBrake = 0; + brakeIO = -1; } - +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 @@ -236,6 +247,17 @@ asynStatus PhytronAxis::move(double position, int relative, double minVelocity, char command[COMLEN], reply[COMLEN]; updateMsgTxtFromDriver(""); + + 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; + } + } if (relative) { position += this->position; @@ -261,6 +283,17 @@ asynStatus PhytronAxis::home(double minVelocity, double maxVelocity, double acce updateMsgTxtFromDriver(""); + 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; + } + } + if(forwards){ sprintf(command, "%s%cO+",pC_->selector,phytronChar); } else { @@ -412,6 +445,17 @@ asynStatus PhytronAxis::poll(bool *moving) *moving = false; next_poll = time(NULL)+IDLEPOLL; setIntegerParam(pC_->motorStatusDone_, true); + + if(haveBrake) { + sprintf(command,"%sA%dR", pC_->selector, brakeIO); + status = 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; + } + } } @@ -487,12 +531,41 @@ asynStatus PhytronAxis::poll(bool *moving) return comStatus; } +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", + driverName, functionName, port); + return asynError; + } + + pC->lock(); + pAxis = pC->getAxis(axis); + status = pAxis->setBrake(brakeNO); + pC->unlock(); + if(!status) { + printf("%s:%s: requested brake IO out of range\n", + driverName, 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 = {"EnoderX", iocshArgInt}; -static const iocshArg PhytronCreateControllerArg4 = {"EnoderY", iocshArgInt}; +static const iocshArg PhytronCreateControllerArg3 = {"EncoderX", iocshArgInt}; +static const iocshArg PhytronCreateControllerArg4 = {"EncoderY", iocshArgInt}; static const iocshArg * const PhytronCreateControllerArgs[] = {&PhytronCreateControllerArg0, &PhytronCreateControllerArg1, &PhytronCreateControllerArg2, @@ -504,9 +577,26 @@ static void PhytronCreateContollerCallFunc(const iocshArgBuf *args) PhytronCreateController(args[0].sval, args[1].sval, args[2].sval, args[3].ival,args[4].ival); } + +/* PhytronConfigureBrake */ +static const iocshArg phytronBrake0 = {"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(&phytronBrakeDef, phytronBrakeCallFunc); } extern "C" { diff --git a/sinqEPICSApp/src/PhytronDriver.h b/sinqEPICSApp/src/PhytronDriver.h index 9b0e07b..8586fb0 100644 --- a/sinqEPICSApp/src/PhytronDriver.h +++ b/sinqEPICSApp/src/PhytronDriver.h @@ -30,6 +30,7 @@ public: asynStatus poll(bool *moving); asynStatus setPosition(double position); asynStatus setClosedLoop(bool closedLoop); + int setBrake(int brakeIO); private: char phytronChar; @@ -39,6 +40,8 @@ private: int homing; time_t next_poll; int encoder; + int haveBrake; + int brakeIO; friend class PhytronController; };