Added fucking brake support to Phytron

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" {

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;
 };