From 1d91dab01afa6a2581c926c856e8b439e8ed339d Mon Sep 17 00:00:00 2001 From: Peter Denison Date: Thu, 22 May 2008 17:05:01 +0000 Subject: [PATCH] Several changes to drvXPSAsyn.c. Implemented setting position in motorAxisSetDouble (for both single XPS axes and XPS groups), implemented setting PID parameters in motorAxisSetDouble, implemented setting of following error status flag in the poller task, added two new IOC shell functions - one to disable the setting of position and one to change the epicsThreadSleep time used when setting position. There are some new wrapper functions for the XPS PID set/get functions (which are very verbose). Also implemented setting of motion direction in the poller task - this is done by looking at the axis velocity (there is also a new deadband #define for this). --- motorApp/NewportSrc/drvXPSAsyn.c | 712 ++++++++++++++++++++++++++++++- 1 file changed, 694 insertions(+), 18 deletions(-) diff --git a/motorApp/NewportSrc/drvXPSAsyn.c b/motorApp/NewportSrc/drvXPSAsyn.c index acaad763..cde62200 100644 --- a/motorApp/NewportSrc/drvXPSAsyn.c +++ b/motorApp/NewportSrc/drvXPSAsyn.c @@ -44,7 +44,8 @@ motorAxisDrvSET_t motorXPS = 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, motorXPS); @@ -63,6 +64,25 @@ typedef struct { AXIS_HDL pAxis; /* array of axes */ } XPSController; +/** Struct that contains information about the XPS corrector loop.*/ +typedef struct +{ + bool ClosedLoopStatus; + double KP; /**< Main proportional term from PID loop.*/ + double KI; /**< Main integral term from PID loop.*/ + double KD; /**< Main differential term from PID loop.*/ + double KS; + double IntegrationTime; + double DerivativeFilterCutOffFrequency; + double GKP; + double GKI; + double GKD; + double KForm; + double FeedForwardGainVelocity; + double FeedForwardGainAcceleration; + double Friction; +} xpsCorrectorInfo_t; + typedef struct motorAxisHandle { XPSController *pController; @@ -70,6 +90,7 @@ typedef struct motorAxisHandle int pollSocket; PARAMS params; double currentPosition; + double currentVelocity; double velocity; double accel; double minJerkTime; /* for the SGamma function */ @@ -87,6 +108,7 @@ typedef struct motorAxisHandle motorAxisLogFunc print; void *logParam; epicsMutexId mutexId; + xpsCorrectorInfo_t xpsCorrectorInfo; } motorAxis; typedef struct @@ -98,6 +120,36 @@ typedef struct epicsTimeStamp now; } motorXPS_t; +/** Struct for a list of strings describing the different corrector types possible on the XPS.*/ +typedef struct { + char *PIPosition; + char *PIDFFVelocity; + char *PIDFFAcceleration; + char *PIDDualFFVoltage; + char *NoCorrector; +} CorrectorTypes_t; + +const static CorrectorTypes_t CorrectorTypes={ + "PositionerCorrectorPIPosition", + "PositionerCorrectorPIDFFVelocity", + "PositionerCorrectorPIDFFAcceleration", + "PositionerCorrectorPIDDualFFVoltage", + "NoCorrector" +}; + +/** This is controlled via the XPSEnableSetPosition function (available via the IOC shell). */ +static int doSetPosition = 1; + +/** + * Parameter to control the sleep time used when setting position. + * A function called XPSSetPosSleepTime(int) (millisec parameter) + * is available in the IOC shell to control this. + */ +static double setPosSleepTime = 0.5; + +/** Deadband to use for the velocity comparison with zero. */ +#define XPS_VELOCITY_DEADBAND 0.0000001 + static int motorXPSLogMsg(void * param, const motorAxisLogMask_t logMask, const char *pFormat, ...); #define PRINT (pAxis->print) #define FLOW motorAxisTraceFlow @@ -119,6 +171,23 @@ static XPSController *pXPSController=NULL; static char* getXPSError(AXIS_HDL pAxis, int status, char *buffer); +/*Utility functions for dealing with XPS groups and setting corrector information.*/ +static int isAxisInGroup(const AXIS_HDL pAxis); +static int setXPSAxisPID(AXIS_HDL pAxis, const double * const value, int pidoption); +static int getXPSAxisPID(AXIS_HDL pAxis); +static void setXPSPIDValue(xpsCorrectorInfo_t *xpsCorrectorInfo, const double * const value, int pidoption); + +/*Wrapper functions for the verbose PositionerCorrector functions.*/ +static int PositionerCorrectorPIPositionGetWrapper(AXIS_HDL pAxis); +static int PositionerCorrectorPIDFFVelocityGetWrapper(AXIS_HDL pAxis); +static int PositionerCorrectorPIDFFAccelerationGetWrapper(AXIS_HDL pAxis); +static int PositionerCorrectorPIDDualFFVoltageGetWrapper(AXIS_HDL pAxis); + +static int PositionerCorrectorPIPositionSetWrapper(AXIS_HDL pAxis); +static int PositionerCorrectorPIDFFVelocitySetWrapper(AXIS_HDL pAxis); +static int PositionerCorrectorPIDFFAccelerationSetWrapper(AXIS_HDL pAxis); +static int PositionerCorrectorPIDDualFFVoltageSetWrapper(AXIS_HDL pAxis); + static void motorAxisReportAxis(AXIS_HDL pAxis, int level) { if (level > 0) @@ -145,6 +214,33 @@ static void motorAxisReport(int level) static int motorAxisInit(void) { + int controller = 0; + int axis = 0; + AXIS_HDL pAxis; + + for(controller=0; controllermutexId) break; + epicsMutexLock(pAxis->mutexId); + + /*Set GAIN_SUPPORT.*/ + motorParam->setInteger(pAxis->params, motorAxisHasClosedLoop, 1); + /*Readback PID and set in motor record.*/ + /*NOTE: this will require PID to be allowed to be set greater than 1 in motor record.*/ + /*And we need to implement this in Asyn layer.*/ + getXPSAxisPID(pAxis); + motorParam->setDouble(pAxis->params, motorAxisPGain, (pAxis->xpsCorrectorInfo).KP); + motorParam->setDouble(pAxis->params, motorAxisIGain, (pAxis->xpsCorrectorInfo).KI); + motorParam->setDouble(pAxis->params, motorAxisDGain, (pAxis->xpsCorrectorInfo).KD); + + motorParam->callCallback(pAxis->params); + epicsMutexUnlock(pAxis->mutexId); + + } + } + return MOTOR_AXIS_OK; } @@ -224,18 +320,120 @@ static int motorAxisSetCallback(AXIS_HDL pAxis, motorAxisCallbackFunc callback, static int motorAxisSetDouble(AXIS_HDL pAxis, motorAxisParam_t function, double value) { int ret_status = MOTOR_AXIS_ERROR; - int status; + int status = 0; + int axisIndex = 0; + int axisIndexInGrp = 0; + int axesInGroup = 0; double deviceValue; - + double positions[XPS_MAX_AXES] = {0.0}; + if (pAxis == NULL) return MOTOR_AXIS_ERROR; else { switch (function) { case motorAxisPosition: - { - PRINT(pAxis->logParam, ERROR, "motorAxisSetDouble[%d,%d]: XPS does not support setting position\n", pAxis->card, pAxis->axis); - break; + { + /*If the user has disabled setting the controller position, skip this.*/ + if (!doSetPosition) { + PRINT(pAxis->logParam, ERROR, "XPS set position is disabled. Enable it using XPSEnableSetPosition(1).\n"); + } else { + /*Test if this axis is in a XPS group.*/ + axesInGroup = isAxisInGroup(pAxis); + + if (axesInGroup>1) { + /*We are in a group, so we need to read the positions of all the axes in the group, + kill the group, and set all the positions in the group using referencing mode. + We read the positions seperately, rather than in one command, because we can't assume + that the ordering is the same in the XPS as in the driver.*/ + for (axisIndex=0; axisIndexpController->numAxes; axisIndex++) { + status = GroupPositionCurrentGet(pAxis->pollSocket, + pAxis->pController->pAxis[axisIndex].positionerName, + 1, + &positions[axisIndex]); + } + if (status != 0) { + PRINT(pAxis->logParam, ERROR, " Error performing GroupPositionCurrentGet(%d,%d). Aborting set position. XPS API Error: %d.\n", + pAxis->card, pAxis->axis, status); + } else { + status = GroupKill(pAxis->pollSocket, + pAxis->groupName); + status = GroupInitialize(pAxis->pollSocket, + pAxis->groupName); + if (status != 0) { + PRINT(pAxis->logParam, ERROR, " Error performing GroupKill/GroupInitialize(%d,%d). Aborting set position. XPS API Error: %d.\n", + pAxis->card, pAxis->axis, status); + } else { + + /*Wait after axis initialisation (we don't want to set position immediately after + initialisation because the stage can oscillate slightly).*/ + epicsThreadSleep(setPosSleepTime); + + status = GroupReferencingStart(pAxis->pollSocket, + pAxis->groupName); + axisIndexInGrp = 0; + /*Set positions for all axes in the group using the cached values.*/ + for (axisIndex=0; axisIndexpController->numAxes; axisIndex++) { + if (!strcmp(pAxis->groupName, pAxis->pController->pAxis[axisIndex].groupName)) { + /*But skip the current axis, because we do this just after the loop.*/ + if (strcmp(pAxis->positionerName, pAxis->pController->pAxis[axisIndex].positionerName)) { + status = GroupReferencingActionExecute(pAxis->pollSocket, + pAxis->pController->pAxis[axisIndex].positionerName, + "SetPosition", + "None", + positions[axisIndexInGrp]); + } + ++axisIndexInGrp; + } + } + /*Now reset the position of the axis we are interested in, using the argument passed into this function.*/ + status = GroupReferencingActionExecute(pAxis->pollSocket, + pAxis->positionerName, + "SetPosition", + "None", + value*(pAxis->stepSize)); + /*Stop referencing, then we are homed on all axes in group.*/ + status = GroupReferencingStop(pAxis->pollSocket, + pAxis->groupName); + if (status != 0) { + PRINT(pAxis->logParam, ERROR, " Error performing referencing set position (%d,%d). XPS API Error: %d.", + pAxis->card, pAxis->axis, status); + } + } + } + } else { + /*We are not in a group, so we just need to use the XPS + referencing mode to set the position.*/ + status = GroupKill(pAxis->pollSocket, + pAxis->groupName); + status = GroupInitialize(pAxis->pollSocket, + pAxis->groupName); + if (status != 0) { + PRINT(pAxis->logParam, ERROR, " Error performing GroupKill/GroupInitialize(%d,%d). XPS API Error: %d. Aborting set position.\n", + pAxis->card, pAxis->axis, status); + } else { + + /*Wait after axis initialisation (we don't want to set position immediately after + initialisation because the stage can oscillate slightly).*/ + epicsThreadSleep(setPosSleepTime); + + status = GroupReferencingStart(pAxis->pollSocket, + pAxis->groupName); + status = GroupReferencingActionExecute(pAxis->pollSocket, + pAxis->positionerName, + "SetPosition", + "None", + value*(pAxis->stepSize)); + status = GroupReferencingStop(pAxis->pollSocket, + pAxis->groupName); + if (status != 0) { + PRINT(pAxis->logParam, ERROR, " Error performing referencing set position (%d,%d). XPS API Error: %d.", + pAxis->card, pAxis->axis, status); + } + } + } + } + break; } case motorAxisEncoderRatio: { @@ -298,18 +496,18 @@ static int motorAxisSetDouble(AXIS_HDL pAxis, motorAxisParam_t function, double } case motorAxisPGain: { - PRINT(pAxis->logParam, ERROR, "XPS does not support setting proportional gain\n"); - break; + status = setXPSAxisPID(pAxis, &value, 0); + break; } case motorAxisIGain: { - PRINT(pAxis->logParam, ERROR, "XPS does not support setting integral gain\n"); - break; + status = setXPSAxisPID(pAxis, &value, 1); + break; } case motorAxisDGain: { - PRINT(pAxis->logParam, ERROR, "XPS does not support setting derivative gain\n"); - break; + status = setXPSAxisPID(pAxis, &value, 2); + break; } case motorAxisClosedLoop: { @@ -569,6 +767,23 @@ static int motorAxisStop(AXIS_HDL pAxis, double acceleration) return MOTOR_AXIS_OK; } + +/*Commented out for now, in case we don't need this.*/ +/*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); + + motorParam->forceCallback(pAxis->params); + + epicsEventSignal(pAxis->pController->pollEventId); + return (MOTOR_AXIS_OK); +}*/ + + static void XPSPoller(XPSController *pController) { @@ -657,6 +872,18 @@ static void XPSPoller(XPSController *pController) * motorParam->setInteger(pAxis->params, motorAxisLowHardLimit, 1); */ } + + /*Test for following error, and set appropriate param.*/ + if ((pAxis->axisStatus == 21 || pAxis->axisStatus == 22) || + (pAxis->axisStatus >= 24 && pAxis->axisStatus <= 26) || + (pAxis->axisStatus == 28 || pAxis->axisStatus == 35)) { + PRINT(pAxis->logParam, FLOW, "XPS Axis %d in following error. XPS State Code: %d\n", + pAxis->axis, pAxis->axisStatus); + motorParam->setInteger(pAxis->params, motorAxisFollowingError, 1); + } else { + motorParam->setInteger(pAxis->params, motorAxisFollowingError, 0); + } + } status = GroupPositionCurrentGet(pAxis->pollSocket, @@ -693,12 +920,20 @@ static void XPSPoller(XPSController *pController) } } - /* We would like a way to query the actual velocity, but this is not possible. If we could we could - * set the direction, and Moving flags - * motorParam->setInteger(pAxis->params, motorAxisDirection, (pAxis->currentVelocity > 0)); - * motorParam->setInteger(pAxis->params, motorAxisMoving, (pAxis->nextpoint.axis[0].v != 0)); - */ - + /*Read the current velocity and use it set motor direction and moving flag.*/ + status = GroupVelocityCurrentGet(pAxis->pollSocket, + pAxis->positionerName, + 1, + &pAxis->currentVelocity); + if (status != 0) { + PRINT(pAxis->logParam, ERROR, "XPSPoller: error calling GroupPositionVelocityGet[%d,%d], status=%d\n", pAxis->card, pAxis->axis, status); + motorParam->setInteger(pAxis->params, motorAxisCommError, 1); + } else { + motorParam->setInteger(pAxis->params, motorAxisCommError, 0); + motorParam->setInteger(pAxis->params, motorAxisDirection, (pAxis->currentVelocity > XPS_VELOCITY_DEADBAND)); + motorParam->setInteger(pAxis->params, motorAxisMoving, (fabs(pAxis->currentVelocity) > XPS_VELOCITY_DEADBAND)); + } + motorParam->callCallback(pAxis->params); epicsMutexUnlock(pAxis->mutexId); @@ -879,6 +1114,426 @@ int XPSConfigAxis(int card, /* specify which controller 0-up*/ return MOTOR_AXIS_OK; } +/** + * Function to enable/disable the write down of position to the + * XPS controller. Call this function at IOC shell. + * @param setPos 0=disable, 1=enable + */ +void XPSEnableSetPosition(int setPos) +{ + doSetPosition = setPos; +} + +/** + * Function to set the threadSleep time used when setting the XPS position. + * The sleep is performed after the axes are initialised, to take account of any + * post initialisation wobble. + * @param posSleep The time in miliseconds to sleep. + */ +void XPSSetPosSleepTime(int posSleep) +{ + setPosSleepTime = (double)posSleep / 1000.0; +} + + +/* Utility functions.*/ + +/** + * Test if axis is configured as an XPS single axis or a group. + * This is done by comparing cached group names. + * @param pAxis Axis struct AXIS_HDL + * @return 1 if in group single group, or return the number of axes in the group. + */ +static int isAxisInGroup(const AXIS_HDL pAxis) +{ + int axisIndex=0; + int group=0; + + for(axisIndex=0; axisIndexpController->numAxes; ++axisIndex) { + if (!strcmp(pAxis->groupName, pAxis->pController->pAxis[axisIndex].groupName)) { + ++group; + } + } + + return group; +} + + +/** + * Function to set the XPS controller PID parameters. + * @param pAxis Axis struct AXIS_HDL + * @param value The desired value of the parameter. + * @param pidoption Set to 0 for P, 1 for I and 2 for D. + * + * @return Zero if success, non-zero if error (and equal to XPS API error if error is from XPS). + */ +static int setXPSAxisPID(AXIS_HDL pAxis, const double * const value, int pidoption) +{ + int status = 0; + char correctorType[250] = {'\0'}; + + /*The XPS function that we use to set the PID parameters is dependant on the + type of corrector in use for that axis.*/ + status = PositionerCorrectorTypeGet(pAxis->pollSocket, + pAxis->positionerName, + correctorType); + if (status != 0) { + PRINT(pAxis->logParam, ERROR, "Error with PositionerCorrectorTypeGet. Card: %d, Axis: %d, XPS API Error: %d\n", + pAxis->card, pAxis->axis, status); + } else { + + if (!strcmp(correctorType, CorrectorTypes.PIPosition)) { + /*Read the PID parameters first.*/ + status = PositionerCorrectorPIPositionGetWrapper(pAxis); + if (status != 0) { + PRINT(pAxis->logParam, ERROR, "Error with PositionerCorrectorPIPositionGet. Aborting setting PID. XPS API Error: %d\n", status); + return status; + } + + /*Set the P, I or D parameter in the xpsCorrectorInfo struct.*/ + setXPSPIDValue(&pAxis->xpsCorrectorInfo, value, pidoption); + + /*Now set the parameters in the XPS.*/ + status = PositionerCorrectorPIPositionSetWrapper(pAxis); + if (status != 0) { + PRINT(pAxis->logParam, ERROR, "Error with PositionerCorrectorPIPositionSet. XPS API Error: %d\n", status); + return status; + } + + } else if (!strcmp(correctorType, CorrectorTypes.PIDFFVelocity)) { + status = PositionerCorrectorPIDFFVelocityGetWrapper(pAxis); + if (status != 0) { + PRINT(pAxis->logParam, ERROR, "Error with PositionerCorrectorPIDFFVelocityGet. Aborting setting PID. XPS API Error: %d\n", status); + return status; + } + + setXPSPIDValue(&pAxis->xpsCorrectorInfo, value, pidoption); + + status = PositionerCorrectorPIDFFVelocitySetWrapper(pAxis); + if (status != 0) { + PRINT(pAxis->logParam, ERROR, "Error with PositionerCorrectorPIDFFVelocitySet. XPS API Error: %d\n", status); + return status; + } + + } else if (!strcmp(correctorType, CorrectorTypes.PIDFFAcceleration)) { + status = PositionerCorrectorPIDFFAccelerationGetWrapper(pAxis); + if (status != 0) { + PRINT(pAxis->logParam, ERROR, "Error with PositionerCorrectorPIDFFAccelerationGet. Aborting setting PID. XPS API Error: %d\n", status); + return status; + } + + setXPSPIDValue(&pAxis->xpsCorrectorInfo, value, pidoption); + + status = PositionerCorrectorPIDFFAccelerationSetWrapper(pAxis); + if (status != 0) { + PRINT(pAxis->logParam, ERROR, "Error with PositionerCorrectorPIDFFAccelerationSet. XPS API Error: %d\n", status); + return status; + } + + } else if (!strcmp(correctorType, CorrectorTypes.PIDDualFFVoltage)) { + status = PositionerCorrectorPIDDualFFVoltageGetWrapper(pAxis); + if (status != 0) { + PRINT(pAxis->logParam, ERROR, "Error with PositionerCorrectorPIDDualFFVoltageGet. Aborting setting PID. XPS API Error: %d\n", status); + return status; + } + + setXPSPIDValue(&pAxis->xpsCorrectorInfo, value, pidoption); + + status = PositionerCorrectorPIDDualFFVoltageSetWrapper(pAxis); + if (status != 0) { + PRINT(pAxis->logParam, ERROR, "Error with PositionerCorrectorPIDDualFFVoltageSet. XPS API Error: %d\n", status); + return status; + } + + } else if (!strcmp(correctorType, CorrectorTypes.NoCorrector)) { + printf("drvXPSAsyn::setXPSAxisPID. XPS corrector type is %s. Cannot set PID.\n", correctorType); + + } else { + printf("ERROR: drvXPSAsyn::setXPSAxisPID. %s is not a valid corrector type. PID not set.\n", correctorType); + } + } + + return status; +} + +/** + * Function to read the PID values from the XPS (and any other XPS corrector info that is valid for the axis). + * The read values are set in the AXIS_HDL struct. + * @param pAxis Axis struct AXIS_HDL. + * @return Zero if success, non-zero if error (and equal to XPS API error if error is from XPS). + */ +static int getXPSAxisPID(AXIS_HDL pAxis) +{ + int status = 0; + char correctorType[250] = {'\0'}; + + /*The XPS function that we use to set the PID parameters is dependant on the + type of corrector in use for that axis.*/ + status = PositionerCorrectorTypeGet(pAxis->pollSocket, + pAxis->positionerName, + correctorType); + if (status != 0) { + PRINT(pAxis->logParam, ERROR, "Error with PositionerCorrectorTypeGet. Card: %d, Axis: %d, XPS API Error: %d\n", + pAxis->card, pAxis->axis, status); + } else { + + if (!strcmp(correctorType, CorrectorTypes.PIPosition)) { + /*Read the PID parameters and set in pAxis.*/ + status = PositionerCorrectorPIPositionGetWrapper(pAxis); + if (status != 0) { + PRINT(pAxis->logParam, ERROR, "Error with PositionerCorrectorPIPositionGet. XPS API Error: %d\n", status); + return status; + } + + } else if (!strcmp(correctorType, CorrectorTypes.PIDFFVelocity)) { + status = PositionerCorrectorPIDFFVelocityGetWrapper(pAxis); + if (status != 0) { + PRINT(pAxis->logParam, ERROR, "Error with PositionerCorrectorPIDFFVelocityGet. XPS API Error: %d\n", status); + return status; + } + + } else if (!strcmp(correctorType, CorrectorTypes.PIDFFAcceleration)) { + status = PositionerCorrectorPIDFFAccelerationGetWrapper(pAxis); + if (status != 0) { + PRINT(pAxis->logParam, ERROR, "Error with PositionerCorrectorPIDFFAccelerationGet. XPS API Error: %d\n", status); + return status; + } + + } else if (!strcmp(correctorType, CorrectorTypes.PIDDualFFVoltage)) { + status = PositionerCorrectorPIDDualFFVoltageGetWrapper(pAxis); + if (status != 0) { + PRINT(pAxis->logParam, ERROR, "Error with PositionerCorrectorPIDDualFFVoltageGet. XPS API Error: %d\n", status); + return status; + } + + } else if (!strcmp(correctorType, CorrectorTypes.NoCorrector)) { + printf("drvXPSAsyn::setXPSAxisPID. XPS corrector type is %s.\n", correctorType); + + } else { + printf("ERROR: drvXPSAsyn::setXPSAxisPID. %s is not a valid corrector type.\n", correctorType); + } + } + + return 0; +} + + +/** + * Set the P, I or D parameter in a xpsCorrectorInfo_t struct. + * @param xpsCorrectorInfo Pointer to a xpsCorrectorInfo_t struct. + * @param value The value to set. + * @param pidoption Set to 0 for P, 1 for I and 2 for D. + */ +static void setXPSPIDValue(xpsCorrectorInfo_t *xpsCorrectorInfo, const double * const value, int pidoption) +{ + if ((pidoption < 0) || (pidoption > 2)) { + printf("ERROR: drvXPSAsyn::setXPSPIDValue. pidoption out of range\n"); + } else { + switch (pidoption) { + case 0: + xpsCorrectorInfo->KP = *value; + break; + case 1: + xpsCorrectorInfo->KI = *value; + break; + case 2: + xpsCorrectorInfo->KD = *value; + break; + default: + /*Do nothing.*/ + break; + } + } +} + +/** + * Wrapper function for PositionerCorrectorPIPositionGet. + * It will set parameters in a AXIS_HDL struct. + * @param pAxis Axis struct AXIS_HDL. + * @return Return value from XPS function. + */ +static int PositionerCorrectorPIPositionGetWrapper(AXIS_HDL pAxis) +{ + xpsCorrectorInfo_t *xpsCorrectorInfo = &pAxis->xpsCorrectorInfo; + return PositionerCorrectorPIPositionGet(pAxis->pollSocket, + pAxis->positionerName, + &xpsCorrectorInfo->ClosedLoopStatus, + &xpsCorrectorInfo->KP, + &xpsCorrectorInfo->KI, + &xpsCorrectorInfo->IntegrationTime); +} + +/** + * Wrapper function for PositionerCorrectorPIDFFVelocityGet. + * It will set parameters in a AXIS_HDL struct. + * @param pAxis Axis struct AXIS_HDL. + * @return Return value from XPS function. + */ +static int PositionerCorrectorPIDFFVelocityGetWrapper(AXIS_HDL pAxis) +{ + xpsCorrectorInfo_t *xpsCorrectorInfo = &pAxis->xpsCorrectorInfo; + return PositionerCorrectorPIDFFVelocityGet(pAxis->pollSocket, + pAxis->positionerName, + &xpsCorrectorInfo->ClosedLoopStatus, + &xpsCorrectorInfo->KP, + &xpsCorrectorInfo->KI, + &xpsCorrectorInfo->KD, + &xpsCorrectorInfo->KS, + &xpsCorrectorInfo->IntegrationTime, + &xpsCorrectorInfo->DerivativeFilterCutOffFrequency, + &xpsCorrectorInfo->GKP, + &xpsCorrectorInfo->GKI, + &xpsCorrectorInfo->GKD, + &xpsCorrectorInfo->KForm, + &xpsCorrectorInfo->FeedForwardGainVelocity); +} + + +/** + * Wrapper function for PositionerCorrectorPIDFFAccelerationGet. + * It will set parameters in a AXIS_HDL struct. + * @param pAxis Axis struct AXIS_HDL. + * @return Return value from XPS function. + */ +static int PositionerCorrectorPIDFFAccelerationGetWrapper(AXIS_HDL pAxis) +{ + xpsCorrectorInfo_t *xpsCorrectorInfo = &pAxis->xpsCorrectorInfo; + return PositionerCorrectorPIDFFAccelerationGet(pAxis->pollSocket, + pAxis->positionerName, + &xpsCorrectorInfo->ClosedLoopStatus, + &xpsCorrectorInfo->KP, + &xpsCorrectorInfo->KI, + &xpsCorrectorInfo->KD, + &xpsCorrectorInfo->KS, + &xpsCorrectorInfo->IntegrationTime, + &xpsCorrectorInfo->DerivativeFilterCutOffFrequency, + &xpsCorrectorInfo->GKP, + &xpsCorrectorInfo->GKI, + &xpsCorrectorInfo->GKD, + &xpsCorrectorInfo->KForm, + &xpsCorrectorInfo->FeedForwardGainAcceleration); +} + + +/** + * Wrapper function for PositionerCorrectorPIDDualFFVoltageGet. + * It will set parameters in a AXIS_HDL struct. + * @param pAxis Axis struct AXIS_HDL. + * @return Return value from XPS function. + */ +static int PositionerCorrectorPIDDualFFVoltageGetWrapper(AXIS_HDL pAxis) +{ + xpsCorrectorInfo_t *xpsCorrectorInfo = &pAxis->xpsCorrectorInfo; + return PositionerCorrectorPIDDualFFVoltageGet(pAxis->pollSocket, + pAxis->positionerName, + &xpsCorrectorInfo->ClosedLoopStatus, + &xpsCorrectorInfo->KP, + &xpsCorrectorInfo->KI, + &xpsCorrectorInfo->KD, + &xpsCorrectorInfo->KS, + &xpsCorrectorInfo->IntegrationTime, + &xpsCorrectorInfo->DerivativeFilterCutOffFrequency, + &xpsCorrectorInfo->GKP, + &xpsCorrectorInfo->GKI, + &xpsCorrectorInfo->GKD, + &xpsCorrectorInfo->KForm, + &xpsCorrectorInfo->FeedForwardGainVelocity, + &xpsCorrectorInfo->FeedForwardGainAcceleration, + &xpsCorrectorInfo->Friction); +} + + +/** + * Wrapper function for PositionerCorrectorPIPositionSet. + * @param pAxis Axis struct AXIS_HDL. + * @return Return value from XPS function. + */ +static int PositionerCorrectorPIPositionSetWrapper(AXIS_HDL pAxis) +{ + xpsCorrectorInfo_t *xpsCorrectorInfo = &pAxis->xpsCorrectorInfo; + return PositionerCorrectorPIPositionSet(pAxis->pollSocket, + pAxis->positionerName, + xpsCorrectorInfo->ClosedLoopStatus, + xpsCorrectorInfo->KP, + xpsCorrectorInfo->KI, + xpsCorrectorInfo->IntegrationTime); +} + + +/** + * Wrapper function for PositionerCorrectorPIDFFVelocitySet. + * @param pAxis Axis struct AXIS_HDL. + * @return Return value from XPS function. + */ +static int PositionerCorrectorPIDFFVelocitySetWrapper(AXIS_HDL pAxis) +{ + xpsCorrectorInfo_t *xpsCorrectorInfo = &pAxis->xpsCorrectorInfo; + return PositionerCorrectorPIDFFVelocitySet(pAxis->pollSocket, + pAxis->positionerName, + xpsCorrectorInfo->ClosedLoopStatus, + xpsCorrectorInfo->KP, + xpsCorrectorInfo->KI, + xpsCorrectorInfo->KD, + xpsCorrectorInfo->KS, + xpsCorrectorInfo->IntegrationTime, + xpsCorrectorInfo->DerivativeFilterCutOffFrequency, + xpsCorrectorInfo->GKP, + xpsCorrectorInfo->GKI, + xpsCorrectorInfo->GKD, + xpsCorrectorInfo->KForm, + xpsCorrectorInfo->FeedForwardGainVelocity); +} + +/** + * Wrapper function for PositionerCorrectorPIDFFAccelerationSet. + * @param pAxis Axis struct AXIS_HDL. + * @return Return value from XPS function. + */ +static int PositionerCorrectorPIDFFAccelerationSetWrapper(AXIS_HDL pAxis) +{ + xpsCorrectorInfo_t *xpsCorrectorInfo = &pAxis->xpsCorrectorInfo; + return PositionerCorrectorPIDFFAccelerationSet(pAxis->pollSocket, + pAxis->positionerName, + xpsCorrectorInfo->ClosedLoopStatus, + xpsCorrectorInfo->KP, + xpsCorrectorInfo->KI, + xpsCorrectorInfo->KD, + xpsCorrectorInfo->KS, + xpsCorrectorInfo->IntegrationTime, + xpsCorrectorInfo->DerivativeFilterCutOffFrequency, + xpsCorrectorInfo->GKP, + xpsCorrectorInfo->GKI, + xpsCorrectorInfo->GKD, + xpsCorrectorInfo->KForm, + xpsCorrectorInfo->FeedForwardGainAcceleration); +} + +/** + * Wrapper function for PositionerCorrectorPIDDualFFVoltageSet. + * @param pAxis Axis struct AXIS_HDL. + * @return Return value from XPS function. + */ +static int PositionerCorrectorPIDDualFFVoltageSetWrapper(AXIS_HDL pAxis) +{ + xpsCorrectorInfo_t *xpsCorrectorInfo = &pAxis->xpsCorrectorInfo; + return PositionerCorrectorPIDDualFFVoltageSet(pAxis->pollSocket, + pAxis->positionerName, + xpsCorrectorInfo->ClosedLoopStatus, + xpsCorrectorInfo->KP, + xpsCorrectorInfo->KI, + xpsCorrectorInfo->KD, + xpsCorrectorInfo->KS, + xpsCorrectorInfo->IntegrationTime, + xpsCorrectorInfo->DerivativeFilterCutOffFrequency, + xpsCorrectorInfo->GKP, + xpsCorrectorInfo->GKI, + xpsCorrectorInfo->GKD, + xpsCorrectorInfo->KForm, + xpsCorrectorInfo->FeedForwardGainVelocity, + xpsCorrectorInfo->FeedForwardGainAcceleration, + xpsCorrectorInfo->Friction); +} + /* Code for iocsh registration */ @@ -953,12 +1608,33 @@ static void configXPSAxisCallFunc(const iocshArgBuf *args) } +/* void XPSEnableSetPosition(int setPos) */ +static const iocshArg XPSEnableSetPositionArg0 = {"Set Position Flag", iocshArgInt}; +static const iocshArg * const XPSEnableSetPositionArgs[1] = {&XPSEnableSetPositionArg0}; +static const iocshFuncDef xpsEnableSetPosition = {"XPSEnableSetPosition", 1, XPSEnableSetPositionArgs}; +static void xpsEnableSetPositionCallFunc(const iocshArgBuf *args) +{ + XPSEnableSetPosition(args[0].ival); +} + +/* void XPSSetPosSleepTime(int posSleep) */ +static const iocshArg XPSSetPosSleepTimeArg0 = {"Set Position Sleep Time", iocshArgInt}; +static const iocshArg * const XPSSetPosSleepTimeArgs[1] = {&XPSSetPosSleepTimeArg0}; +static const iocshFuncDef xpsSetPosSleepTime = {"XPSSetPosSleepTime", 1, XPSSetPosSleepTimeArgs}; +static void xpsSetPosSleepTimeCallFunc(const iocshArgBuf *args) +{ + XPSSetPosSleepTime(args[0].ival); +} + + static void XPSRegister(void) { iocshRegister(&setupXPS, setupXPSCallFunc); iocshRegister(&configXPS, configXPSCallFunc); iocshRegister(&configXPSAxis, configXPSAxisCallFunc); + iocshRegister(&xpsEnableSetPosition, xpsEnableSetPositionCallFunc); + iocshRegister(&xpsSetPosSleepTime, xpsSetPosSleepTimeCallFunc); iocshRegister(&TCLRun, TCLRunCallFunc); #ifdef vxWorks iocshRegister(&XPSC8GatheringTest, XPSC8GatheringTestCallFunc);