From defcc66c3396d5ed01f401e969ce613a28ddd929 Mon Sep 17 00:00:00 2001
From: Ron Sluiter <rsluiter@users.noreply.github.com>
Date: Wed, 2 Apr 2008 19:51:03 +0000
Subject: [PATCH] - asyn motor GET_INFO command support. - SET_POS support. -
 initial communication retries.

---
 motorApp/NewportSrc/drvMM4000Asyn.c | 37 ++++++++++++++++++++++++++---
 1 file changed, 34 insertions(+), 3 deletions(-)

diff --git a/motorApp/NewportSrc/drvMM4000Asyn.c b/motorApp/NewportSrc/drvMM4000Asyn.c
index 8db0b06d..fe8a34ee 100644
--- a/motorApp/NewportSrc/drvMM4000Asyn.c
+++ b/motorApp/NewportSrc/drvMM4000Asyn.c
@@ -40,7 +40,8 @@ motorAxisDrvSET_t motorMM4000 =
     motorAxisHome,              /**< Pointer to function to execute a more to reference or home */
     motorAxisMove,              /**< Pointer to function to execute a position move */
     motorAxisVelocityMove,      /**< Pointer to function to execute a velocity mode move */
-    motorAxisStop               /**< Pointer to function to stop motion */
+    motorAxisStop,              /**< Pointer to function to stop motion */
+    motorAxisforceCallback      /**< Pointer to function to request a poller status update */
   };
 
 epicsExportAddress(drvet, motorMM4000);
@@ -248,7 +249,10 @@ static int motorAxisSetDouble(AXIS_HDL pAxis, motorAxisParam_t function, double
         {
         case motorAxisPosition:
         {
-            PRINT(pAxis->logParam, ERROR, "motorAxisSetDouble: MM4000 does not support setting position\n");
+            deviceValue = value*pAxis->stepSize;
+            sprintf(buff, "%dSH%.*f;%dDH;%dSH%.*f", pAxis->axis+1, pAxis->maxDigits, deviceValue,
+                    pAxis->axis+1, pAxis->axis+1,  pAxis->maxDigits, pAxis->homePreset);
+            ret_status = sendOnly(pAxis->pController, buff);
             break;
         }
         case motorAxisEncoderRatio:
@@ -430,6 +434,22 @@ static int motorAxisStop(AXIS_HDL pAxis, double acceleration)
     return MOTOR_AXIS_OK;
 }
 
+static int motorAxisforceCallback(AXIS_HDL pAxis)
+{
+    if (pAxis == NULL)
+        return (MOTOR_AXIS_ERROR);
+
+    PRINT(pAxis->logParam, FLOW, "motorAxisforceCallback: request card %d, axis %d status update\n",
+          pAxis->card, pAxis->axis);
+    
+    /* Force a status update. */
+    motorParam->forceCallback(pAxis->params);
+
+    /* Send a signal to the poller task which will make it do a status update */
+    epicsEventSignal(pAxis->pController->pollEventId);
+    return (MOTOR_AXIS_OK);
+}
+
 
 static void MM4000Poller(MM4000Controller *pController)
 {
@@ -585,6 +605,8 @@ int MM4000AsynConfig(int card,             /* Controller number */
     int loopState;
     int digits;
     int modelNum;
+    int retry = 0;
+    asynStatus rtnval;
     char *p, *tokSave;
     char inputBuff[BUFFER_SIZE];
     char outputBuff[BUFFER_SIZE];
@@ -615,7 +637,16 @@ int MM4000AsynConfig(int card,             /* Controller number */
         return MOTOR_AXIS_ERROR;
     }
 
-    sendAndReceive(pController, "VE;", inputBuff, sizeof(inputBuff));
+    do
+    {
+        rtnval = sendAndReceive(pController, "VE;", inputBuff, sizeof(inputBuff));
+        retry++;
+        /* Return value is length of response string */
+    } while (rtnval != asynSuccess && retry < 3);
+
+    if (status != asynSuccess)
+        return (MOTOR_AXIS_ERROR);
+
     strcpy(pController->firmwareVersion, &inputBuff[2]);  /* Skip "VE" */
 
     /* Set Motion Master model indicator. */