From bbdc2f820d0d333e23b6d0337a07dd5bb570b4fa Mon Sep 17 00:00:00 2001 From: Douglas Clowes Date: Fri, 4 May 2007 13:47:21 +1000 Subject: [PATCH] minor, trivial and cosmetic changes not related to state machine implementation r1938 | dcl | 2007-05-04 13:47:21 +1000 (Fri, 04 May 2007) | 2 lines --- site_ansto/motor_dmc2280.c | 137 ++++++++++++++++++++----------------- 1 file changed, 75 insertions(+), 62 deletions(-) diff --git a/site_ansto/motor_dmc2280.c b/site_ansto/motor_dmc2280.c index 2e8ef56c..b57a7cef 100644 --- a/site_ansto/motor_dmc2280.c +++ b/site_ansto/motor_dmc2280.c @@ -37,12 +37,12 @@ * with valid values. * \see getDMCSetting */ +enum dmcsetting {dmcspeed, dmcacceleration, dmcdeceleration}; 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 @@ -92,7 +92,7 @@ struct __MoDriv { int motorHome; /**< motor home position in steps */ int noPowerSave; /**< Flag = 1 to leave motors on after a move */ float stepsPerX; /**< steps per physical unit */ - int abs_endcoder; /**< Flag = 1 if there is an abs enc */ + int abs_encoder; /**< Flag = 1 if there is an abs enc */ int absEncHome; /**< Home position in counts for abs enc */ float cntsPerX; /**< absolute encoder counts per physical unit */ int motOffDelay; /**< number of msec to wait before switching motor off, default=0 */ @@ -119,6 +119,7 @@ struct __MoDriv { int DMC2280MotionControl = 1; /* defaults to enabled */ +#define AIR_POLL_TIMER 1000 #define AIRPADS_DOWN 0 #define AIRPADS_RAISE 1 #define AIRPADS_UP 2 @@ -134,7 +135,7 @@ int DMC2280MotionControl = 1; /* defaults to enabled */ #define RVRSLIM -8 #define FWDLIM -9 #define POSFAULT -11 // NOT SET -#define BADCUSHION -12 // NOT SET +#define BADCUSHION -12 // Air Cushion Fault #define ERRORLIM -13 #define IMPOSSIBLE_LIM_SW -14 #define BGFAIL -15 // NOT SET @@ -144,7 +145,7 @@ int DMC2280MotionControl = 1; /* defaults to enabled */ #define MOTCMDTMO -19 /* Motor Command Timeout */ /*--------------------------------------------------------------------*/ #define STATUSMOVING 128 /* Motor is moving */ -#define STATUSERRORLIMIT 64 /* Number of errorss exceed limit */ +#define STATUSERRORLIMIT 64 /* Number of errors exceed limit */ #define STATUSOFF 32 /* Motor off */ #define STATUSFWDLIMIT 8 /* Forward limit switch active */ #define STATUSRVRSLIMIT 4 /* Reverse limit switch active */ @@ -321,7 +322,7 @@ static int DMC_Rx(void* ctx, int rxchar) { return iRet; } -static int DMC_SendCmd(pMultiChan unit, +static int DMC_SendCommand(pMultiChan unit, char* command, int cmd_len, CommandCallback callback, void* context, int rsp_len) { @@ -354,6 +355,15 @@ static int DMC_SendCmd(pMultiChan unit, return MultiChanEnque(unit, myCmd, DMC_Tx, DMC_Rx); } +static int DMC_SendCmd(pDMC2280Driv self, + char* command, + CommandCallback callback) +{ + return DMC_SendCommand(self->mcc, + command, strlen(command), + callback, self, CMDLEN); +} + static void DMC_Notify(void* context, int event) { pDMC2280Driv self = (pDMC2280Driv) context; @@ -447,7 +457,7 @@ static int DMC_transact(pDMC2280Driv self, void *send, int sendLen, assert(self); txn.transReply = reply; txn.transWait = 1; - DMC_SendCmd(self->mcc, + DMC_SendCommand(self->mcc, send, sendLen, TransCallback, &txn, replyLen); while (txn.transWait == 1) @@ -460,7 +470,7 @@ static int DMC_transact(pDMC2280Driv self, void *send, int sendLen, static int DMC2280Queue(pDMC2280Driv self, char *cmd, CommandCallback cb) { if (cb == NULL) cb = SendCallback; - return DMC_SendCmd(self->mcc, cmd, strlen(cmd), cb, self, CMDLEN); + return DMC_SendCmd(self, cmd, cb); } /** \brief Sends a DMC2280 command to the motor controller. @@ -704,7 +714,7 @@ static int DMC2280RunCommon(pDMC2280Driv self,float fValue){ } } - if (1 == self->abs_endcoder) { + if (1 == self->abs_encoder) { absEncHome = self->absEncHome; cntsPerX = self->cntsPerX; #if 0 @@ -798,8 +808,10 @@ static int airpad_callback(pCommand pCmd) { if (resp_len > 0) { float fReply; - if (self->debug) - SICSLogWrite(resp, eStatus); + if (self->debug) { + SICSLogWrite(pCmd->inp_buf, eStatus); + SICSLogWrite(pCmd->out_buf, eStatus); + } fReply = (float) atof(resp); if (self->airpad_state == AIRPADS_RAISE && fReply > 0) { int iRet; @@ -838,7 +850,8 @@ static int airpad_timeout(void* ctx, int mode) { return 0; } --self->airpad_counter; - NetWatchRegisterTimer(&self->airpad_timer, 1000, + NetWatchRegisterTimer(&self->airpad_timer, + AIR_POLL_TIMER, airpad_timeout, self); if (FAILURE == DMC2280Queue(self, "MG APDONE", airpad_callback)) { @@ -876,7 +889,8 @@ static int DMC_AirPads(pDMC2280Driv self, int flag) { return 0; } self->airpad_counter = 10; - NetWatchRegisterTimer(&self->airpad_timer, 1000, + NetWatchRegisterTimer(&self->airpad_timer, + AIR_POLL_TIMER, airpad_timeout, self); return 1; } @@ -913,7 +927,7 @@ static int motoff_timeout(void* context, int mode) { * - 0 MOTOR BLOCKED, no significant change in position detected. */ static int checkMotion(void *pData) { - float precision, steps, counts, ratio_obs, ratio_exp, ratio_cmp; + float steps, counts, ratio_obs, ratio_exp, ratio_cmp; long int usec_TimeDiff; struct timeval now; @@ -921,7 +935,7 @@ static int checkMotion(void *pData) { self = (pDMC2280Driv)pData; assert(self != NULL); /* we can only test if there is an absolute encoder */ - if (!self->abs_endcoder) + if (!self->abs_encoder) return 1; if (self->time_lastPos_set.tv_sec == 0) { /* first time - initialise the data */ @@ -937,9 +951,6 @@ static int checkMotion(void *pData) { 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, then not blocked */ @@ -1014,7 +1025,7 @@ static int DMC2280GetPos(void *pData, float *fPos){ reply[0]='\0'; self = (pDMC2280Driv)pData; assert(self != NULL); - if (1 == self->abs_endcoder) { + if (1 == self->abs_encoder) { if (readAbsEnc(self, &absEncPos) == FAILURE) return HWFault; *fPos = (absEncPos - self->absEncHome)/self->cntsPerX + self->home; @@ -1066,9 +1077,7 @@ static int DMC2280Run(void *pData,float fValue){ float currPos; DMC2280GetPos(pData, &currPos); self->lastPosition = currPos; -#if 1 self->time_lastPos_set.tv_sec = 0; -#endif } while (0); self->fTarget = fValue; @@ -1172,7 +1181,7 @@ static int DMC2280Status(void *pData){ return HWFault; } #ifdef BACKLASHFIX - if (self->abs_endcoder == 1) { + if (self->abs_encoder == 1) { /* Make sure that the servo loop is closed by checking if * the CLSLOOP thread is running on the controller.*/ if (FAILURE == DMC2280SendReceive(self, "MG _XQ1", reply)) @@ -1521,7 +1530,7 @@ static int DMC2280GetPar(void *pData, char *name, *fValue = self->backlash_offset; return 1; } - if (self->abs_endcoder != 0) { + if (self->abs_encoder != 0) { if (strcasecmp(name,"absenc") == 0) { if (readAbsEnc(self, fValue) == SUCCESS) return 1; @@ -1709,7 +1718,7 @@ static int DMC2280SetPar(void *pData, SConnection *pCon, /* Invoke Home Run routine in controller, * managers only */ - if(self->abs_endcoder == 0 && strcasecmp(name,"homerun") == 0) { + if(self->abs_encoder == 0 && strcasecmp(name,"homerun") == 0) { if(!SCMatchRights(pCon,usMugger)) return 1; else { @@ -1786,14 +1795,16 @@ static int DMC2280SetPar(void *pData, SConnection *pCon, * \param *name (r) name of motor. * \param *pCon (r) connection object. */ -static void DMC2280StrList(void *self, char *name, SConnection *pCon){ +static void DMC2280StrList(pDMC2280Driv self, char *name, SConnection *pCon){ char buffer[BUFFLEN]; - snprintf(buffer, BUFFLEN, "%s.long_name = %s\n", name, ((pDMC2280Driv)self)->long_name); + snprintf(buffer, BUFFLEN, "%s.part = %s\n", name, self->part); SCWrite(pCon, buffer, eStatus); - snprintf(buffer, BUFFLEN, "%s.axis = %c\n", name, ((pDMC2280Driv)self)->axisLabel); + snprintf(buffer, BUFFLEN, "%s.long_name = %s\n", name, self->long_name); SCWrite(pCon, buffer, eStatus); - snprintf(buffer, BUFFLEN, "%s.units = %s\n", name, ((pDMC2280Driv)self)->units); + snprintf(buffer, BUFFLEN, "%s.axis = %c\n", name, self->axisLabel); + SCWrite(pCon, buffer, eStatus); + snprintf(buffer, BUFFLEN, "%s.units = %s\n", name, self->units); SCWrite(pCon, buffer, eStatus); return; } @@ -1802,50 +1813,51 @@ static void DMC2280StrList(void *self, char *name, SConnection *pCon){ * \param *name (r) name of motor. * \param *pCon (r) connection object. */ -static void DMC2280List(void *self, char *name, SConnection *pCon){ +static void DMC2280List(void *pData, char *name, SConnection *pCon){ + pDMC2280Driv self = (pDMC2280Driv) pData; char buffer[BUFFLEN]; - snprintf(buffer, BUFFLEN, "%s.home = %f\n", name, ((pDMC2280Driv)self)->home); + snprintf(buffer, BUFFLEN, "%s.home = %f\n", name, self->home); SCWrite(pCon, buffer, eStatus); - snprintf(buffer, BUFFLEN, "%s.speed = %f\n", name, ((pDMC2280Driv)self)->speed); + snprintf(buffer, BUFFLEN, "%s.speed = %f\n", name, self->speed); SCWrite(pCon, buffer, eStatus); - snprintf(buffer, BUFFLEN, "%s.maxSpeed = %f\n", name, ((pDMC2280Driv)self)->maxSpeed); + snprintf(buffer, BUFFLEN, "%s.maxSpeed = %f\n", name, self->maxSpeed); SCWrite(pCon, buffer, eStatus); - snprintf(buffer, BUFFLEN, "%s.accel = %f\n", name, ((pDMC2280Driv)self)->accel); + snprintf(buffer, BUFFLEN, "%s.accel = %f\n", name, self->accel); SCWrite(pCon, buffer, eStatus); - snprintf(buffer, BUFFLEN, "%s.maxAccel = %f\n", name, ((pDMC2280Driv)self)->maxAccel); + snprintf(buffer, BUFFLEN, "%s.maxAccel = %f\n", name, self->maxAccel); SCWrite(pCon, buffer, eStatus); - snprintf(buffer, BUFFLEN, "%s.decel = %f\n", name, ((pDMC2280Driv)self)->decel); + snprintf(buffer, BUFFLEN, "%s.decel = %f\n", name, self->decel); SCWrite(pCon, buffer, eStatus); - snprintf(buffer, BUFFLEN, "%s.maxDecel = %f\n", name, ((pDMC2280Driv)self)->maxDecel); + snprintf(buffer, BUFFLEN, "%s.maxDecel = %f\n", name, self->maxDecel); SCWrite(pCon, buffer, eStatus); - snprintf(buffer, BUFFLEN, "%s.motOffDelay = %d\n", name, ((pDMC2280Driv)self)->motOffDelay); + snprintf(buffer, BUFFLEN, "%s.motOffDelay = %d\n", name, self->motOffDelay); SCWrite(pCon, buffer, eStatus); - snprintf(buffer, BUFFLEN, "%s.Debug = %d\n", name, ((pDMC2280Driv)self)->debug); + snprintf(buffer, BUFFLEN, "%s.Debug = %d\n", name, self->debug); SCWrite(pCon, buffer, eStatus); - snprintf(buffer, BUFFLEN, "%s.Settle = %d\n", name, ((pDMC2280Driv)self)->settle); + snprintf(buffer, BUFFLEN, "%s.Settle = %d\n", name, self->settle); SCWrite(pCon, buffer, eStatus); - snprintf(buffer, BUFFLEN, "%s.Blockage_Check_Interval = %f\n", name, ((pDMC2280Driv)self)->blockage_ckInterval); + snprintf(buffer, BUFFLEN, "%s.Blockage_Check_Interval = %f\n", name, self->blockage_ckInterval); SCWrite(pCon, buffer, eStatus); - snprintf(buffer, BUFFLEN, "%s.Blockage_Thresh = %f\n", name, ((pDMC2280Driv)self)->blockage_thresh); + snprintf(buffer, BUFFLEN, "%s.Blockage_Thresh = %f\n", name, self->blockage_thresh); SCWrite(pCon, buffer, eStatus); - snprintf(buffer, BUFFLEN, "%s.Blockage_Ratio = %f\n", name, ((pDMC2280Driv)self)->blockage_ratio); + snprintf(buffer, BUFFLEN, "%s.Blockage_Ratio = %f\n", name, self->blockage_ratio); SCWrite(pCon, buffer, eStatus); - snprintf(buffer, BUFFLEN, "%s.Blockage_Fail = %d\n", name, ((pDMC2280Driv)self)->blockage_fail); + snprintf(buffer, BUFFLEN, "%s.Blockage_Fail = %d\n", name, self->blockage_fail); SCWrite(pCon, buffer, eStatus); - snprintf(buffer, BUFFLEN, "%s.Backlash_offset = %f\n", name, ((pDMC2280Driv)self)->backlash_offset); + snprintf(buffer, BUFFLEN, "%s.Backlash_offset = %f\n", name, self->backlash_offset); SCWrite(pCon, buffer, eStatus); - snprintf(buffer, BUFFLEN, "%s.AirPads = %d\n", name, ((pDMC2280Driv)self)->has_airpads); + snprintf(buffer, BUFFLEN, "%s.AirPads = %d\n", name, self->has_airpads); SCWrite(pCon, buffer, eStatus); - snprintf(buffer, BUFFLEN, "%s.absEnc = %d\n", name, ((pDMC2280Driv)self)->abs_endcoder); + snprintf(buffer, BUFFLEN, "%s.absEnc = %d\n", name, self->abs_encoder); SCWrite(pCon, buffer, eStatus); - if (((pDMC2280Driv)self)->abs_endcoder) { - snprintf(buffer, BUFFLEN, "%s.absEncHome = %d\n", name, ((pDMC2280Driv)self)->absEncHome); + if (self->abs_encoder) { + snprintf(buffer, BUFFLEN, "%s.absEncHome = %d\n", name, self->absEncHome); SCWrite(pCon, buffer, eStatus); - snprintf(buffer, BUFFLEN, "%s.cntsPerX = %f\n", name, ((pDMC2280Driv)self)->cntsPerX); + snprintf(buffer, BUFFLEN, "%s.cntsPerX = %f\n", name, self->cntsPerX); SCWrite(pCon, buffer, eStatus); } - snprintf(buffer, BUFFLEN, "%s.stepsPerX = %f\n", name, ((pDMC2280Driv)self)->stepsPerX); + snprintf(buffer, BUFFLEN, "%s.stepsPerX = %f\n", name, self->stepsPerX); SCWrite(pCon, buffer, eStatus); return; } @@ -1987,6 +1999,14 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) { pNew->backlash_offset = 0.0; /* PARAMETERS: Fetch parameter values */ + + /* Debug: this motor driver logs exchanges */ + if ((pPtr=getParam(pCon, interp, params,"debug",_OPTIONAL)) == NULL) + pNew->debug=0; + else { + sscanf(pPtr,"%d",&(pNew->debug)); + } + if ((pPtr=getParam(pCon, interp, params, LONG_NAME, _OPTIONAL)) != NULL) { strncpy(pNew->long_name, pPtr, sizeof(pNew->long_name)); pNew->long_name[sizeof(pNew->long_name) - 1] = '\0'; @@ -2047,13 +2067,6 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) { 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; @@ -2077,15 +2090,15 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) { /* 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; + pNew->abs_encoder = 0; else { - sscanf(pPtr,"%d",&(pNew->abs_endcoder)); + sscanf(pPtr,"%d",&(pNew->abs_encoder)); if ((pPtr=getParam(pCon, interp, params,"absenchome",_REQUIRED)) == NULL) - pNew->absEncHome=0; + pNew->absEncHome = 0; else sscanf(pPtr,"%d",&(pNew->absEncHome)); if ((pPtr=getParam(pCon, interp, params,"cntsperx",_REQUIRED)) == NULL) - pNew->cntsPerX=1.0; + pNew->cntsPerX = 1.0; else sscanf(pPtr,"%f",&(pNew->cntsPerX)); } @@ -2150,6 +2163,7 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData, return 1; } else if (strcasecmp("list", argv[1]) == 0) { + /* Handle in generic motor driver */ } else if (strcasecmp("slist", argv[1]) == 0) { DMC2280StrList(self, argv[0], pCon); @@ -2176,8 +2190,7 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData, return 1; } newZero = (currPos - newValue); - MotorSetPar(self->pMot, pCon, "softzero", newZero); - return 1; + return MotorSetPar(self->pMot, pCon, "softzero", newZero); } } return MotorAction(pCon, pSics, pData, argc, argv);