Merge pull request #25 from anderssandstrom/master

Some fixes

RELEASE.md

@@ -1,5 +1,11 @@
 Release Notes
 ===
+
+# ECMC 6.2.2
+* Allow JVEL change while jogging
+* Smoother trajectory at stopping (start stop seq. one cycle earlier than before to avoid small jerk at stop)
+* Update to support autosave of motor position
+
 # ECMC 6.2.1
 * Add exprtk support for plc functions with stings as args  (need ess-exprtk 1.2.1)
 * Add possabilty ti use plc-functions with strings a sarguments in plugins

devEcmcSup/motion/ecmcAxisBase.cpp

@@ -1377,6 +1377,14 @@ int ecmcAxisBase::moveVelocity(
     return errorCode;
   }
 
+  // check if already moveVelo then just update vel and acc
+  if(getExecute() && getCommand() == ECMC_CMD_MOVEVEL && getBusy()) {
+    getSeq()->setTargetVel(velocitySet);
+    getTraj()->setAcc(accelerationSet);
+    getTraj()->setDec(decelerationSet);
+    return 0;
+  }
+
   errorCode = setExecute(0);
   if (errorCode) {
     return errorCode;
@@ -1442,6 +1450,37 @@ int ecmcAxisBase::moveHome(int    nCmdData,
   return 0;
 }
 
+int ecmcAxisBase::setPosition(double homePositionSet) {
+
+  int errorCode = getErrorID();
+  if (errorCode) {
+    return errorCode;
+  }
+
+  errorCode = setExecute(0);
+  if (errorCode) {
+    return errorCode;
+  }
+
+  errorCode = setCommand(ECMC_CMD_HOMING);
+  if (errorCode) {
+    return errorCode;
+  }
+  errorCode = setCmdData(ECMC_SEQ_HOME_SET_POS);
+
+  if (errorCode) {
+    return errorCode;
+  }
+  getSeq()->setHomePosition(homePositionSet);
+  errorCode = setExecute(1);
+
+  if (errorCode) {
+    return errorCode;
+  }
+  return 0;
+}
+
+
 int ecmcAxisBase::stopMotion(int killAmplifier) {
 
   int errorCode = setExecute(0);

devEcmcSup/motion/ecmcAxisBase.h

@@ -234,6 +234,7 @@ class ecmcAxisBase : public ecmcError {
                                  double velocityOffCamSet,                            
                                  double accelerationSet,
                                  double decelerationSet);
+  int                   setPosition(double homePositionSet);  // Autosave
   int                   stopMotion(int killAmplifier);
 
  protected:

devEcmcSup/motion/ecmcAxisSequencer.cpp

@@ -241,6 +241,7 @@ void ecmcAxisSequencer::execute() {
 }
 
 int ecmcAxisSequencer::setExecute(bool execute) {
+
   int errorCode=0;
   if (traj_ == NULL) {
     return setErrorID(__FILE__, __FUNCTION__, __LINE__, ERROR_SEQ_TRAJ_NULL);
@@ -521,6 +522,7 @@ double ecmcAxisSequencer::getTargetPos() {
 
 void ecmcAxisSequencer::setTargetVel(double velTarget) {
   data_->command_.velocityTarget = velTarget;
+  traj_->setTargetVel(velTarget);
 }
 
 double ecmcAxisSequencer::getTargetVel() {
@@ -2100,7 +2102,7 @@ int ecmcAxisSequencer::setAxisDataRef(ecmcAxisData *data) {
 }
 
 int ecmcAxisSequencer::checkVelAccDec() {
-  if (data_->command_.command == ECMC_CMD_HOMING) {
+  if (data_->command_.command == ECMC_CMD_HOMING && data_->command_.cmdData != ECMC_SEQ_HOME_SET_POS) {
     if ((std::abs(homeVelTwordsCam_) == 0) ||
         (std::abs(homeVelOffCam_) == 0)) {
       return setErrorID(__FILE__,
@@ -2123,15 +2125,15 @@ int ecmcAxisSequencer::checkVelAccDec() {
   }
 
   // Sanity check of acceleration
-  if (traj_->getAcc() <= 0) {
+  if (traj_->getAcc() <= 0 && data_->command_.cmdData != ECMC_SEQ_HOME_SET_POS) {
     return setErrorID(__FILE__,
                       __FUNCTION__,
                       __LINE__,
                       ERROR_SEQ_ERROR_ACCELERATION_ZERO);
   }
 
   // Sanity check of deceleration
-  if (traj_->getDec() <= 0) {
+  if (traj_->getDec() <= 0 && data_->command_.cmdData != ECMC_SEQ_HOME_SET_POS) {
     return setErrorID(__FILE__,
                       __FUNCTION__,
                       __LINE__,

devEcmcSup/motion/ecmcMotion.cpp

@@ -147,6 +147,22 @@ LOGINFO4(
                                    decelerationSet);
 }
 
+int setPosition(int    axisIndex,
+                double homePositionSet) {
+LOGINFO4(
+    "%s/%s:%d axisIndex=%d,homePositionSet=%lf\n",
+    __FILE__,
+    __FUNCTION__,
+    __LINE__,
+    axisIndex,    
+    homePositionSet);
+
+  CHECK_AXIS_RETURN_IF_ERROR_AND_BLOCK_COM(axisIndex);
+  CHECK_AXIS_SEQ_RETURN_IF_ERROR(axisIndex);
+  CHECK_AXIS_TRAJ_RETURN_IF_ERROR(axisIndex);
+
+  return axes[axisIndex]->setPosition(homePositionSet);
+}
 
 int stopMotion(int axisIndex, int killAmplifier) {
   LOGINFO4("%s/%s:%d axisIndex=%d, killAmplifier=%d\n",

devEcmcSup/motion/ecmcMotion.h

@@ -186,6 +186,18 @@ int moveHome(int    axisIndex,
              double accelerationSet,
              double decelerationSet);
 
+/** \brief set position.\n
+ *  used by epics autosave to restore axis value after ioc reboot.
+ * 
+ * \param[in] axisIndex Axis index.\n
+ * \param[in] homePositionSet Position to set at homing.\n
+
+ *
+ * \return 0 if success or otherwise an error code.\n
+ */
+int setPosition(int    axisIndex,
+                double homePositionSet);
+
 /** \brief Get axis error state.\n
  *
  * \param[in] axisIndex Axis index.\n

devEcmcSup/motion/ecmcTrajectoryTrapetz.cpp

@@ -212,14 +212,21 @@ double ecmcTrajectoryTrapetz::moveVel(double currSetpoint,
 
   *trajBusy = true;
 
-  if (std::abs(currVelo) < std::abs(targetVelo)) {
-    positionStep = std::abs(prevStepSize_) + stepACC_;    
+  if(std::abs(prevStepSize_) > stepNOM_) {
+    positionStep = std::abs(prevStepSize_) - stepDEC_;
+    if(positionStep < stepNOM_) {      
+      positionStep = stepNOM_;
+    }
   } else {
-    positionStep = stepNOM_;
-  }
-
-  if(positionStep > stepNOM_) {
-    positionStep = stepNOM_;
+    if(std::abs(prevStepSize_) < stepNOM_) {
+      positionStep = std::abs(prevStepSize_) + stepACC_;
+      if(positionStep > stepNOM_) {        
+        positionStep = stepNOM_;
+      }
+    }
+    else {
+      positionStep = stepNOM_;
+    }
   }
 
   if (setDirection_ == ECMC_DIR_FORWARD) {
@@ -240,24 +247,20 @@ double ecmcTrajectoryTrapetz::movePos(double currSetpoint,
   double positionStep = 0;
   double posSetTemp   = 0;
   bool   stopping   = false;
-  //bool   changeDir    = false;
 
   *trajBusy = true;
-  /*changeDir =
-    ((targetSetpoint - currSetpoint) * currVelo < 0 && std::abs(currVelo)) > 0;*/
 
   double distToTargetOld = dist(currSetpoint,targetSetpoint,setDirection_);
 
-  stopping = stopDistance > std::abs(distToTargetOld); /*||
-               changeDir;*/
+  stopping = stopDistance  >= std::abs(distToTargetOld) - std::abs(prevStepSize_); // compensate for this motion step
 
   if (!stopping) {
     if (std::abs(currVelo) < std::abs(targetVelo)) {
       positionStep = std::abs(prevStepSize_) + stepACC_;
     } else {
       positionStep = stepNOM_;
     }
-  } else {
+  } else {  //Stopping
     positionStep = std::abs(prevStepSize_) - stepDEC_;
   }
 
@@ -354,16 +357,6 @@ int ecmcTrajectoryTrapetz::getIndex() {
 }
 
 double ecmcTrajectoryTrapetz::getVel() {
-  double absVelTarget = std::abs(velocityTarget_);
-
-  if ((velocity_ >= absVelTarget) && (setDirection_ == ECMC_DIR_FORWARD)) {
-    return absVelTarget;
-  }
-
-  if ((velocity_ <= -absVelTarget) && (setDirection_ == ECMC_DIR_BACKWARD)) {
-    return -absVelTarget;
-  }
-
   return velocity_;
 }
 
@@ -659,4 +652,4 @@ double ecmcTrajectoryTrapetz::checkModuloPos(double pos,
   }
 
   return posSetTemp;
-}
+}

devEcmcSup/motor/ecmcMotorRecordAxis.cpp

@@ -675,25 +675,10 @@ asynStatus ecmcMotorRecordAxis::setPosition(double value)
 
   drvlocal.eeAxisWarning = eeAxisWarningNoWarning;
 
-  // Read from records / params
-  double homPos    = 0.0; /* The homPos may be undefined, then use 0.0 */
-
-  // Home proc should always be 15 for this function (just set value)
-  //int    cmdData   = -1; 
-  // nCmdData (sequence number) Must be "manual cmddata (=15)"
-  // asynStatus status = pC_->getIntegerParam(axisNo_, pC_->ecmcMotorRecordHomProc_,&cmdData);
-  // if (cmdData != ECMC_SEQ_HOME_SET_POS || status != asynSuccess) {
-  //   return asynError;
-  // }
-
-  // Home position
-  (void)pC_->getDoubleParam(axisNo_, pC_->ecmcMotorRecordHomPos_, &homPos);
-
-  // always use home sequence 15 for setPosition
   if(ecmcRTMutex) epicsMutexLock(ecmcRTMutex);
-  int errorCode =  drvlocal.ecmcAxis->moveHome(ECMC_SEQ_HOME_SET_POS,
-                                               homPos,0,0,0,0);
+  int errorCode =  drvlocal.ecmcAxis->setPosition(value);
   if(ecmcRTMutex) epicsMutexUnlock(ecmcRTMutex);
+
   return errorCode == 0 ? asynSuccess:asynError;
 }