Re: I implemented the Model 3 of EPICS motor driver support, but the VAL and RBV of motor record are zero after IOC reboot.

[LiangChih Chiang via Tech-talk <tech-talk at aps.anl.gov>]

Hi, thanks for replying.

> without having seen yourcode,

I attached the source code.

For the Kohzu ARIES/LYNX motor controller.

> connected via a serial cable, USB or Ethernet?

Ethernet.

>To explain a little more, it is important that the driver has completed the first poll cycle for the position and status of each axis before iocInit.  

>This is because the motor record looks at those positions during iocInit, and if there are non-zero it uses those values as the current position.  

>You can add the following to your startup script after you create the controller and axes to sleep for 1 second.
>epicsThreadSleep(1.0)

Thanks a lot. Hope this will fix the issue.

It may not be possible to try because it's beam time in my light source facility.

On Wed, Nov 20, 2024 at 11:04 PM Mark Rivers <rivers at cars.uchicago.edu> wrote:

To explain a little more, it is important that the driver has completed the first poll cycle for the position and status of each axis before iocInit.  This is because the motor record looks at those positions during iocInit, and if there are non-zero it uses those values as the current position.  You can add the following to your startup script after you create the controller and axes to sleep for 1 second.

epicsThreadSleep(1.0)

Let us know if this does not fix the problem.

Mark

---

From: Tech-talk <tech-talk-bounces at aps.anl.gov> on behalf of Torsten Bögershausen via Tech-talk <tech-talk at aps.anl.gov>
Sent: Wednesday, November 20, 2024 5:15 AM
To: LiangChih Chiang <yehnan174 at gmail.com>; tech-talk at aps.anl.gov <tech-talk at aps.anl.gov>
Subject: Re: I implemented the Model 3 of EPICS motor driver support, but the VAL and RBV of motor record are zero after IOC reboot.

 

Hej,

without having seen ypur code, I would speculate that you run
into a not-unknown issue using Model 3 motors, connected via
a serial cable, USB or Ethernet?

The value that the motorRecord needs is read from the driver,
wich must have been able to read it from your hardware.
Here are ESS we have a patched motor, which can handle this.
If you use the upstream version, the easist workaround may be
to add a sleep() after the createController() line in your st.cmd

HTH
/Torsten


On 2024-11-20 09:47, LiangChih Chiang via Tech-talk wrote:
> Hello, EPICS mates.
>
> I implemented the Model 3 of EPICS motor driver support for some motor
> controller,
> but the VAL and RBV of the motor record are zero after IOC reboot.
>
> I think the motor record doesn't restore the values of motor pulse and
> encoder count from the motor controller after IOC reboot.
>
> What member functions of asynMotorController and asynMotorAxis classes
> should I implement?
>
>
> Best regards.
>

/*
FILENAME... KohzuAriesDriver.cpp
USAGE...    Motor driver support for the Kohzu ARIES motor controller

LiangChih Chiang
December 20, 20181

*/

#include "KohzuAriesDriver.h"

#include <stdio.h>
#include <string.h>
#include <stdlib.h>

#include <iocsh.h>
#include <epicsThread.h>

#include <asynOctetSyncIO.h>

#include "asynMotorController.h"
#include "asynMotorAxis.h"

#include <epicsExport.h>

#define RSP_PREFIX_SUCCESS 'C'
#define RSP_PREFIX_WARNING 'W'
#define RSP_PREFIX_ERROR 'E'

#define RSP_DELIMITER "\t"

static const char *driverName = "KohzuAriesDriver";

/** Creates a new KohzuAriesController object.
  * \param[in] portName           The name of the asyn port that will be created for this driver
  * \param[in] KohzuAriesPortName The name of the drvAsynIPPPort that was created previously to connect to the KohzuAriesPortName controller
  * \param[in] numAxes            The number of axes that this controller supports
  * \param[in] movingPollPeriod   The time between polls when any axis is moving
  * \param[in] idlePollPeriod     The time between polls when no axis is moving
  */
KohzuAriesController::KohzuAriesController(const char *portName, const char *KohzuAriesPortName, int numAxes,
                             double movingPollPeriod, double idlePollPeriod)
  :  asynMotorController(portName, numAxes, NUM_KOHZUARIES_PARAMS,
		  	  	  	  	 0,
						 0,
                         ASYN_CANBLOCK | ASYN_MULTIDEVICE,
                         1, // autoconnect
                         0, 0)  // Default priority and stack size
{
  int axis;
  asynStatus status;
  KohzuAriesAxis *pAxis;
  static const char *functionName = "KohzuAriesController";

  createParam(KohzuAriesModelVersionString,         asynParamOctet,       &KohzuAriesModelVersion_);
  createParam(KohzuAriesAxisNumTotalString,         asynParamInt32,       &KohzuAriesAxisNumTotal_);
  createParam(KohzuAriesAxisNumControlledString,    asynParamInt32,       &KohzuAriesAxisNumControlled_);

  createParam(KohzuAriesDrivingStateString,         asynParamInt32,       &KohzuAriesDrivingState_);
  createParam(KohzuAriesEmgString,                  asynParamInt32,       &KohzuAriesEmg_);
  createParam(KohzuAriesOrgStateString,             asynParamInt32,       &KohzuAriesOrgState_);
  createParam(KohzuAriesNorgStateString,            asynParamInt32,       &KohzuAriesNorgState_);
  createParam(KohzuAriesSoftlimitStateposString,    asynParamInt32,       &KohzuAriesSoftlimitStatepos_);
  createParam(KohzuAriesSoftlimitStatenegString,    asynParamInt32,       &KohzuAriesSoftlimitStateneg_);
  createParam(KohzuAriesFeedbackStateString,        asynParamInt32,       &KohzuAriesFeedbackState_);

  createParam(KohzuAriesReleaseEmgString,           asynParamInt32,       &KohzuAriesReleaseEmg_);

  createParam(KohzuAriesOrgOffsetString,     asynParamInt32,       &KohzuAriesOrgOffset_);
  createParam(KohzuAriesOrgTypeString,       asynParamInt32,       &KohzuAriesOrgType_);
  createParam(KohzuAriesOrgSpeedString,      asynParamInt32,       &KohzuAriesOrgSpeed_);
  createParam(KohzuAriesPmPrescaleString,    asynParamInt32,       &KohzuAriesPmPrescale_);
  createParam(KohzuAriesPmRotateString,      asynParamInt32,       &KohzuAriesPmRotate_);
  createParam(KohzuAriesLimitSwapString,     asynParamInt32,       &KohzuAriesLimitSwap_);
  createParam(KohzuAriesPmClockString,       asynParamInt32,       &KohzuAriesPmClock_);
  createParam(KohzuAriesPmLogicString,       asynParamInt32,       &KohzuAriesPmLogic_);
  createParam(KohzuAriesBacklashPulseString, asynParamInt32,       &KohzuAriesBacklashPulse_);
  createParam(KohzuAriesBacklashTypeString,  asynParamInt32,       &KohzuAriesBacklashType_);
  createParam(KohzuAriesSoftlimitOnString,   asynParamInt32,       &KohzuAriesSoftlimitOn_);
  createParam(KohzuAriesSoftlimitPosString,  asynParamInt32,       &KohzuAriesSoftlimitPos_);
  createParam(KohzuAriesSoftlimitNegString,  asynParamInt32,       &KohzuAriesSoftlimitNeg_);
  createParam(KohzuAriesMaxSpeedString,      asynParamInt32,       &KohzuAriesMaxSpeed_);
  createParam(KohzuAriesLimitLogicString,    asynParamInt32,       &KohzuAriesLimitLogic_);
  createParam(KohzuAriesNorgLogicString,     asynParamInt32,       &KohzuAriesNorgLogic_);
  createParam(KohzuAriesOrgLogicString,      asynParamInt32,       &KohzuAriesOrgLogic_);
  createParam(KohzuAriesEncMulString,        asynParamInt32,       &KohzuAriesEncMul_);
  createParam(KohzuAriesEncPrescaleString,   asynParamInt32,       &KohzuAriesEncPrescale_);
  createParam(KohzuAriesEncNumString,        asynParamInt32,       &KohzuAriesEncNum_);
  createParam(KohzuAriesEncDenString,        asynParamInt32,       &KohzuAriesEncDen_);
  createParam(KohzuAriesEncDirString,        asynParamInt32,       &KohzuAriesEncDir_);
  createParam(KohzuAriesZphaseLogicString,   asynParamInt32,       &KohzuAriesZphaseLogic_);
  createParam(KohzuAriesEncSyncString,       asynParamInt32,       &KohzuAriesEncSync_);
  createParam(KohzuAriesEncFilterString,     asynParamInt32,       &KohzuAriesEncFilter_);
  createParam(KohzuAriesFeedbackTypeString,  asynParamInt32,       &KohzuAriesFeedbackType_);
  createParam(KohzuAriesPermitRangeString,   asynParamInt32,       &KohzuAriesPermitRange_);
  createParam(KohzuAriesRetryCountString,    asynParamInt32,       &KohzuAriesRetryCount_);
  createParam(KohzuAriesWaitTimeString,      asynParamInt32,       &KohzuAriesWaitTime_);
  createParam(KohzuAriesTriggerSouceString,    asynParamInt32,       &KohzuAriesTriggerSouce_);
  createParam(KohzuAriesTriggerEdgeString,     asynParamInt32,       &KohzuAriesTriggerEdge_);
  createParam(KohzuAriesTriggerPmratioString,  asynParamInt32,       &KohzuAriesTriggerPmratio_);
  createParam(KohzuAriesTriggerEncratioString, asynParamInt32,       &KohzuAriesTriggerEncratio_);
  createParam(KohzuAriesTriggerWidthString,    asynParamInt32,       &KohzuAriesTriggerWidth_);
  createParam(KohzuAriesTriggerLogicString,    asynParamInt32,       &KohzuAriesTriggerLogic_);
  createParam(KohzuAriesExcitationString,      asynParamInt32,       &KohzuAriesExcitation_);
  createParam(KohzuAriesMotorTypeString,       asynParamInt32,       &KohzuAriesMotorType_);
  createParam(KohzuAriesAlarmSignalString,     asynParamInt32,       &KohzuAriesAlarmSignal_);
  createParam(KohzuAriesMicrostepString,       asynParamInt32,       &KohzuAriesMicrostep_);
  createParam(KohzuAriesLimitStoptypeString,   asynParamInt32,       &KohzuAriesLimitStoptype_);

  /* Connect to Kohzu Aries controller */
  status = pasynOctetSyncIO->connect(KohzuAriesPortName, 0, &pasynUserController_, NULL);
  if (status) {
    asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, 
      "%s:%s: cannot connect to Kohzu Aries controller\n",
      driverName, functionName);
  }

  // Create the axis objects
  // todo: should check against MAX_KOHZUARIES_AXES
  for (axis = 0; axis < numAxes; axis++) {
    pAxis = new KohzuAriesAxis(this, axis);
  }

  startPoller(movingPollPeriod, idlePollPeriod, 2);
}

/** Creates a new KohzuAriesController object.
  * Configuration command, called directly or from iocsh
  * \param[in] portName           The name of the asyn port that will be created for this driver
  * \param[in] KohzuAriesPortName The name of the drvAsynIPPPort that was created previously to connect to the Kohzu Aries controller
  * \param[in] numAxes            The number of axes that this controller supports
  * \param[in] movingPollPeriod   The time in ms between polls when any axis is moving
  * \param[in] idlePollPeriod     The time in ms between polls when no axis is moving
  */
extern "C" int KohzuAriesCreateController(const char *portName, const char *KohzuAriesPortName, int numAxes,
                                   int movingPollPeriod, int idlePollPeriod)
{
  new KohzuAriesController(portName, KohzuAriesPortName, numAxes, movingPollPeriod/1000., idlePollPeriod/1000.);
  return(asynSuccess);
}

/** Reports on status of the driver
  * \param[in] fp The file pointer on which report information will be written
  * \param[in] level The level of report detail desired
  *
  * If details > 0 then information is printed about each axis.
  * After printing controller-specific information calls asynMotorController::report()
  */
void KohzuAriesController::report(FILE *fp, int level)
{
  fprintf(fp, "Kohzu Aries motor driver %s, numAxes=%d, moving poll period=%f, idle poll period=%f\n",
    this->portName, numAxes_, movingPollPeriod_, idlePollPeriod_);

  // Call the base class method
  asynMotorController::report(fp, level);
}

/** Returns a pointer to a KohzuAriesMotorAxis object.
  * Returns NULL if the axis number encoded in pasynUser is invalid.
  * \param[in] pasynUser asynUser structure that encodes the axis index number. */
KohzuAriesAxis* KohzuAriesController::getAxis(asynUser *pasynUser)
{
  return static_cast<KohzuAriesAxis*>(asynMotorController::getAxis(pasynUser));
}

/** Returns a pointer to a KohzuAriesAxis object.
  * Returns NULL if the axis number encoded in pasynUser is invalid.
  * \param[in] axisNo Axis index number. */
KohzuAriesAxis* KohzuAriesController::getAxis(int axisNo)
{
  return static_cast<KohzuAriesAxis*>(asynMotorController::getAxis(axisNo));
}

/* response data is separated by TAB('\t').
 * Using strtoke to replace TAB with NULL.
 * argument rsp: char array containing response data. Usually it's inString_.
 * rsp_segs will contain the pointer to the segments of the response data.
 * rsp_segs_num will be the number of the segments.
 * For example, response data is "C\tRSY1\t1\t567",
 *   rsp_segs[0] pointts "C",   rsp_segs[1] points to "RSY1",
 *   rsp_segs[2] points to "1", rsp_segs[3] points to "567"
 */
int KohzuAriesController::rsp_parse(char *rsp)
{
    char *token;
    int i = 0;

    token = strtok(rsp, RSP_DELIMITER);
    while(token != NULL && i < RSP_SEGS_MAX){
        rsp_segs[i++] = token;
       	token = strtok(NULL, RSP_DELIMITER);
    }
    rsp_segs_num = i;

    for(; i < RSP_SEGS_MAX; i++){
        rsp_segs[i] = NULL;
    }

    return rsp_segs_num;
}

// No. of System Settings
// return -1 if not found
int KohzuAriesController::function_to_sysno(int function)
{
	int sysno = -1;
	if(function == KohzuAriesOrgOffset_){
		sysno = 1;
	}
	else if(function == KohzuAriesOrgType_){
		sysno = 2;
	}
	else if(function == KohzuAriesOrgSpeed_){
		sysno = 3;
	}
	else if(function == KohzuAriesPmPrescale_){
		sysno = 6;
	}
	else if(function == KohzuAriesPmRotate_){
		sysno = 7;
	}
	else if(function == KohzuAriesLimitSwap_){
		sysno = 8;
	}
	else if(function == KohzuAriesPmClock_){
		sysno = 9;
	}
	else if(function == KohzuAriesPmLogic_){
		sysno = 10;
	}
	else if(function == KohzuAriesBacklashPulse_){
		sysno = 11;
	}
	else if(function == KohzuAriesBacklashType_){
		sysno = 12;
	}
	else if(function == KohzuAriesSoftlimitOn_){
		sysno = 13;
	}
	else if(function == KohzuAriesSoftlimitPos_){
		sysno = 14;
	}
	else if(function == KohzuAriesSoftlimitNeg_){
		sysno = 15;
	}
	else if(function == KohzuAriesMaxSpeed_){
		sysno = 16;
	}
	else if(function == KohzuAriesLimitLogic_){
		sysno = 21;
	}
	else if(function == KohzuAriesNorgLogic_){
		sysno = 22;
	}
	else if(function == KohzuAriesOrgLogic_){
		sysno = 23;
	}
	else if(function == KohzuAriesEncMul_){
		sysno = 31;
	}
	else if(function == KohzuAriesEncPrescale_){
		sysno = 32;
	}
	else if(function == KohzuAriesEncNum_){
		sysno = 33;
	}
	else if(function == KohzuAriesEncDen_){
		sysno = 34;
	}
	else if(function == KohzuAriesEncDir_){
		sysno = 35;
	}
	else if(function == KohzuAriesZphaseLogic_){
		sysno = 36;
	}
	else if(function == KohzuAriesEncSync_){
		sysno = 37;
	}
	else if(function == KohzuAriesEncFilter_){
		sysno = 38;
	}
	else if(function == KohzuAriesFeedbackType_){
		sysno = 41;
	}
	else if(function == KohzuAriesPermitRange_){
		sysno = 42;
	}
	else if(function == KohzuAriesRetryCount_){
		sysno = 43;
	}
	else if(function == KohzuAriesWaitTime_){
		sysno = 44;
	}
	else if(function == KohzuAriesTriggerSouce_){
		sysno = 51;
	}
	else if(function == KohzuAriesTriggerEdge_){
		sysno = 52;
	}
	else if(function == KohzuAriesTriggerPmratio_){
		sysno = 53;
	}
	else if(function == KohzuAriesTriggerEncratio_){
		sysno = 54;
	}
	else if(function == KohzuAriesTriggerWidth_){
		sysno = 55;
	}
	else if(function == KohzuAriesTriggerLogic_){
		sysno = 56;
	}
	else if(function == KohzuAriesExcitation_){
		sysno = 61;
	}
	else if(function == KohzuAriesMotorType_){
		sysno = 62;
	}
	else if(function == KohzuAriesAlarmSignal_){
		sysno = 63;
	}
	else if(function == KohzuAriesMicrostep_){
		sysno = 65;
	}
	else if(function == KohzuAriesLimitStoptype_){
		sysno = 99;
	}
	else{
	    sysno = -1;
	}

	return sysno;
}

asynStatus KohzuAriesController::readInt32(asynUser *pasynUser, epicsInt32 *value)
{
  int function = pasynUser->reason;
  asynStatus status = asynSuccess;
  KohzuAriesAxis *pAxis = getAxis(pasynUser);
  static const char *functionName = "readInt32";

  if (function == KohzuAriesAxisNumTotal_ || function == KohzuAriesAxisNumControlled_) {
	sprintf(outString_, "RAX");     // 0  1    2   3   4   5   6   7   8   9   10  12  segment index
	status = writeReadController(); // C\tRAX\t64\t32\tb0\tb1\tb2\tb3\tb4\tb5\tb6\tb7
	if(status != asynSuccess)
	  return status;
	rsp_parse(inString_);
	setIntegerParam(KohzuAriesAxisNumTotal_, atoi(rsp_segs[2]));
	setIntegerParam(KohzuAriesAxisNumControlled_, atoi(rsp_segs[3]));
  }
  else if(function_to_sysno(function) != -1){
	sprintf(outString_, "RSY%s/%d", pAxis->axisName_, function_to_sysno(function));
	status = writeReadController();                   // C\tRSY1\t1\t0
	                                                  // 0  1     2  3  segment index

    // test, start
	asynPrint(this->pasynUserSelf, ASYN_TRACE_WARNING,
	  "            zxy readInt32 axis %s, f %d, sysno %d, value %d, status %d, %s\n",
	  pAxis->axisName_, function, function_to_sysno(function), *value, status, inString_);
	// test, end


	if(status != asynSuccess)
	  return status;
	rsp_parse(inString_);
	pAxis->setIntegerParam(function, atoi(rsp_segs[3]));
  }

  /* Call base class method */
  status = asynMotorController::readInt32(pasynUser, value);

  /* Do callbacks so higher layers see any changes */
  callParamCallbacks(pAxis->axisNo_);

  return status;
}

asynStatus KohzuAriesController::writeInt32(asynUser *pasynUser, epicsInt32 value)
{
  int function = pasynUser->reason;
  asynStatus status = asynSuccess;
  KohzuAriesAxis *pAxis = getAxis(pasynUser);
  static const char *functionName = "writeInt32";
  int sysno;

  if(function_to_sysno(function) != -1){
	sprintf(outString_, "WSY%s/%d/%d", pAxis->axisName_, function_to_sysno(function), value);
	status = writeReadController();

    // test, start
	asynPrint(this->pasynUserSelf, ASYN_TRACE_WARNING,
	  "        xzy writeInt32 axis %s, f %d, sysno %d, value %d, status %d, %s\n",
	  pAxis->axisName_, function, function_to_sysno(function), value, status, inString_);
	// test, end

	if(status != asynSuccess)
	  return status;
  }
  else if(function == KohzuAriesReleaseEmg_){
	sprintf(outString_, "REM");
	status = writeReadController();
	if(status != asynSuccess)
	  return status;
  }
/* todo: how to sync? CNEN and Sys.62 excitation
  else if(function == motorClosedLoop_){
      asynPrint(pasynUser, ASYN_TRACEIO_DRIVER,
            "zzyyxx closedLoop %s:%s: function=%d, value=%d\n",
            driverName, functionName, function, value);
  }
*/

  /* Call base class method */
  status = asynMotorController::writeInt32(pasynUser, value);

  /* Do callbacks so higher layers see any changes */
  callParamCallbacks(pAxis->axisNo_);

  return status;
}

asynStatus KohzuAriesController::readOctet(asynUser *pasynUser,
                                           char *value, size_t maxChars, size_t *nActual,
                                           int *eomReason)
{
  int function = pasynUser->reason;
  asynStatus status = asynSuccess;
  KohzuAriesAxis *pAxis = getAxis(pasynUser);
  static const char *functionName = "readOctet";
  char temp[128]; // magic number

  if (function == KohzuAriesModelVersion_) {
	sprintf(outString_, "IDN");     // 0  1    2      3  4  5  segment index
	status = writeReadController(); // C\tIDN\tARIES\t1\t4\t3
	if(status != asynSuccess)
      return status;
	rsp_parse(inString_);
	sprintf(temp, "%s %s.%s.%s", rsp_segs[2], rsp_segs[3], rsp_segs[4], rsp_segs[5]);

	setStringParam(function, temp);
  }

  /* Call base class method */
  status = asynMotorController::readOctet(pasynUser, value, maxChars, nActual, eomReason);

  /* Do callbacks so higher layers see any changes */
  callParamCallbacks(pAxis->axisNo_);

  return status;
}


// These are the KohzuAriesAxis methods

/** Creates a new KohzuAriesAxis object.
  * \param[in] pC Pointer to the KohzuAriesController to which this axis belongs.
  * \param[in] axisNo Index number of this axis, range 0 to pC->numAxes_-1.
  *
  * Initializes register numbers, etc.
  */
KohzuAriesAxis::KohzuAriesAxis(KohzuAriesController *pC, int axisNo)
  : asynMotorAxis(pC, axisNo),
    pC_(pC)
{
  asynStatus status;

  sprintf(axisName_, "%d", axisNo+1); // note: base's axisNo_ is 0-based; axis no. in ARIES command is 1-based

  setIntegerParam(pC->motorStatusGainSupport_, 1);
  setIntegerParam(pC->motorStatusHasEncoder_, 1);
  callParamCallbacks();
}

/** Reports on status of the driver
  * \param[in] fp The file pointer on which report information will be written
  * \param[in] level The level of report detail desired
  *
  * If details > 0 then information is printed about each axis.
  * After printing controller-specific information calls asynMotorController::report()
  */
void KohzuAriesAxis::report(FILE *fp, int level)
{
  if (level > 0) {
    fprintf(fp, "  axis no.(0-based) %d\n"
            "    encoder position=%f\n"
            "    theory position=%f\n"
            "    limitCW=%d\n"
            "    limitCCW=%d\n"
            "    homed=%d\n",
            axisNo_,
            encoderPosition_, theoryPosition_,
			currentLimitCW, currentLimitCCW, currentHomed);
  }

  // Call the base class method
  asynMotorAxis::report(fp, level);
}

// speed table row 0 for move, 1 for jogging, 2 for homing
// unit of position, pulse
// unit of minVelocity and maxVelocity, pulse/second
// unit of acceleration, pulse/second^2,   = (maxV - minV) / ACCL

// note: VBAS -> minVelocity -> speedtable start speed, VELO -> maxVelocity -> speedtable max speed
// start speed must be <= (50% of max speed)
asynStatus KohzuAriesAxis::move(double position, int relative, double minVelocity, double maxVelocity, double acceleration)
{
  asynStatus status;
  int t;
  // static const char *functionName = "moveAxis";
  if(minVelocity >= maxVelocity / 2){
	  minVelocity = maxVelocity / 2 - 1;
  }
  t = (maxVelocity - minVelocity) / acceleration * 100; // unit: 10 msec

  sprintf(pC_->outString_, "WTB%s/0/%.0f/%.0f/%d/%d/2", axisName_, minVelocity, maxVelocity, t, t);
  status = pC_->writeReadController();

  // test, start
  asynPrint(pasynUser_, ASYN_TRACE_WARNING,
    "xzy axis %d move(pos %f, relative %d, minV %f, maxV %f, acc %f, t %d) return %d, %s\n",
	axisNo_, position, relative, minVelocity, maxVelocity, acceleration, t, status, pC_->inString_);
  // test, end

  sprintf(pC_->outString_, "%s%s/0/%.0f/1", relative ? "RPS" : "APS", axisName_, position);
  status = pC_->writeReadController();

  return status;
}

// jog
asynStatus KohzuAriesAxis::moveVelocity(double minVelocity, double maxVelocity, double acceleration)
{
  asynStatus status;
  int dir=0; // 0: CW, 1: CCW
  int t;
  // static const char *functionName = "moveVelocityAxis";

  if(maxVelocity < 0){
	  maxVelocity = -maxVelocity;
	  dir = 1;
  }
  if(minVelocity >= maxVelocity / 2){
	  minVelocity = maxVelocity / 2 - 1;
  }
  t = (maxVelocity - minVelocity) / acceleration * 100; // unit: 10 msec

  sprintf(pC_->outString_, "WTB%s/1/%.0f/%.0f/%d/%d/2", axisName_, minVelocity, maxVelocity, t, t);
  status = pC_->writeReadController();

  // test, start
  asynPrint(pasynUser_, ASYN_TRACE_WARNING,
    "xzy axis %d moveVelocity(minV %f, maxV %f, acc %f, t %d) return %d, %s\n",
	axisNo_, minVelocity, maxVelocity, acceleration, t, status, pC_->inString_);
  // test, end

  sprintf(pC_->outString_, "FRP%s/1/%d", axisName_, dir); // FRP axis no./speed table no./0:CW 1:CCW
  status = pC_->writeReadController();
  return status;
}

// forwards: 1 if HOMF, 0 if HOMR
asynStatus KohzuAriesAxis::home(double minVelocity, double maxVelocity, double acceleration, int forwards)
{
  asynStatus status;
  int t;
  int orgtype = -1;
  // static const char *functionName = "homeAxis";

  if(minVelocity >= maxVelocity / 2){
	  minVelocity = maxVelocity / 2 - 1;
  }
  t = (maxVelocity - minVelocity) / acceleration * 100; // unit: 10 msec

  status = pC_->getIntegerParam(axisNo_, pC_->KohzuAriesOrgType_, &orgtype);
  if(orgtype == 5 || orgtype == 7 || orgtype == 14){
    if(forwards != 1){
	  asynPrint(pasynUser_, ASYN_TRACE_WARNING,
	    "Warning: axis %d homing direction(forwards %d) is inconsistent with ORG type(%d)\n",
		axisNo_, forwards, orgtype);
	}
  }
  else if(orgtype == 6 || orgtype == 8 || orgtype == 15 || orgtype == 11){
    if(forwards != 0){
	  asynPrint(pasynUser_, ASYN_TRACE_WARNING,
	    "Warning: axis %d homing direction(forwards %d) is inconsistent with ORG type(%d)\n",
		axisNo_, forwards, orgtype);
	}
  }

  sprintf(pC_->outString_, "WTB%s/2/%.0f/%.0f/%d/%d/2", axisName_, minVelocity, maxVelocity, t, t);
  status = pC_->writeReadController();

  // test, start
  asynPrint(pasynUser_, ASYN_TRACE_WARNING,
    "xzy axis %d home(minV %f, maxV %f, acc %f, forwards %d, t %d, orgtype %d) return %d, %s\n",
	axisNo_, minVelocity, maxVelocity, acceleration, forwards, t, orgtype, status, pC_->inString_);
  // test, end

  sprintf(pC_->outString_, "ORG%s/2/1", axisName_);
  status = pC_->writeReadController();
  return status;
}

asynStatus KohzuAriesAxis::stop(double acceleration)
{
  asynStatus status;
  //static const char *functionName = "stopAxis";

  sprintf(pC_->outString_, "STP%s/0", axisName_); // 0: decelerate and stop; 1: emergency(no-decelerate) stop
  status = pC_->writeReadController();
  return status;
}

asynStatus KohzuAriesAxis::setPosition(double position)
{
  asynStatus status;
  double mres = 9.487;
  double enc_ratio;


  //pC_->getDoubleParam(axisNo_, pC_->motorResolution_, &mres);
  pC_->getDoubleParam(axisNo_, pC_->motorEncoderRatio_, &enc_ratio);
  // pC_->getDoubleParam(axisNo_, pC_->GalilEncoderResolution_, &eres);
  asynPrint(pasynUser_, ASYN_TRACE_WARNING,
    "xzy axis %d, mres %f, enc_ratio %f\n",
	axisNo_, mres, enc_ratio);

  setEncoderPosition(position * enc_ratio);

  sprintf(pC_->outString_, "WRP%s/%.0f", axisName_, position);
  status = pC_->writeReadController();

  // test, start
  asynPrint(pasynUser_, ASYN_TRACE_WARNING,
    "xzy axis %d setPosition (pos %f), WRP status %d\n",
	axisNo_, position, status);
  // test, end

  return status;
}

asynStatus KohzuAriesAxis::setEncoderPosition(double position)
{
  asynStatus status;

  sprintf(pC_->outString_, "WRE%s/%.0f", axisName_, position);
  status = pC_->writeReadController();

  // test, start
  asynPrint(pasynUser_, ASYN_TRACE_WARNING,
    "xzy axis %d setEncoderPosition(pos %f), WRE status %d, inString %s\n",
	axisNo_, position, status, pC_->inString_);
  // test, end

  //sprintf(pC_->outString_, "WRE%s/%.0f", axisName_, position);
  //status = pC_->writeReadController();

  //asynPrint(pasynUser_, ASYN_TRACE_WARNING,
  //  "xzy axis %d setEncoderPosition(pos %f), WRE status %d, inString %s\n",
  //  axisNo_, position, status, pC_->inString_);
  // test, end

  return status;
}

asynStatus KohzuAriesAxis::setClosedLoop(bool closedLoop)
{
  asynStatus status;

  // System Setting No. 61 excitation, used in setClosedLoop
  sprintf(pC_->outString_, "WSY%s/61/%d", axisName_, closedLoop ? 1 : 0);
  status = pC_->writeReadController();

  if(status != asynSuccess)
    return status;

  return status;
}

/** Polls the axis.
  * This function reads the controller position, encoder position, the limit status, the moving status, 
  * and the drive power-on status.  It does not current detect following error, etc. but this could be
  * added.
  * It calls setIntegerParam() and setDoubleParam() for each item that it polls,
  * and then calls callParamCallbacks() at the end.
  * \param[out] moving A flag that is set indicating that the axis is moving (1) or done (0). */
asynStatus KohzuAriesAxis::poll(bool *moving)
{ 
  int done;
  int driveOn;
  int limit;
  asynStatus comStatus;

  // Read the current encoder position
  sprintf(pC_->outString_, "RDE%s", axisName_);
  comStatus = pC_->writeReadController();
  if (comStatus) goto skip;
  encoderPosition_ = atof(pC_->inString_ + 7); // C[TAB]RDE1[TAB]123456
  setDoubleParam(pC_->motorEncoderPosition_, encoderPosition_);

  // Read the current theoretical position
  sprintf(pC_->outString_, "RDP%s", axisName_);
  comStatus = pC_->writeReadController();
  if (comStatus) goto skip;
  theoryPosition_ = atof(pC_->inString_ + 7); // C[TAB]RDE1[TAB]123456
  setDoubleParam(pC_->motorPosition_, theoryPosition_);

  // Read the current status, example: C\tSTR1\t0\t0\t0\t0\t0\t0
                                    // 0  2  5  7  9  11 13 15 17  index
  sprintf(pC_->outString_, "STR%s", axisName_);
  comStatus = pC_->writeReadController();
  if (comStatus) goto skip;
  done = pC_->inString_[7] == '0';
  setIntegerParam(pC_->motorStatusDone_, done);
  *moving = done ? false:true;

  setIntegerParam(pC_->KohzuAriesDrivingState_, pC_->inString_[7] - '0');

  setIntegerParam(pC_->KohzuAriesEmg_, pC_->inString_[9] - '0');

  setIntegerParam(pC_->KohzuAriesOrgState_,  (pC_->inString_[11] - '0') & 0x2);
  setIntegerParam(pC_->KohzuAriesNorgState_, (pC_->inString_[11] - '0') & 0x1);

  // the current limit status
  switch (pC_->inString_[13]) {
  case '0':
	  currentLimitCW = 0;
	  currentLimitCCW = 0;
	  break;
  case '1':
	  currentLimitCW = 0;
	  currentLimitCCW = 1;
	  break;
  case '2':
	  currentLimitCW = 1;
	  currentLimitCCW = 0;
	  break;
  case '3':
	  currentLimitCW = 1;
	  currentLimitCCW = 1;
	  break;
  default:
	  currentLimitCW = 0;
	  currentLimitCCW = 0;
	  break;
  }
  setIntegerParam(pC_->motorStatusHighLimit_, currentLimitCW);
  setIntegerParam(pC_->motorStatusLowLimit_, currentLimitCCW);

  setIntegerParam(pC_->KohzuAriesSoftlimitStatepos_,  pC_->inString_[15] == '1' ? 1 : 0);
  setIntegerParam(pC_->KohzuAriesSoftlimitStateneg_,  pC_->inString_[15] == '2' ? 1 : 0);

  setIntegerParam(pC_->KohzuAriesFeedbackState_,  pC_->inString_[17] == '1' ? 1 : 0);


  sprintf(pC_->outString_, "ROG%s", axisName_);
  comStatus = pC_->writeReadController();
  if (comStatus) goto skip;
  currentHomed = pC_->inString_[7] == '1' ? 1 : 0; // C[TAB]ROG1[TAB]1
  setIntegerParam(pC_->motorStatusHomed_, currentHomed);


  // Read the drive power on status
  // System Setting No. 61 excitation, also used in setClosedLoop
  sprintf(pC_->outString_, "RSY%s/61", axisName_);
  comStatus = pC_->writeReadController();
  if (comStatus) goto skip;
  driveOn = pC_->inString_[10] == '1' ? 1 : 0; // C[TAB]RSY1[TAB]61[TAB]1
  setIntegerParam(pC_->motorStatusPowerOn_, driveOn);

  setIntegerParam(pC_->motorStatusProblem_, 0);

  skip:
  setIntegerParam(pC_->motorStatusProblem_, comStatus ? 1:0);

  callParamCallbacks();
  return comStatus ? asynError : asynSuccess;
}


/** Code for iocsh registration */
static const iocshArg KohzuAriesCreateControllerArg0 = {"Port name", iocshArgString};
static const iocshArg KohzuAriesCreateControllerArg1 = {"Kohzu Aries port name", iocshArgString};
static const iocshArg KohzuAriesCreateControllerArg2 = {"Number of axes", iocshArgInt};
static const iocshArg KohzuAriesCreateControllerArg3 = {"Moving poll period (ms)", iocshArgInt};
static const iocshArg KohzuAriesCreateControllerArg4 = {"Idle poll period (ms)", iocshArgInt};
static const iocshArg * const KohzuAriesCreateControllerArgs[] = {&KohzuAriesCreateControllerArg0,
                                                                  &KohzuAriesCreateControllerArg1,
                                                                  &KohzuAriesCreateControllerArg2,
                                                                  &KohzuAriesCreateControllerArg3,
                                                                  &KohzuAriesCreateControllerArg4};
static const iocshFuncDef KohzuAriesCreateControllerDef = {"KohzuAriesCreateController", 5, KohzuAriesCreateControllerArgs};
static void KohzuAriesCreateContollerCallFunc(const iocshArgBuf *args)
{
  KohzuAriesCreateController(args[0].sval, args[1].sval, args[2].ival, args[3].ival, args[4].ival);
}

static void KohzuAriesRegister(void)
{
  iocshRegister(&KohzuAriesCreateControllerDef, KohzuAriesCreateContollerCallFunc);
}

extern "C" {
epicsExportRegistrar(KohzuAriesRegister);
}

/*
FILENAME...   KohzuAriesrDriver.h
USAGE...      Motor driver support for the Kohzu ARIES motor controller

LiangChih Chiang
December 20, 2018

*/

#include "asynMotorController.h"
#include "asynMotorAxis.h"

#define MAX_KOHZUARIES_AXES 32

// The number of segments of response data:
//   RTB, WTB commands: 10
//   RAX commands: 12 (max), example: C\tRAX\t64\t32\tb0\tb1\tb2\tb3\tb4\tb5\tb6\tb7
#define RSP_SEGS_MAX 12

#define KohzuAriesModelVersionString        "KOHZUARIES_MODEL_VERSION" //70
#define KohzuAriesAxisNumTotalString        "KOHZUARIES_AXISNUM_TOTAL"       // total number of devices
#define KohzuAriesAxisNumControlledString   "KOHZUARIES_AXISNUM_CONTROLLED"  // number of axes that can be controlled

#define KohzuAriesDrivingStateString        "KOHZUARIES_DRIVING_STATE"
#define KohzuAriesEmgString                 "KOHZUARIES_EMG"
#define KohzuAriesOrgStateString            "KOHZUARIES_ORG_STATE"
#define KohzuAriesNorgStateString           "KOHZUARIES_NORG_STATE"
#define KohzuAriesSoftlimitStateposString   "KOHZUARIES_SOFTLIMIT_STATEPOS"
#define KohzuAriesSoftlimitStatenegString   "KOHZUARIES_SOFTLIMIT_STATENEG"
#define KohzuAriesFeedbackStateString       "KOHZUARIES_FEEDBACK_STATE"

#define KohzuAriesReleaseEmgString          "KOHZUARIES_RELEASE_EMG"

// start, System settings
#define KohzuAriesOrgOffsetString           "KOHZUARIES_ORG_OFFSET"  //81
#define KohzuAriesOrgTypeString             "KOHZUARIES_ORG_TYPE"
#define KohzuAriesOrgSpeedString            "KOHZUARIES_ORG_SPEED"
#define KohzuAriesPmPrescaleString          "KOHZUARIES_PM_PRESCALE"
#define KohzuAriesPmRotateString            "KOHZUARIES_PM_ROTATE"
#define KohzuAriesLimitSwapString           "KOHZUARIES_LIMIT_SWAP"
#define KohzuAriesPmClockString             "KOHZUARIES_PM_CLOCK"
#define KohzuAriesPmLogicString             "KOHZUARIES_PM_LOGGIC"
#define KohzuAriesBacklashPulseString       "KOHZUARIES_BACKLASH_PULSE"
#define KohzuAriesBacklashTypeString        "KOHZUARIES_BACKLASH_TYPE"
#define KohzuAriesSoftlimitOnString         "KOHZUARIES_SOFTLIMIT_ON"
#define KohzuAriesSoftlimitPosString        "KOHZUARIES_SOFTLIMIT_POS"
#define KohzuAriesSoftlimitNegString        "KOHZUARIES_SOFTLIMIT_NEG"
#define KohzuAriesMaxSpeedString            "KOHZUARIES_MAX_SPEED"     //94
#define KohzuAriesLimitLogicString          "KOHZUARIES_LIMIT_LOGIC"
#define KohzuAriesNorgLogicString           "KOHZUARIES_NORG_LOGIC"
#define KohzuAriesOrgLogicString            "KOHZUARIES_ORG_LOGIC"
#define KohzuAriesEncMulString              "KOHZUARIES_ENC_MUL"
#define KohzuAriesEncPrescaleString         "KOHZUARIES_ENC_PRESCALE"
#define KohzuAriesEncNumString              "KOHZUARIES_ENC_NUM"     //100
#define KohzuAriesEncDenString              "KOHZUARIES_ENC_DEN"
#define KohzuAriesEncDirString              "KOHZUARIES_ENC_DIR"
#define KohzuAriesZphaseLogicString         "KOHZUARIES_ZPHASE_LOGIC"
#define KohzuAriesEncSyncString             "KOHZUARIES_ENC_SYNC"
#define KohzuAriesEncFilterString           "KOHZUARIES_ENC_FILTER"
#define KohzuAriesFeedbackTypeString        "KOHZUARIES_FEEDBACK_TYPE"
#define KohzuAriesPermitRangeString         "KOHZUARIES_PERMIT_RANGE"
#define KohzuAriesRetryCountString          "KOHZUARIES_RETRY_COUNT"
#define KohzuAriesWaitTimeString            "KOHZUARIES_WAIT_TIME"

#define KohzuAriesTriggerSouceString        "KOHZUARIES_TRIGGER_SOURCE"
#define KohzuAriesTriggerEdgeString         "KOHZUARIES_TRIGGER_EDGE"
#define KohzuAriesTriggerPmratioString      "KOHZUARIES_TRIGGER_PMRATIO"
#define KohzuAriesTriggerEncratioString     "KOHZUARIES_TRIGGER_ENCRATIO"
#define KohzuAriesTriggerWidthString        "KOHZUARIES_TRIGGER_WIDTH"
#define KohzuAriesTriggerLogicString        "KOHZUARIES_TRIGGER_LOGIC"

#define KohzuAriesExcitationString          "KOHZUARIES_EXCITATION"
#define KohzuAriesMotorTypeString           "KOHZUARIES_MOTOR_TYPE"
#define KohzuAriesAlarmSignalString         "KOHZUARIES_ALARM_SIGNAL"
#define KohzuAriesMicrostepString           "KOHZUARIES_MICROSTEP"

#define KohzuAriesLimitStoptypeString       "KOHZUARIES_LIMIT_STOPTYPE"
// end, System settings


class epicsShareClass KohzuAriesAxis : public asynMotorAxis
{
public:
  /* These are the methods we override from the base class */
  KohzuAriesAxis(class KohzuAriesController *pC, int axis);
  void report(FILE *fp, int level);
  asynStatus move(double position, int relative, double min_velocity, double max_velocity, double acceleration);
  asynStatus moveVelocity(double min_velocity, double max_velocity, double acceleration);
  asynStatus home(double min_velocity, double max_velocity, double acceleration, int forwards);
  asynStatus stop(double acceleration);
  asynStatus poll(bool *moving);
  asynStatus setPosition(double position);
  asynStatus setEncoderPosition(double position);
  asynStatus setClosedLoop(bool closedLoop);

private:
  KohzuAriesController *pC_;      /**< Pointer to the asynMotorController to which this axis belongs.
                                *   Abbreviated because it is used very frequently */

  char axisName_[3];      /**< Name of each axis, used in commands to Kohzu ARIES controller */

  double encoderPosition_; /**< Cached copy of the encoder position */ 
  double theoryPosition_;  /**< Cached copy of the theoretical position */
  int currentLimitCW;      /**< Cached copy of the current limits */
  int currentLimitCCW;
  int currentHomed;
  
friend class KohzuAriesController;
};


class epicsShareClass KohzuAriesController : public asynMotorController {
public:
  KohzuAriesController(const char *portName, const char *KohzuAriesPortName, int numAxes, double movingPollPeriod, double idlePollPeriod);

  /* These are the methods that we override from asynPortDriver */
  //asynStatus writeUInt32Digital(asynUser *pasynUser, epicsUInt32 value, epicsUInt32 mask);
  asynStatus readInt32(asynUser *pasynUser, epicsInt32 *value);
  asynStatus writeInt32(asynUser *pasynUser, epicsInt32 value);
  asynStatus readOctet(asynUser *pasynUser, char *value, size_t maxChars, size_t *nActual, int *eomReason);
  
  /* These are the methods that we override from asynMotorDriver */
  //asynStatus writeInt32(asynUser *pasynUser, epicsInt32 value);
  //asynStatus writeFloat64(asynUser *pasynUser, epicsFloat64 value);
  void report(FILE *fp, int level);
  KohzuAriesAxis* getAxis(asynUser *pasynUser);
  KohzuAriesAxis* getAxis(int axisNo);

protected:
#define FIRST_KOHZUARIES_PARAM KohzuAriesModelVersion_
  int KohzuAriesModelVersion_; /* example: ARIES 1.4.3 */
  int KohzuAriesAxisNumTotal_; /* total number of devices(axes?) */
  int KohzuAriesAxisNumControlled_; /* number of axes that can be controlled */
  int KohzuAriesDrivingState_;
  int KohzuAriesEmg_;
  int KohzuAriesOrgState_;
  int KohzuAriesNorgState_;
  int KohzuAriesSoftlimitStatepos_;
  int KohzuAriesSoftlimitStateneg_;
  int KohzuAriesFeedbackState_;
  int KohzuAriesReleaseEmg_;
  int KohzuAriesOrgOffset_; /* start, System settings */
  int KohzuAriesOrgType_;
  int KohzuAriesOrgSpeed_;
  int KohzuAriesPmPrescale_;
  int KohzuAriesPmRotate_;
  int KohzuAriesLimitSwap_;
  int KohzuAriesPmClock_;
  int KohzuAriesPmLogic_;
  int KohzuAriesBacklashPulse_;
  int KohzuAriesBacklashType_;
  int KohzuAriesSoftlimitOn_;
  int KohzuAriesSoftlimitPos_;
  int KohzuAriesSoftlimitNeg_;
  int KohzuAriesMaxSpeed_;
  int KohzuAriesLimitLogic_;
  int KohzuAriesNorgLogic_;
  int KohzuAriesOrgLogic_;
  int KohzuAriesEncMul_;
  int KohzuAriesEncPrescale_;
  int KohzuAriesEncNum_;
  int KohzuAriesEncDen_;
  int KohzuAriesEncDir_;
  int KohzuAriesZphaseLogic_;
  int KohzuAriesEncSync_;
  int KohzuAriesEncFilter_;
  int KohzuAriesFeedbackType_;
  int KohzuAriesPermitRange_;
  int KohzuAriesRetryCount_;
  int KohzuAriesWaitTime_;
  int KohzuAriesTriggerSouce_;
  int KohzuAriesTriggerEdge_;
  int KohzuAriesTriggerPmratio_;
  int KohzuAriesTriggerEncratio_;
  int KohzuAriesTriggerWidth_;
  int KohzuAriesTriggerLogic_;
  int KohzuAriesExcitation_;
  int KohzuAriesMotorType_;
  int KohzuAriesAlarmSignal_;
  int KohzuAriesMicrostep_;
  int KohzuAriesLimitStoptype_; /* end, System settings */
#define LAST_KOHZUARIES_PARAM KohzuAriesLimitStoptype_

#define NUM_KOHZUARIES_PARAMS (&LAST_KOHZUARIES_PARAM - &FIRST_KOHZUARIES_PARAM + 1)

private:
  char *rsp_segs[RSP_SEGS_MAX];
  int rsp_segs_num;
  int rsp_parse(char *rsp);

  int function_to_sysno(int function);

friend class KohzuAriesAxis;
};