- Fixed reference run for Phytron

- Added brake handling to phytron - Added code to set speed for phytron - Removed scaling on Selene pmac limits - Removed enabling on Selene pmac axis
2020-07-09 14:15:26 +02:00
parent d1d74f4db3
commit c9e7830274
5 changed files with 164 additions and 17 deletions
--- 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 ===========================================*/