diff --git a/site_ansto/motor_dmc2280.c b/site_ansto/motor_dmc2280.c index 42dd40eb..9dd69ca0 100644 --- a/site_ansto/motor_dmc2280.c +++ b/site_ansto/motor_dmc2280.c @@ -19,9 +19,11 @@ #include #include #include +#include #include #include #include +#include #include #include #include @@ -99,9 +101,24 @@ typedef struct __MoDriv { int cntsPerX; /**< absolute encoder counts per physical unit */ int motOffDelay; /**< number of msec to wait before switching motor off, default=0 */ float lastPosition; /**< Position at last position check */ + float lastSteps; + float lastCounts; struct timeval time_lastPos_set; /**< Time when lastPosition was set */ float blockage_ckInterval; /**< Interval for checking blocked motors, seconds */ + int has_airpads; /**< Flag = 1 if there is are airpads for this motor */ + float fTarget; + int settle; + struct timeval time_settle_done; + int airpad_state; + int airpad_counter; + pNWTimer airpad_timer; + pNWTimer motor_timer; + int debug; } DMC2280Driv, *pDMC2280Driv; +#define AIRPADS_DOWN 0 +#define AIRPADS_RAISE 1 +#define AIRPADS_UP 2 +#define AIRPADS_LOWER 3 /*------------------- error codes ----------------------------------*/ #define BADADR -1 // NOT SET: Unknown host/port? #define BADBSY -2 @@ -142,10 +159,12 @@ typedef struct __MoDriv { #define MAXACCEL "maxaccel" #define DECEL "decel" #define MAXDECEL "maxdecel" +#define MOTOFFDELAY "motoffdelay" +#define AIRPADS "airpads" +#define SETTLE "settle" #define BLOCKAGE_CHECK_INTERVAL "blockage_check_interval" static int DMC2280Halt(void *pData); -static void set_lastPos(void *pData, float posn); static int DMC2280SetPar(void *pData, SConnection *pCon, char *name, float newValue); static int DMC2280Receive(pDMC2280Driv self, /*@out@*/ char *reply); @@ -242,6 +261,9 @@ static int DMC2280Send(pDMC2280Driv self, char *command) { /*@+mayaliasunique@*/ } + if (self->debug) + SICSLogWrite(command, eStatus); + /*@+matchanyintegral@ let size_t from strlen match int */ status = writeRS232(self->controller, command, strlen(command)); /*@-matchanyintegral@*/ @@ -296,6 +318,8 @@ static int DMC2280Receive(pDMC2280Driv self, /*@out@*/char *reply) { status=readRS232TillTerm(self->controller, reply, &dataLen); switch (status) { case 1: + if (self->debug) + SICSLogWrite(reply, eStatus); return dataLen; case TIMEOUT: self->errorCode = status; @@ -309,6 +333,29 @@ static int DMC2280Receive(pDMC2280Driv self, /*@out@*/char *reply) { return FAILURE; } +/** \brief Record the given posn and timestamp it. + * + * \param *pData provides access to a motor's data + * \param posn, the axis position which you want to remember. + * */ +static void set_lastMotion(pDMC2280Driv self, float steps, float counts) { + assert(self != NULL); + self->lastSteps = steps; + self->lastCounts = counts; + gettimeofday(&(self->time_lastPos_set), NULL); +} + +/** \brief Record the given posn and timestamp it. + * + * \param *pData provides access to a motor's data + * \param posn, the axis position which you want to remember. + * */ +static void set_lastPos(pDMC2280Driv self, float posn) { + assert(self != NULL); + self->lastPosition = posn; + gettimeofday(&(self->time_lastPos_set), NULL); +} + /**\brief Convenience function for getting speed, acceleration * or deceleration * @@ -373,6 +420,29 @@ static int getDMCSetting(void *pData, enum dmcsetting cmdIndex){ } } +/** \brief Reads motion. + * + * \param *steps motor steps + * \param *counts encoder counts + * \return + * SUCCESS + * FAILURE + */ +static int readMotion(pDMC2280Driv self, float *steps, float *counts) { + char reply[1024]; + char cmd[CMDLEN]; + + snprintf(cmd, CMDLEN, "MG _TD%c,_TP%c", self->axisLabel, self->axisLabel); + if (FAILURE == DMC2280Send(self, cmd)) + return FAILURE; + if (FAILURE == DMC2280Receive(self, reply)) + return FAILURE; + + if (2 != sscanf(reply, "%f %f", steps, counts)) + return FAILURE; + return SUCCESS; +} + /** \brief Reads absolute encoder. * * \param *pos is the absolute encoder reading on SUCCESS. @@ -427,6 +497,86 @@ static int DMC2280GetPos(void *pData, float *fPos){ } return OKOK; } + +static int DMC2280Run(void *pData,float fValue); + +static int airpad_callback(void* context, int mode) { + pDMC2280Driv self = (pDMC2280Driv) context; + char reply[1024]; + float fReply; + + self->airpad_timer = NULL; + if (FAILURE == DMC2280Send(self, "MG APDONE")) { + self->errorCode = BADCUSHION; + self->airpad_state = AIRPADS_DOWN; + return 0; + } + if (FAILURE == DMC2280Receive(self, reply)) { + self->errorCode = BADCUSHION; + self->airpad_state = AIRPADS_DOWN; + return 0; + } + fReply = (float) atof(reply); + if (self->airpad_state == AIRPADS_RAISE && fReply > 0) { + int iRet; + self->airpad_state = AIRPADS_UP; + iRet = DMC2280Run(self, self->fTarget); + if (iRet != OKOK) + self->errorCode = BADCUSHION; + return 0; + } + if (self->airpad_state == AIRPADS_LOWER && fReply == 0) { + self->airpad_state = AIRPADS_DOWN; + return 0; + } + if (self->airpad_counter <= 0) { + self->errorCode = BADCUSHION; + self->airpad_state = AIRPADS_DOWN; + return 0; + } + --self->airpad_counter; + NetWatchRegisterTimer(&self->airpad_timer, 1000, + airpad_callback, self); + return 0; +} + +static int motor_callback(void* context, int mode) { + pDMC2280Driv self = (pDMC2280Driv) context; + char cmd[CMDLEN]; + + self->motor_timer = NULL; + if (self->has_airpads) { + if (FAILURE == DMC2280Send(self, "FTUBE=0")) { + self->errorCode = BADCUSHION; + return 0; + } + self->airpad_state = AIRPADS_LOWER; + self->airpad_counter = 10; + NetWatchRegisterTimer(&self->airpad_timer, 1000, + airpad_callback, self); + return 0; + } + snprintf(cmd, CMDLEN, "MO%c", self->axisLabel); + DMC2280Send(self, cmd); + return 0; +} + +static int DMC2280RunAir(void *pData, float fValue) { + pDMC2280Driv self = (pDMC2280Driv)pData; + + self->fTarget = fValue; + if (self->airpad_timer) + NetWatchRemoveTimer(self->airpad_timer); + self->airpad_timer = NULL; + if (FAILURE == DMC2280Send(self, "FTUBE=1")) + return HWFault; + self->airpad_state = AIRPADS_RAISE; + self->airpad_counter = 10; + NetWatchRegisterTimer(&self->airpad_timer, 1000, + airpad_callback, self); + return OKOK; +} + /** \brief DMC2280 implementation of the RunTo * method in the MotorDriver interface. * @@ -442,54 +592,132 @@ static int DMC2280Run(void *pData,float fValue){ char axis; char cmd[CMDLEN], SHx[CMDLEN], BGx[CMDLEN], absPosCmd[CMDLEN]; int absEncHome, stepsPerX, motorHome, cntsPerX, newAbsPosn; - float target, currPos; + float target; self = (pDMC2280Driv)pData; assert(self != NULL); - DMC2280GetPos(pData, &currPos); - set_lastPos(pData, currPos); + + if (self->motor_timer) + NetWatchRemoveTimer(self->motor_timer); + self->motor_timer = NULL; + + if (self->has_airpads) + if (self->airpad_state != AIRPADS_UP) + return DMC2280RunAir(self, fValue); + + if (self->settle) + self->time_settle_done.tv_sec = 0; + + do { +#if 1 + float steps, counts; + if (FAILURE == readMotion(self, &steps, &counts)) + return HWFault; + set_lastMotion(pData, steps, counts); +#else + float currPos; + DMC2280GetPos(pData, &currPos); + set_lastPos(pData, currPos); +#endif + } while (0); axis=self->axisLabel; motorHome = self->motorHome; stepsPerX=self->stepsPerX; snprintf(SHx, CMDLEN, "SH%c", axis); snprintf(BGx, CMDLEN, "BG%c", axis); target = fValue - self->home; - newAbsPosn = (int)(target * stepsPerX + motorHome + 0.5); - snprintf(absPosCmd, CMDLEN, "PA%c=%d",axis, newAbsPosn); if (1 == self->abs_endcoder) { - /* Ensure that the defined motor position matches actual position */ absEncHome = self->absEncHome; cntsPerX = self->cntsPerX; - snprintf(cmd, CMDLEN, "DP%c=(_TP%c - %d)*(%d/%d) + %d",axis,axis,absEncHome,stepsPerX,cntsPerX,motorHome); - if (FAILURE == DMC2280Send(self, cmd)) - return HWFault; + /* PAF=-((absEncHome-_TPF)/-cntsPerX + target)*stepsPerX + _TDF */ + snprintf(absPosCmd, CMDLEN, + "PA%c=(((%d-_TP%c)/%d)+%f)*%d + _TD%c", + axis, + absEncHome, + axis, + cntsPerX, + target, + stepsPerX, + axis); #ifdef BACKLASHFIX snprintf(cmd, CMDLEN, "%cQTARGET=%d", axis, (int) (target * cntsPerX + absEncHome + 0.5)); if (FAILURE == DMC2280Send(self, cmd)) return HWFault; #endif + } else { + newAbsPosn = (int)(target * stepsPerX + motorHome + 0.5); + snprintf(absPosCmd, CMDLEN, "PA%c=%d",axis, newAbsPosn); } - if (FAILURE == DMC2280Send(self, absPosCmd)) - return HWFault; if (FAILURE == DMC2280Send(self, SHx)) return HWFault; + if (FAILURE == DMC2280Send(self, absPosCmd)) + return HWFault; if (FAILURE == DMC2280Send(self, BGx)) return HWFault; return OKOK; } -/** \brief Record the given posn and timestamp it. +/** \brief Check if the axis has moved significantly since + * the last check. + * + * The motion is checked against the expected at intervals of + * pDMC2280Driv->blockage_ckInterval. * * \param *pData provides access to a motor's data - * \param posn, the axis position which you want to remember. - * */ -static void set_lastPos(void *pData, float posn) { + * \return + * - 1 MOTOR OK, position has changed significantly during move + * - 0 MOTOR BLOCKED, no significant change in position detected. + */ +static int checkMotion(void *pData) { +#if 1 + float precision, steps, counts, ratio_obs, ratio_exp; + long int usec_TimeDiff; + struct timeval now; + pDMC2280Driv self; self = (pDMC2280Driv)pData; assert(self != NULL); - self->lastPosition = posn; - gettimeofday(&(self->time_lastPos_set), NULL); + gettimeofday(&now, NULL); + usec_TimeDiff = now.tv_sec - self->time_lastPos_set.tv_sec; + usec_TimeDiff *= 1000000; + usec_TimeDiff += now.tv_usec; + usec_TimeDiff -= self->time_lastPos_set.tv_usec; + if (usec_TimeDiff < (long int)(1e6*self->blockage_ckInterval)) + return 1; + if (self->pMot == NULL) + self->pMot = FindMotor(pServ->pSics, self->name); + MotorGetPar(self->pMot,"precision",&precision); + if (FAILURE == readMotion(self, &steps, &counts)) + return 0; + /* If not stepping, thern not blocked */ + if (steps == self->lastSteps) + return 1; + /* calculate observed and expected steps per count ratios */ + ratio_obs = (steps - self->lastSteps) / (counts - self->lastCounts); + ratio_exp = (float) self->stepsPerX / (float) self->cntsPerX; + /* less than half or more than double is trouble */ + if (ratio_obs / ratio_exp < 0.5 || ratio_obs / ratio_exp > 2.0) { + char msg[132]; + snprintf(msg, sizeof(msg), "Motion check fail: obs=%f, exp=%f", + ratio_obs, ratio_exp); + SICSLogWrite(msg, eError); + snprintf(msg, sizeof(msg), "steps=%f-%f, counts=%f-%f, exp=%d/%d", + steps, self->lastSteps, counts, self->lastCounts, + self->stepsPerX, self->cntsPerX); + SICSLogWrite(msg, eError); +#if 1 + set_lastMotion(pData, steps, counts); + return 1; +#else + return 0; +#endif + } else { + set_lastMotion(pData, steps, counts); + return 1; + } +#endif + return 1; } /** \brief Check if the axis position has changed significantly since @@ -552,6 +780,14 @@ static int DMC2280Status(void *pData){ self = (pDMC2280Driv)pData; assert(self != NULL); + + /* + * If we are waiting for the motor or airpads then we + * are busy + */ + if (self->motor_timer || self->airpad_timer) + return HWBusy; + /* Make sure that speed, accel and decel are set correctly */ /* ckSpeedAccelDecel(self); */ /* Get status of switches @@ -572,9 +808,15 @@ static int DMC2280Status(void *pData){ return HWFault; } if (moving) { + int iRet; /* If pos hasn't changed since last * check then stop and scream */ - if (checkPosition(pData) == 0) { +#if 0 + iRet = checkPosition(pData); +#else + iRet = checkMotion(pData); +#endif + if (iRet == 0) { DMC2280Halt(pData); self->errorCode = BLOCKED; return HWFault; @@ -623,9 +865,58 @@ static int DMC2280Status(void *pData){ } } #endif + /* + * When we get here, the motion has completed and we + * must determine when and how to shut off the motor + */ + if (self->settle) { + struct timeval *then = &self->time_settle_done; + struct timeval now; + gettimeofday(&now, NULL); + if (then->tv_sec == 0 || + (then->tv_sec - now.tv_sec) > self->settle) { + gettimeofday(then, NULL); + then->tv_sec += self->settle; + return HWBusy; + } else { + if ((now.tv_sec > then->tv_sec) || + ((now.tv_sec == then->tv_sec) && + (now.tv_usec >= then->tv_usec))) { + /* it's finished, fall through */ + } else { + return HWBusy; + } + } + } + if (self->has_airpads) { + if (self->airpad_state == AIRPADS_DOWN) + return HWIdle; + if (self->airpad_state == AIRPADS_LOWER) + return HWBusy; + if (self->motOffDelay > 0 ) { + NetWatchRegisterTimer(&self->motor_timer, + self->motOffDelay, + motor_callback, self); + return HWIdle; + } + if (FAILURE == DMC2280Send(self, "FTUBE=0")) + return HWFault; + self->airpad_state = AIRPADS_LOWER; + self->airpad_counter = 10; + NetWatchRegisterTimer(&self->airpad_timer, 1000, + airpad_callback, self); + return HWIdle; + } if (self->noPowerSave == _SAVEPOWER) { if (self->motOffDelay > 0 ) { +#if 0 snprintf(cmd, CMDLEN, "AT %d; MO%c", self->motOffDelay, self->axisLabel); +#else + NetWatchRegisterTimer(&self->motor_timer, + self->motOffDelay, + motor_callback, self); + return HWIdle; +#endif } else { snprintf(cmd, CMDLEN, "MO%c", self->axisLabel); } @@ -846,6 +1137,22 @@ static int DMC2280GetPar(void *pData, char *name, *fValue = self->maxDecel; return 1; } + if(strcmp(name,MOTOFFDELAY) == 0) { + *fValue = self->motOffDelay; + return 1; + } + if(strcmp(name,"debug") == 0) { + *fValue = self->debug; + return 1; + } + if(strcmp(name,SETTLE) == 0) { + *fValue = self->settle; + return 1; + } + if(strcmp(name,AIRPADS) == 0) { + *fValue = self->has_airpads; + return 1; + } if(strcmp(name,BLOCKAGE_CHECK_INTERVAL) == 0) { *fValue = self->blockage_ckInterval; return 1; @@ -913,6 +1220,46 @@ static int DMC2280SetPar(void *pData, SConnection *pCon, return 1; } + /* Set motor off delay, managers only */ + if(strcmp(name,MOTOFFDELAY) == 0) { + if(!SCMatchRights(pCon,usMugger)) + return 1; + else { + self->motOffDelay = newValue; + return 1; + } + } + + /* Debug, managers only */ + if(strcmp(name,"debug") == 0) { + if(!SCMatchRights(pCon,usMugger)) + return 1; + else { + self->debug = newValue; + return 1; + } + } + + /* Setttle, managers only */ + if(strcmp(name,SETTLE) == 0) { + if(!SCMatchRights(pCon,usMugger)) + return 1; + else { + self->settle = newValue; + return 1; + } + } + + /* Set airpads, managers only */ + if(strcmp(name,AIRPADS) == 0) { + if(!SCMatchRights(pCon,usMugger)) + return 1; + else { + self->has_airpads = newValue; + return 1; + } + } + /* Set interval between blocked motor checks, * managers only */ if(strcmp(name,BLOCKAGE_CHECK_INTERVAL) == 0) { @@ -1009,6 +1356,22 @@ static void DMC2280List(void *self, char *name, SConnection *pCon){ SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.maxDecel = %f\n", name, ((pDMC2280Driv)self)->maxDecel); SCWrite(pCon, buffer, eStatus); + snprintf(buffer, BUFFLEN, "%s.motOffDelay = %d\n", name, ((pDMC2280Driv)self)->motOffDelay); + SCWrite(pCon, buffer, eStatus); + snprintf(buffer, BUFFLEN, "%s.Debug = %d\n", name, ((pDMC2280Driv)self)->debug); + SCWrite(pCon, buffer, eStatus); + snprintf(buffer, BUFFLEN, "%s.Settle = %d\n", name, ((pDMC2280Driv)self)->settle); + SCWrite(pCon, buffer, eStatus); + snprintf(buffer, BUFFLEN, "%s.AirPads = %d\n", name, ((pDMC2280Driv)self)->has_airpads); + SCWrite(pCon, buffer, eStatus); + snprintf(buffer, BUFFLEN, "%s.absEnc = %d\n", name, ((pDMC2280Driv)self)->abs_endcoder); + SCWrite(pCon, buffer, eStatus); + if (((pDMC2280Driv)self)->abs_endcoder) { + snprintf(buffer, BUFFLEN, "%s.absEncHome = %d\n", name, ((pDMC2280Driv)self)->absEncHome); + SCWrite(pCon, buffer, eStatus); + snprintf(buffer, BUFFLEN, "%s.cntsPerX = %d\n", name, ((pDMC2280Driv)self)->cntsPerX); + SCWrite(pCon, buffer, eStatus); + } return; } /** \brief Free memory if motor is removed @@ -1104,6 +1467,7 @@ static void KillDMC2280(/*@only@*/void *pData){ eError); return NULL; } + memset(pNew, 0, sizeof(DMC2280Driv)); pNew->controller = NULL; pNew->name = NULL; pNew->errorCode = 0; @@ -1216,6 +1580,27 @@ static void KillDMC2280(/*@only@*/void *pData){ else sscanf(pPtr,"%d",&(pNew->motOffDelay)); + /* Debug: this motor driver logs exchanges */ + if ((pPtr=getParam(pCon, interp, params,"debug",_OPTIONAL)) == NULL) + pNew->debug=0; + else { + sscanf(pPtr,"%d",&(pNew->debug)); + } + + /* SETTLE: this motor need time to settle */ + if ((pPtr=getParam(pCon, interp, params,"settle",_OPTIONAL)) == NULL) + pNew->settle=0; + else { + sscanf(pPtr,"%d",&(pNew->settle)); + } + + /* AIRPADS: this motor need airpads */ + if ((pPtr=getParam(pCon, interp, params,"airpads",_OPTIONAL)) == NULL) + pNew->has_airpads=0; + else { + sscanf(pPtr,"%d",&(pNew->has_airpads)); + } + /* ABSENC: If the parameter requires an abs enc add it to the else block */ if ((pPtr=getParam(pCon, interp, params,"absenc",_OPTIONAL)) == NULL) pNew->abs_endcoder=0;