From 082355318f0ed811f20db747c7b6245d5cdef2a5 Mon Sep 17 00:00:00 2001 From: Douglas Clowes Date: Fri, 23 Mar 2007 18:06:20 +1100 Subject: [PATCH] Refactoring, reduce duplicate code, eliminate obsolete code. Fix a few bugs. Improve blocked motor detection code. Align with MultiChan version r1708 | dcl | 2007-03-23 18:06:20 +1100 (Fri, 23 Mar 2007) | 5 lines --- site_ansto/motor_dmc2280.c | 614 +++++++++++++++++-------------------- 1 file changed, 287 insertions(+), 327 deletions(-) diff --git a/site_ansto/motor_dmc2280.c b/site_ansto/motor_dmc2280.c index a852638c..da0b8087 100644 --- a/site_ansto/motor_dmc2280.c +++ b/site_ansto/motor_dmc2280.c @@ -32,9 +32,7 @@ #define UNITSLEN 256 #define TEXTPARLEN 1024 -void KillRS232(/*@only@*/ void *pData); -/*@+incondefs@*/ - +#define CMDLEN 1024 /** \brief Used to ensure that the getDMCSetting function is called * with valid values. * \see getDMCSetting @@ -81,8 +79,8 @@ typedef struct __MoDriv { float decel; /**< physical units per second^2 */ float maxDecel; /**< physical units per second^2 */ char axisLabel; - char lastCmd[1024]; - char dmc2280Error[1024]; + char lastCmd[CMDLEN]; + char dmc2280Error[CMDLEN]; float home; /**< home position for axis, default=0 */ int motorHome; /**< motor home position in steps */ int noPowerSave; /**< Flag = 1 to leave motors on after a move */ @@ -144,7 +142,6 @@ int DMC2280MotionControl = 1; /* defaults to enabled */ #define INIT_STR_SIZE 256 #define STR_RESIZE_LENGTH 256 -#define CMDLEN 1024 #define BUFFLEN 512 #define _SAVEPOWER 0 @@ -167,7 +164,7 @@ int DMC2280MotionControl = 1; /* defaults to enabled */ static int DMC2280Halt(void *pData); static int DMC2280SetPar(void *pData, SConnection *pCon, char *name, float newValue); -static int DMC2280Receive(pDMC2280Driv self, /*@out@*/ char *reply); + /** \brief Convert axis speed in physical units to * motor speed in steps/sec. @@ -234,6 +231,39 @@ static int DMC2280ReadChar(pDMC2280Driv self, /*@out@*/char *reply) { return FAILURE; } +/** \brief Gets output from the DMC2280, the abstract motor code should + * handle retries if the request times out. + * + * Note: The timeout for readRS232TillTerm is set by DMC2280Connect + * \param self (rw) provides access to the motor's data structure + * \param *reply (w) the data from the DMC2280. + * \return + * - SUCCESS + * - FAILURE + * \see SUCCESS FAILURE + */ +static int DMC2280Receive(pDMC2280Driv self, char *reply) { + int i, status, retries=1, dataLen=255; + reply[0] = '\0'; + for (i=0; icontroller, reply, &dataLen); + switch (status) { + case 1: + if (self->debug) + SICSLogWrite(reply, eStatus); + return dataLen; + case TIMEOUT: + self->errorCode = status; + continue; + /* TODO case INCOMPLETE: */ + default: + self->errorCode = status; + return FAILURE; + } + } + return FAILURE; +} + /** \brief Sends a DMC2280 command to the motor controller. * * If the command fails it displays the DMC2280 error message to the client @@ -249,7 +279,9 @@ static int DMC2280ReadChar(pDMC2280Driv self, /*@out@*/char *reply) { */ /* First character returned by controller is '?' for an invalid command or - ':' or space for a valid command */ + ':' for a valid command with no response + ' ' for a valid command with a response (':' follows) + */ static int DMC2280Send(pDMC2280Driv self, char *command) { char cmdValid, reply[256]; char *GetEMsg = "TC 1"; @@ -300,37 +332,22 @@ static int DMC2280Send(pDMC2280Driv self, char *command) { } } -/** \brief Gets output from the DMC2280, the abstract motor code should - * handle retries if the request times out. - * - * Note: The timeout for readRS232TillTerm is set by DMC2280Connect - * \param self (rw) provides access to the motor's data structure - * \param *reply (w) the data from the DMC2280. +/** + * \brief Sends a command and waits for a response + * + * \param self motor data + * \param cmd command to send + * \param reply space to return response * \return - * - SUCCESS - * - FAILURE - * \see SUCCESS FAILURE */ -static int DMC2280Receive(pDMC2280Driv self, /*@out@*/char *reply) { - int i, status, retries=1, dataLen=255; - reply[0] = '\0'; - for (i=0; icontroller, reply, &dataLen); - switch (status) { - case 1: - if (self->debug) - SICSLogWrite(reply, eStatus); - return dataLen; - case TIMEOUT: - self->errorCode = status; - continue; - /* TODO case INCOMPLETE: */ - default: - self->errorCode = status; - return FAILURE; - } - } - return FAILURE; +static int DMC2280SendReceive(pDMC2280Driv self, char *cmd, char* reply) { + int status; + + if (FAILURE == DMC2280Send(self, cmd)) + return FAILURE; + if (FAILURE == DMC2280Receive(self, reply)) + return FAILURE; + return OKOK; } /** \brief Record the given posn and timestamp it. @@ -345,82 +362,6 @@ static void set_lastMotion(pDMC2280Driv self, float steps, float counts) { gettimeofday(&(self->time_lastPos_set), NULL); } -#if 0 -/** \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 - * - * \param *pData provides access to a motor's data - * \param cmdIndex selects value to request from controller. - * \return Either speed acceleration or deceleration as requested. - * \see dmcsetting getMotSpeed getMotAccel getMotDecel - */ -static int getDMCSetting(void *pData, enum dmcsetting cmdIndex){ - pDMC2280Driv self = NULL; - char cmd[CMDLEN], reply[256]; - int dmcSetting; - - self = (pDMC2280Driv)pData; - switch (cmdIndex) { - case dmcspeed: - snprintf(cmd, CMDLEN, "MG _SP%c", self->axisLabel); - break; - case dmcacceleration: - snprintf(cmd, CMDLEN, "MG _AC%c", self->axisLabel); - break; - case dmcdeceleration: - snprintf(cmd, CMDLEN, "MG _DC%c", self->axisLabel); - } - if (FAILURE == DMC2280Send(self, cmd)) - return HWFault; - if (FAILURE == DMC2280Receive(self, reply)) - return HWFault; - dmcSetting =atoi(reply); - return dmcSetting; -} - -/** \brief Call this to make sure that the speed, - * acceleration and deceleration are set to the correct value.\n - * XXX Unused: This will interfere with progs running on the - * controller like #LIMSWI which sets maximum deceleration when a - * limit switch is hit. - */ -/*@unused@*/static void ckSpeedAccelDecel(pDMC2280Driv self) { - int motSetting; - char cmd[CMDLEN]; - - motSetting = getDMCSetting(self, dmcspeed); - /* Reset speed if it has been changed externally */ - if (motSetting != motSpeed(self, self->speed)) { - snprintf(cmd,CMDLEN,"SP%c=%d", self->axisLabel, motSpeed(self,self->speed)); - DMC2280Send(self, cmd); - } - - /* Reset acceleration if it has been changed externally */ - motSetting = getDMCSetting(self, dmcacceleration); - if (motSetting != motAccel(self, self->accel)) { - snprintf(cmd,CMDLEN,"AC%c=%d", self->axisLabel, motAccel(self,self->accel)); - DMC2280Send(self, cmd); - } - - /* Reset deceleration if it has been changed externally */ - motSetting = getDMCSetting(self, dmcdeceleration); - if (motSetting != motDecel(self, self->decel)) { - snprintf(cmd,CMDLEN,"DC%c=%d", self->axisLabel, motDecel(self,self->decel)); - DMC2280Send(self, cmd); - } -} -#endif /** \brief Reads motion. * @@ -431,13 +372,11 @@ static int getDMCSetting(void *pData, enum dmcsetting cmdIndex){ * FAILURE */ static int readMotion(pDMC2280Driv self, float *steps, float *counts) { - char reply[1024]; + char reply[CMDLEN]; 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)) + if (FAILURE == DMC2280SendReceive(self, cmd, reply)) return FAILURE; if (2 != sscanf(reply, "%f %f", steps, counts)) @@ -453,13 +392,11 @@ static int readMotion(pDMC2280Driv self, float *steps, float *counts) { * FAILURE */ static int readAbsEnc(pDMC2280Driv self, float *pos) { - char reply[1024]; + char reply[CMDLEN]; char cmd[CMDLEN]; snprintf(cmd, CMDLEN, "TP%c", self->axisLabel); - if (FAILURE == DMC2280Send(self, cmd)) - return FAILURE; - if (FAILURE == DMC2280Receive(self, reply)) + if (FAILURE == DMC2280SendReceive(self, cmd, reply)) return FAILURE; *pos = (float) atoi(reply); @@ -474,13 +411,11 @@ static int readAbsEnc(pDMC2280Driv self, float *pos) { * FAILURE */ static int ReadThread(pDMC2280Driv self, int thread, float *pos) { - char reply[1024]; + char reply[CMDLEN]; char cmd[CMDLEN]; snprintf(cmd, CMDLEN, "MG _XQ%d", thread); - if (FAILURE == DMC2280Send(self, cmd)) - return 0; - if (FAILURE == DMC2280Receive(self, reply)) + if (FAILURE == DMC2280SendReceive(self, cmd, reply)) return 0; *pos = (float) atoi(reply); @@ -495,13 +430,11 @@ static int ReadThread(pDMC2280Driv self, int thread, float *pos) { * FAILURE */ static int readHomeRun(pDMC2280Driv self, float *pos) { - char reply[1024]; + char reply[CMDLEN]; char cmd[CMDLEN]; snprintf(cmd, CMDLEN, "MG HOMERUN"); - if (FAILURE == DMC2280Send(self, cmd)) - return FAILURE; - if (FAILURE == DMC2280Receive(self, reply)) + if (FAILURE == DMC2280SendReceive(self, cmd, reply)) return FAILURE; *pos = (float) atoi(reply); @@ -547,7 +480,7 @@ static int RunHomeRoutine(pDMC2280Driv self, float newValue) { * */ static int DMC2280GetPos(void *pData, float *fPos){ pDMC2280Driv self = NULL; - char reply[1024]; + char reply[CMDLEN]; char cmd[CMDLEN]; float absEncPos, motorPos; @@ -560,9 +493,7 @@ static int DMC2280GetPos(void *pData, float *fPos){ *fPos = (absEncPos - self->absEncHome)/self->cntsPerX + self->home; } else { snprintf(cmd, ERRLEN, "TD%c", self->axisLabel); - if (FAILURE == DMC2280Send(self, cmd)) - return HWFault; - if (FAILURE == DMC2280Receive(self, reply)) + if (FAILURE == DMC2280SendReceive(self, cmd, reply)) return HWFault; motorPos =(float)atof(reply); *fPos = (motorPos - self->motorHome)/self->stepsPerX + self->home; @@ -570,136 +501,23 @@ 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. +/** + * \brief calculate and issue the motion commands * - * \param *pData provides access to a motor's data + * \param self 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; + + */ +static int DMC2280RunCommon(pDMC2280Driv self,float fValue){ char axis; char SHx[CMDLEN], BGx[CMDLEN], absPosCmd[CMDLEN]; int absEncHome, stepsPerX, motorHome, cntsPerX, newAbsPosn; float target; - 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->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; @@ -743,6 +561,174 @@ static int DMC2280Run(void *pData,float fValue){ return OKOK; } + +/** + * \brief request the airpad status periodically + */ +static int airpad_timeout(void* ctx, int mode) { + pDMC2280Driv self = (pDMC2280Driv) ctx; + char reply[CMDLEN]; + float fReply; + + self->airpad_timer = NULL; + + if (self->airpad_state == AIRPADS_UP || + self->airpad_state == AIRPADS_DOWN) + return 0; + + if (self->airpad_counter <= 0) { + self->errorCode = BADCUSHION; + self->airpad_state = AIRPADS_DOWN; + return 0; + } + if (FAILURE == DMC2280SendReceive(self, "MG APDONE", 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 = DMC2280RunCommon(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_timeout, self); + return 0; +} + +/** + * \brief initiate the raising or lowering of the airpads + * + * \param self motor data + * \param flag 1 for raise and 0 for lower + * \return 1 for SUCCESS or 0 for FAILURE + */ +static int DMC_AirPads(pDMC2280Driv self, int flag) { + char *cmd = NULL; + + if (self->airpad_timer) + NetWatchRemoveTimer(self->airpad_timer); + self->airpad_timer = NULL; + if (flag) { + cmd = "FTUBE=1"; + self->airpad_state = AIRPADS_RAISE; + } + else { + cmd = "FTUBE=0"; + self->airpad_state = AIRPADS_LOWER; + } + if (FAILURE == DMC2280Send(self, cmd)) { + self->airpad_state = AIRPADS_DOWN; + self->errorCode = BADCUSHION; + return 0; + } + self->airpad_counter = 10; + NetWatchRegisterTimer(&self->airpad_timer, 1000, + airpad_timeout, self); + return 1; +} + +/** + * \brief turn the motor off after a delay + * + * \param context motor data + * \param mode + */ +static int motor_timeout(void* context, int mode) { + pDMC2280Driv self = (pDMC2280Driv) context; + char cmd[CMDLEN]; + + self->motor_timer = NULL; + if (self->has_airpads) { + DMC_AirPads(self, 0); + return 0; + } + snprintf(cmd, CMDLEN, "MO%c", self->axisLabel); + DMC2280Send(self, cmd); + 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 + */ + do { +#if 1 + self->time_lastPos_set.tv_sec = 0; +#else + float currPos; + DMC2280GetPos(pData, &currPos); + set_lastPos(pData, currPos); +#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. * @@ -762,6 +748,13 @@ static int checkMotion(void *pData) { pDMC2280Driv self; self = (pDMC2280Driv)pData; assert(self != NULL); + if (self->time_lastPos_set.tv_sec == 0) { + /* first time - initialise the data */ + if (FAILURE == readMotion(self, &steps, &counts)) + return 0; + set_lastMotion(pData, steps, counts); + return 1; + } gettimeofday(&now, NULL); usec_TimeDiff = now.tv_sec - self->time_lastPos_set.tv_sec; usec_TimeDiff *= 1000000; @@ -775,7 +768,7 @@ static int checkMotion(void *pData) { if (FAILURE == readMotion(self, &steps, &counts)) return 0; /* If not stepping, then not blocked */ - if (fabs(steps - self->lastSteps) < self->blockage_thresh * self->cntsPerX) { + if (fabs(steps - self->lastSteps) < fabs(self->blockage_thresh * self->stepsPerX)) { /* just update the timestamp */ set_lastMotion(pData, self->lastSteps, self->lastCounts); return 1; @@ -799,48 +792,18 @@ static int checkMotion(void *pData) { set_lastMotion(pData, steps, counts); return 1; } else { + if (self->debug) { + char msg[132]; + snprintf(msg, sizeof(msg), "Motion check pass: obs=%f, exp=%f", + ratio_obs, ratio_exp); + SICSLogWrite(msg, eError); + } set_lastMotion(pData, steps, counts); return 1; } return 1; } -#if 0 -/** \brief Check if the axis position has changed significantly since - * the last check. - * - * The position change is checked against the 'precision' at intervals of - * pDMC2280Driv->blockage_ckInterval. - * - * \param *pData provides access to a motor's data - * \return - * - 1 MOTOR OK, position has changed significantly during move - * - 0 MOTOR BLOCKED, no significant change in position detected. - */ -static int checkPosition(void *pData) { - float precision, currPos; - long int usec_TimeDiff; - struct timeval now; - - pDMC2280Driv self; - self = (pDMC2280Driv)pData; - assert(self != NULL); - gettimeofday(&now, NULL); - usec_TimeDiff = (now.tv_sec - (self->time_lastPos_set).tv_sec)*1e6 + (now.tv_usec - (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); - DMC2280GetPos(pData, &currPos); - if ( precision - fabs(self->lastPosition - currPos) >= FLT_EPSILON) { - return 0; - } else { - set_lastPos(pData, currPos); - return 1; - } -} -#endif /** \brief Returns the motor status while it's moving, * implements the GetStatus method in the MotorDriver interface. @@ -857,9 +820,9 @@ static int DMC2280Status(void *pData){ pDMC2280Driv self = NULL; char cmd[CMDLEN]; int switches; - char switchesAscii[10]; + char switchesAscii[CMDLEN]; #ifdef BACKLASHFIX - char reply[256]; + char reply[CMDLEN]; int SERVO_LOOP_NOT_RUNNING = -1, servoLoopStatus; int SHOULD_FIXPOS=1, should_fixpos; #endif @@ -880,9 +843,7 @@ static int DMC2280Status(void *pData){ /* Get status of switches * see TS (Tell Switches) in Galil manc2xx.pdf */ snprintf(cmd, CMDLEN, "TS%c", self->axisLabel); - if (FAILURE == DMC2280Send(self, cmd)) - return HWFault; - if (FAILURE == DMC2280Receive(self, switchesAscii)) + if (FAILURE == DMC2280SendReceive(self, cmd, switchesAscii)) return HWFault; sscanf(switchesAscii, "%d", &switches); moving = (switches & STATUSMOVING)>0; @@ -935,9 +896,7 @@ static int DMC2280Status(void *pData){ if (self->abs_endcoder == 1) { /* Make sure that the servo loop is closed by checking if * the CLSLOOP thread is running on the controller.*/ - if (FAILURE == DMC2280Send(self, "MG _XQ1")) - return HWFault; - if (FAILURE == DMC2280Receive(self, reply)) + if (FAILURE == DMC2280SendReceive(self, "MG _XQ1", reply)) return HWFault; sscanf(reply, "%d", &servoLoopStatus); if (servoLoopStatus == SERVO_LOOP_NOT_RUNNING) { @@ -946,9 +905,7 @@ static int DMC2280Status(void *pData){ return HWFault; } snprintf(cmd, CMDLEN, "MG %cSHLDFIX", self->axisLabel); - if (FAILURE == DMC2280Send(self, cmd)) - return HWFault; - if (FAILURE == DMC2280Receive(self, reply)) + if (FAILURE == DMC2280SendReceive(self, cmd, reply)) return HWFault; sscanf(reply, "%d", &should_fixpos); if (should_fixpos == SHOULD_FIXPOS) { @@ -991,15 +948,11 @@ static int DMC2280Status(void *pData){ if (self->motOffDelay > 0 ) { NetWatchRegisterTimer(&self->motor_timer, self->motOffDelay, - motor_callback, self); + motor_timeout, self); return HWIdle; } - if (FAILURE == DMC2280Send(self, "FTUBE=0")) + if (!DMC_AirPads(self, 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) { @@ -1009,7 +962,7 @@ static int DMC2280Status(void *pData){ #else NetWatchRegisterTimer(&self->motor_timer, self->motOffDelay, - motor_callback, self); + motor_timeout, self); return HWIdle; #endif } else { @@ -1053,50 +1006,50 @@ static void DMC2280Error(void *pData, int *iCode, char *error, int errLen){ strncpy(error, "Command Timeout", (size_t)errLen); break; case BADADR: - strncpy(error,"Bad address",(size_t)errLen); + strncpy(error, "Bad address", (size_t)errLen); break; case BADBSY: - strncpy(error,"Motor still busy",(size_t)errLen); + strncpy(error, "Motor still busy", (size_t)errLen); break; case BADCMD: snprintf(error, (size_t)errLen, "Bad command: '%s'\ndmcError: ", self->lastCmd); strncat(error, self->dmc2280Error, (size_t)errLen); break; case BADPAR: - strncpy(error,"Bad parameter",(size_t)errLen); + strncpy(error, "Bad parameter", (size_t)errLen); break; case BADUNKNOWN: - strncpy(error,"Unknown error condition",(size_t)errLen); + strncpy(error, "Unknown error condition", (size_t)errLen); break; case BADSTP: - strncpy(error,"Motor is stopped",(size_t)errLen); + strncpy(error, "Motor is stopped", (size_t)errLen); break; case BADEMERG: - strncpy(error,"Emergency stop is engaged",(size_t)errLen); + strncpy(error, "Emergency stop is engaged", (size_t)errLen); break; case BGFAIL: - strncpy(error,"Begin not possible due to limit switch",(size_t)errLen); + strncpy(error, "Begin not possible due to limit switch", (size_t)errLen); break; case RVRSLIM: - strncpy(error,"Crashed into limit switch",(size_t)errLen); + strncpy(error, "Crashed into limit switch", (size_t)errLen); break; case FWDLIM: - strncpy(error,"Crashed into limit switch",(size_t)errLen); + strncpy(error, "Crashed into limit switch", (size_t)errLen); break; case POSFAULT: - strncpy(error,"Positioning fault detected",(size_t)errLen); + strncpy(error, "Positioning fault detected", (size_t)errLen); break; case BADCUSHION: - strncpy(error,"Air cushion problem",(size_t)errLen); + strncpy(error, "Air cushion problem", (size_t)errLen); break; case ERRORLIM: - strncpy(error,"Axis error exceeds error limit",(size_t)errLen); + strncpy(error, "Axis error exceeds error limit", (size_t)errLen); break; case IMPOSSIBLE_LIM_SW: - strncpy(error,"Both limit switches seem active, maybe the polarity is set 'active low'. You should configure the controller with CN 1,-1,-1,0", (size_t)errLen); + strncpy(error, "Both limit switches seem active, maybe the polarity is set 'active low'. You should configure the controller with CN 1,-1,-1,0", (size_t)errLen); break; case BLOCKED: - strncpy(error,"STOPPING MOTOR, MOTION SEEMS TO BE BLOCKED", (size_t)errLen); + strncpy(error, "STOPPING MOTOR, MOTION SEEMS TO BE BLOCKED", (size_t)errLen); break; case MOTIONCONTROLOFF: strncpy(error, "MOTION CONTROL SEEMS TO BE DISABLED", (size_t)errLen); @@ -1106,9 +1059,10 @@ static void DMC2280Error(void *pData, int *iCode, char *error, int errLen){ break; default: /* FIXME What's the default */ + snprintf(error, (size_t)errLen, "Unknown Motor Error: %d", self->errorCode); break; } - strncpy(self->errorMsg,error,(size_t)errLen); + strncpy(self->errorMsg, error, (size_t)errLen); } /** \brief Attempts to recover from an error. Implements the TryAndFixIt @@ -1173,6 +1127,7 @@ static int DMC2280Halt(void *pData){ snprintf(cmd, CMDLEN, "ST%c", self->axisLabel); if (FAILURE == DMC2280Send(self, cmd)) return HWFault; + /* TODO: FIXME cannot do this until motor stops */ snprintf(cmd, CMDLEN, "MO%c", self->axisLabel); if (FAILURE == DMC2280Send(self, cmd)) return HWFault; @@ -1596,11 +1551,17 @@ static void KillDMC2280(/*@only@*/void *pData){ pDMC2280Driv self = NULL; self = (pDMC2280Driv)pData; assert(self != NULL); - if (self->name) + if (self->name) { free(self->name); - if (self->errorMsg) + self->name = NULL; + } + if (self->errorMsg) { free(self->errorMsg); - free(self); + self->errorMsg = NULL; + } +/* Not required as performed in caller + * free(self); + */ return; } /*@only@*/ prs232 createRS232(char *host, int iPort); @@ -1660,7 +1621,7 @@ static void KillDMC2280(/*@only@*/void *pData){ * - Get the parameter from the parameter array, see PARAMETERS: below * - If the parameter requires an abs enc then add it after ABSENC: */ -/*@only@*//*@null@*/ MotorDriver *CreateDMC2280(/*@observer@*/SConnection *pCon, /*@observer@*/char *motor, /*@observer@*/char *params){ +MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) { pDMC2280Driv pNew = NULL; char *pPtr = NULL; char buffer[132]; @@ -1713,7 +1674,6 @@ static void KillDMC2280(/*@only@*/void *pData){ return NULL; } - /*FIXME Tell splint that there's no memory leak because pointers are being initialised here */ pNew->name = (char *)malloc(sizeof(char)*(strlen(motor)+1)); if (pNew->name == NULL) { (void) SCWrite(pCon,"ERROR: no memory to allocate motor driver",