diff --git a/site_ansto/motor_dmc2280.c b/site_ansto/motor_dmc2280.c index e58ed767..45646a72 100644 --- a/site_ansto/motor_dmc2280.c +++ b/site_ansto/motor_dmc2280.c @@ -37,12 +37,17 @@ * with valid values. * \see getDMCSetting */ +typedef struct __MoDriv DMC2280Driv, *pDMC2280Driv; + +typedef struct __command Command, *pCommand; +typedef int (*CommandCallback)(pCommand pCmd); + enum dmcsetting {dmcspeed, dmcacceleration, dmcdeceleration}; /*----------------------------------------------------------------------- The motor driver structure. Please note that the first set of fields has be identical with the fields of AbstractModriv in ../modriv.h ------------------------------------------------------------------------*/ -typedef struct __MoDriv { +struct __MoDriv { /* general motor driver interface fields. _REQUIRED! */ @@ -110,7 +115,7 @@ typedef struct __MoDriv { pNWTimer motor_timer; /**< motor off timer */ int debug; int preseek; /**< Flag = 1 if current move is a backlash preseek */ -} DMC2280Driv, *pDMC2280Driv; +}; int DMC2280MotionControl = 1; /* defaults to enabled */ @@ -170,9 +175,6 @@ static int DMC2280Halt(void *pData); static int DMC2280SetPar(void *pData, SConnection *pCon, char *name, float newValue); -typedef struct __command Command, *pCommand; -typedef int (*CommandCallback)(pCommand pCmd); - struct __command { pMultiChan unit; int cstate; @@ -656,38 +658,6 @@ static int RunHomeRoutine(pDMC2280Driv self, float newValue) { return SUCCESS; } -/** \brief Reads motor position, implements the GetPosition - * method in the MotorDriver interface. - * - * \param *pData provides access to a motor's data - * \param *fPos contains the motor position in physical units after a call. - * \return - * - OKOK request succeeded - * - HWFault request failed - * */ -static int DMC2280GetPos(void *pData, float *fPos){ - pDMC2280Driv self = NULL; - char reply[CMDLEN]; - char cmd[CMDLEN]; - float absEncPos, motorPos; - - reply[0]='\0'; - self = (pDMC2280Driv)pData; - assert(self != NULL); - if (1 == self->abs_endcoder) { - if (readAbsEnc(self, &absEncPos) == FAILURE) - return HWFault; - *fPos = (absEncPos - self->absEncHome)/self->cntsPerX + self->home; - } else { - snprintf(cmd, ERRLEN, "TD%c", self->axisLabel); - if (FAILURE == DMC2280SendReceive(self, cmd, reply)) - return HWFault; - motorPos =(float)atof(reply); - *fPos = (motorPos - self->motorHome)/self->stepsPerX + self->home; - } - return OKOK; -} - /** * \brief calculate and issue the motion commands * @@ -931,72 +901,6 @@ static int motor_timeout(void* context, int mode) { return 0; } -/** - * \brief handle the run command for a motor with airpads - * - * \param self motor data - * \param fValue new motor position - */ -static int DMC2280RunAir(pDMC2280Driv self, float fValue) { - self->fTarget = fValue; - if (!DMC_AirPads(self, 1)) - return HWFault; - return OKOK; -} - -/** \brief DMC2280 implementation of the RunTo - * method in the MotorDriver interface. - * - * \param *pData provides access to a motor's data - * \param fValue target position in physical units, software zeros - * have already been applied. - * \return - * - OKOK request succeeded - * - HWFault request failed - * */ -static int DMC2280Run(void *pData,float fValue){ - pDMC2280Driv self = NULL; - self = (pDMC2280Driv)pData; - assert(self != NULL); - - /* If Motion Control is off, report HWFault */ - if (DMC2280MotionControl != 1) { - if (DMC2280MotionControl == 0) - self->errorCode = MOTIONCONTROLOFF; - else - self->errorCode = MOTIONCONTROLUNK; - return HWFault; - } - - if (self->motor_timer) - NetWatchRemoveTimer(self->motor_timer); - self->motor_timer = NULL; - - if (self->settle) - self->time_settle_done.tv_sec = 0; - - /* - * Note: this will read the current position which will block - */ - do { - float currPos; - DMC2280GetPos(pData, &currPos); - self->lastPosition = currPos; -#if 1 - self->time_lastPos_set.tv_sec = 0; -#endif - } while (0); - - /* - * Note: this passes control to a timer routine - */ - if (self->has_airpads) - if (self->airpad_state != AIRPADS_UP) - return DMC2280RunAir(self, fValue); - - return DMC2280RunCommon(self, fValue); -} - /** \brief Check if the axis has moved significantly since * the last check. * @@ -1080,6 +984,105 @@ static int checkMotion(void *pData) { return 1; } +/** + * \brief handle the run command for a motor with airpads + * + * \param self motor data + * \param fValue new motor position + */ +static int DMC2280RunAir(pDMC2280Driv self, float fValue) { + if (!DMC_AirPads(self, 1)) + return HWFault; + return OKOK; +} + +/** \brief Reads motor position, implements the GetPosition + * method in the MotorDriver interface. + * + * \param *pData provides access to a motor's data + * \param *fPos contains the motor position in physical units after a call. + * \return + * - OKOK request succeeded + * - HWFault request failed + * */ +static int DMC2280GetPos(void *pData, float *fPos){ + pDMC2280Driv self = NULL; + char reply[CMDLEN]; + char cmd[CMDLEN]; + float absEncPos, motorPos; + + reply[0]='\0'; + self = (pDMC2280Driv)pData; + assert(self != NULL); + if (1 == self->abs_endcoder) { + if (readAbsEnc(self, &absEncPos) == FAILURE) + return HWFault; + *fPos = (absEncPos - self->absEncHome)/self->cntsPerX + self->home; + } else { + snprintf(cmd, ERRLEN, "TD%c", self->axisLabel); + if (FAILURE == DMC2280SendReceive(self, cmd, reply)) + return HWFault; + motorPos =(float)atof(reply); + *fPos = (motorPos - self->motorHome)/self->stepsPerX + self->home; + } + return OKOK; +} + +/** \brief DMC2280 implementation of the RunTo + * method in the MotorDriver interface. + * + * \param *pData provides access to a motor's data + * \param fValue target position in physical units, software zeros + * have already been applied. + * \return + * - OKOK request succeeded + * - HWFault request failed + * */ +static int DMC2280Run(void *pData,float fValue){ + pDMC2280Driv self = NULL; + self = (pDMC2280Driv)pData; + assert(self != NULL); + + /* If Motion Control is off, report HWFault */ + if (DMC2280MotionControl != 1) { + if (DMC2280MotionControl == 0) + self->errorCode = MOTIONCONTROLOFF; + else + self->errorCode = MOTIONCONTROLUNK; + return HWFault; + } + + if (self->motor_timer) + NetWatchRemoveTimer(self->motor_timer); + self->motor_timer = NULL; + + if (self->settle) + self->time_settle_done.tv_sec = 0; + + /* + * Note: this will read the current position which will block + */ + do { + float currPos; + DMC2280GetPos(pData, &currPos); + self->lastPosition = currPos; +#if 1 + self->time_lastPos_set.tv_sec = 0; +#endif + } while (0); + + self->fTarget = fValue; + + /* + * Note: this passes control to a timer routine + */ + if (self->has_airpads) + if (self->airpad_state != AIRPADS_UP) + return DMC2280RunAir(self, fValue); + + return DMC2280RunCommon(self, fValue); +} + /** \brief Returns the motor status while it's moving, * implements the GetStatus method in the MotorDriver interface. @@ -1197,7 +1200,7 @@ static int DMC2280Status(void *pData){ * When we get here, the motion has completed */ if (self->preseek) { - DMC_RunCommon(self, self->fTarget); + DMC2280RunCommon(self, self->fTarget); return HWBusy; } /* and we @@ -1419,23 +1422,6 @@ static int DMC2280Halt(void *pData){ return 1; } -/** \brief Allow motor drivers to return text values like units. - * - * Refactoring: Should replace this with a function that will - * return hdb values. - * This is the first step in the refactoring process. */ -int DMC2280GetTextPar(void *pData, char *name, char *textPar) { - pDMC2280Driv self = NULL; - - self = (pDMC2280Driv)pData; - - if(strcasecmp(name,UNITS) == 0) { - snprintf(textPar, UNITSLEN, self->units); - return 1; - } - return 0; -} - /** \brief Fetches the value of the named parameter, * implements the GetDriverPar method in the MotorDriver interface. * @@ -2104,19 +2090,19 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) { sscanf(pPtr,"%f",&(pNew->cntsPerX)); } - /* Set speed */ - snprintf(cmd,CMDLEN,"SP%c=%d", pNew->axisLabel, motSpeed(pNew, pNew->speed)); - if (FAILURE == DMC2280Send(pNew, cmd)) - return NULL; - /* Set acceleration */ - snprintf(cmd,CMDLEN,"AC%c=%d", pNew->axisLabel, motAccel(pNew, pNew->accel)); - if (FAILURE == DMC2280Send(pNew, cmd)) - return NULL; - /* Set deceleration */ - snprintf(cmd,CMDLEN,"DC%c=%d", pNew->axisLabel, motDecel(pNew, pNew->decel)); - if (FAILURE == DMC2280Send(pNew, cmd)) - return NULL; - /* TODO Initialise current position and target to get a sensible initial list output */ + /* Set speed */ + snprintf(cmd,CMDLEN,"SP%c=%d", pNew->axisLabel, motSpeed(pNew, pNew->speed)); + if (FAILURE == DMC2280Send(pNew, cmd)) + return NULL; + /* Set acceleration */ + snprintf(cmd,CMDLEN,"AC%c=%d", pNew->axisLabel, motAccel(pNew, pNew->accel)); + if (FAILURE == DMC2280Send(pNew, cmd)) + return NULL; + /* Set deceleration */ + snprintf(cmd,CMDLEN,"DC%c=%d", pNew->axisLabel, motDecel(pNew, pNew->decel)); + if (FAILURE == DMC2280Send(pNew, cmd)) + return NULL; + /* TODO Initialise current position and target to get a sensible initial list output */ return (MotorDriver *)pNew; }