Merge pull request #93 from softwareschneiderei/oms_encoder_sync

OMS: Synchronize motor position with encoder position before every move.
epics-modules · Jun 21, 2018 · 31e0afe · 31e0afe
2 parents 6529abb + d061e91
commit 31e0afe
Show file tree

Hide file tree

Showing 2 changed files with 38 additions and 8 deletions.
diff --git a/motorApp/OmsAsynSrc/omsBaseAxis.cpp b/motorApp/OmsAsynSrc/omsBaseAxis.cpp
@@ -28,6 +28,7 @@ omsBaseAxis::omsBaseAxis(omsBaseController *pController, int axis, char axisChar
     stepper = 1;
     invertLimit = 0;
     lastminvelo = 0;
+    encoderRatio = 1.0;
 }
 
 asynStatus omsBaseAxis::move(double position, int relative, double min_velocity, double max_velocity, double acceleration)
@@ -36,9 +37,36 @@ asynStatus omsBaseAxis::move(double position, int relative, double min_velocity,
     static const char *functionName = "moveAxis";
 
     asynStatus status = asynError;
+
+    char buff[100];
+    char encoderPositionResponse[64];
+
+    int hasEncoder = 0;
+    if (pC_->getIntegerParam(axisNo_, pC_->motorStatusHasEncoder_, &hasEncoder) == asynError)
+    {
+        return asynError;
+    }
+
+    if (hasEncoder != 0)
+    {
+        sprintf(buff, "A%1c;RE;", axisChar);
+        status = pC_->sendReceiveLock(buff, encoderPositionResponse, 64);
+        if (status == asynError)
+        {
+            return status;
+        }
+
+        int motorPosition = static_cast<int>(atoi(encoderPositionResponse) / encoderRatio);
+        asynPrint(pasynUser_, ASYN_TRACE_FLOW,
+            "%s:%s: Set driver %s motor position %d from encoder position %s\n",
+            driverName, functionName, pC_->portName, motorPosition, encoderPositionResponse);
+
+        sprintf(buff, "A%1c;LO%d;", axisChar, motorPosition);
+        status = pC_->sendOnlyLock(buff);
+    }
+
     epicsInt32 minvelo, velo, acc, rela, pos;
     char *relabs[2] = {(char *) "MA", (char *) "MR"};
-    char buff[100];
 
     if (relative)
         rela = 1;
@@ -81,7 +109,7 @@ asynStatus omsBaseAxis::move(double position, int relative, double min_velocity,
     status = pC_->sendOnlyLock(buff);
 
     asynPrint(pasynUser_, ASYN_TRACE_FLOW,
-        "%s:%s: Set driver %s, axis %d move to %f, min vel=%f, max_vel=%f, accel=%f",
+        "%s:%s: Set driver %s, axis %d move to %f, min vel=%f, max_vel=%f, accel=%f\n",
         driverName, functionName, pC_->portName, axisNo_, position, min_velocity, max_velocity, acceleration );
 
     return status;
@@ -124,7 +152,7 @@ asynStatus omsBaseAxis::home(double min_velocity, double max_velocity, double ac
     homing = 1;
 
     asynPrint(pasynUser_, ASYN_TRACE_FLOW,
-        "%s:%s: Set driver %s, axis %d to home %s, min vel=%f, max_vel=%f, accel=%f",
+        "%s:%s: Set driver %s, axis %d to home %s, min vel=%f, max_vel=%f, accel=%f\n",
         driverName, functionName, pC_->portName, axisNo_, (forwards?"FORWARDS":"REVERSE"), min_velocity, max_velocity, acceleration );
 
     return status;

diff --git a/motorApp/OmsAsynSrc/omsBaseAxis.h b/motorApp/OmsAsynSrc/omsBaseAxis.h
@@ -29,11 +29,12 @@ class omsBaseAxis : public asynMotorAxis
     virtual asynStatus setPosition(double position);
     virtual asynStatus poll(bool *moving);
 
-    int getAxis(){return axisNo_;};
-    int isStepper(){return stepper;};
-    void setStepper(int val){stepper=val;};
-    int getLimitInvert(){return invertLimit;};
-    void setLimitInvert(int val){invertLimit=val;};
+    int getAxis(){return axisNo_;}
+    int isStepper(){return stepper;}
+    void setStepper(int val){stepper=val;}
+    int getLimitInvert(){return invertLimit;}
+    void setLimitInvert(int val){invertLimit=val;}
+    virtual asynStatus setEncoderRatio(double ratio){encoderRatio=ratio; return asynSuccess;}
     int card;
     int moveDelay;
     char axisChar;
@@ -45,6 +46,7 @@ class omsBaseAxis : public asynMotorAxis
     int stepper;
     int invertLimit;
     epicsInt32 lastminvelo;
+    double encoderRatio;
 
 friend class omsBaseController;
 };