From 77657615e60f9d69de22871a1318b3e233f207e5 Mon Sep 17 00:00:00 2001 From: Douglas Clowes Date: Fri, 10 Aug 2007 10:32:40 +1000 Subject: [PATCH] MAJOR changes: remove deprecated pre state machine code, revise state transitions, add state entry events, part of the RUNx implementation, comments, .... r2106 | dcl | 2007-08-10 10:32:40 +1000 (Fri, 10 Aug 2007) | 2 lines --- site_ansto/motor_dmc2280.c | 1217 +++++++++++++----------------------- 1 file changed, 423 insertions(+), 794 deletions(-) diff --git a/site_ansto/motor_dmc2280.c b/site_ansto/motor_dmc2280.c index 92e1bad0..c6826674 100644 --- a/site_ansto/motor_dmc2280.c +++ b/site_ansto/motor_dmc2280.c @@ -1,4 +1,3 @@ -#define DEPRECATED /** \file motor_dmc2280.c * \brief Driver for Galil DMC2280 motor controller. * @@ -44,11 +43,18 @@ enum commandtype {CMD_RUN=1, CMD_HALT=2}; typedef struct __MoDriv DMC2280Driv, *pDMC2280Driv; -enum eventtype {eTimerEvent, eMessageEvent, eCommandEvent, eTimeoutEvent}; +enum eventtype { + eStateEvent, + eTimerEvent, + eMessageEvent, + eCommandEvent, + eTimeoutEvent +}; typedef struct EvtEvent_s EvtEvent, *pEvtEvent; typedef void (*StateFunc)(pDMC2280Driv self, pEvtEvent event); +typedef struct EvtState_s { } EvtState; typedef struct EvtTimer_s { } EvtTimer; typedef struct EvtMessage_s { @@ -64,6 +70,7 @@ typedef struct EvtTimeout_s { } EvtTimeout; struct EvtEvent_s { enum eventtype event_type; union { + EvtState sta; EvtTimer tmr; EvtMessage msg; EvtCommand cmd; @@ -125,7 +132,7 @@ struct __MoDriv { 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 */ + int motOffDelay; /**< msec to wait before switching motor off, default=0 */ int currFlags; int currSteps; int currCounts; @@ -133,6 +140,12 @@ struct __MoDriv { float lastPosition; /**< Position at last position check */ float lastSteps; float lastCounts; + int thread0; /**< last read of _XQ0 */ + unsigned short int input0; /**< last read of _TI0 */ + unsigned short int input1; /**< last read of _TI1 */ + bool ampError; /**< amplifier error */ + bool threadError; /**< thread error */ + bool moving; /**< true if moving */ struct timeval time_lastPos_set; /**< Time when lastPosition was set */ float blockage_ckInterval; /**< Interval for checking blocked motors, seconds */ float blockage_thresh; /**< motion threshold for blockage checking */ @@ -141,6 +154,7 @@ struct __MoDriv { int has_airpads; /**< Flag = 1 if there is are airpads for this motor */ float backlash_offset; /**< signed offset to drive from */ float fTarget; /**< target passed from SICS to timer callback */ + float fPreseek; /**< preseek target when preseek is active */ int settle; /**< motor settling time in seconds */ struct timeval time_settle_done; /**< time when settling will be over */ int airpad_state; /**< state of the airpads finite state machine */ @@ -166,16 +180,12 @@ struct __MoDriv { int DMC2280MotionControl = 1; /* defaults to enabled */ #define AIR_POLL_TIMER 1000 -#define MOTOR_POLL_FAST 100 -#define MOTOR_POLL_SLOW 1000 +#define MOTOR_POLL_FAST 200 +#define MOTOR_POLL_SLOW 5000 #define ON_SETTLE_TIMER 1000 #define MAX_CREEP_STEPS 100 #define MAX_RESTARTS 0 -#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 @@ -232,6 +242,7 @@ static int DMC2280SetPar(void *pData, SConnection *pCon, /** \brief Convert axis speed in physical units to * motor speed in steps/sec. + * * \param self (r) provides access to the motor's data structure * \param speed in physical units, eg mm/sec degrees/sec * \return the speed in motor steps/sec @@ -243,7 +254,8 @@ static int motSpeed(pDMC2280Driv self, float axisSpeed) { } /** \brief Convert axis acceleration in physical units to - * to motor speed in steps/sec^2 + * motor speed in steps/sec^2 + * * \param self (r) provides access to the motor's data structure * \param acceleration in physical units, eg mm/sec^2 degrees/sec^2 * \return the acceleration in motor steps/sec^2 @@ -256,6 +268,7 @@ static int motAccel(pDMC2280Driv self, float axisAccel) { /** \brief Convert axis deceleration in physical units to * motor deceleration in steps/sec^2 + * * \param self (r) provides access to the motor's data structure * \param deceleration in physical units, eg mm/sec^2 degrees/sec^2 * \return the deceleration in motor steps/sec^2 @@ -268,6 +281,7 @@ static int motDecel(pDMC2280Driv self, float axisDecel) { /** \brief Convert motor position to physical units * using the current motor steps or encoder counts + * * \param self (r) provides access to the motor's data structure * \return the motor position in physical units */ @@ -282,6 +296,7 @@ static float motPosit(pDMC2280Driv self) { /** \brief Convert motor target in physical units to * motor absolute position in steps + * * \param self (r) provides access to the motor's data structure * \param target in physical units, eg mm, degrees * \return the absolute position in motor steps @@ -317,7 +332,7 @@ static int motAbsol(pDMC2280Driv self, float target) { result = (int) absolute; if (self->debug) { char line[CMDLEN]; - snprintf(line, CMDLEN, "Rounding %f to %d", absolute, result); + snprintf(line, CMDLEN, "motAbsol Rounding %f to %d", absolute, result); SICSLogWrite(line, eStatus); } return result; @@ -325,7 +340,8 @@ static int motAbsol(pDMC2280Driv self, float target) { /** \brief Convert motor target in physical units to * motor absolute position in steps - * \param self (r) provides access to the motor's data structure + * + * \param self provides access to the motor's data structure * \param target in physical units, eg mm, degrees * \return the absolute position in motor steps */ @@ -360,9 +376,27 @@ static int motCreep(pDMC2280Driv self, float target) { } } + if (self->debug) { + char text[CMDLEN]; + snprintf(text, CMDLEN, "CREEP: cur=%d, target=%d, offset=%d, new=%d", + self->currSteps, target_steps, offset, self->currSteps + offset); + SICSLogWrite(text, eStatus); + } if (self->creep_val > 0) /* moving down handle as positive */ offset = -offset; if (offset > fabs(self->stepsPerX * self->creep_precision)) { +#if 1 + /* if half offset is more than creep_offset warp to creep_offset */ + if (offset > (int) (2.0 * fabs(self->stepsPerX * self->creep_offset))) + offset = offset - fabs(self->stepsPerX * self->creep_offset); + else { + /* if closer than one count, single step else go half way */ + if (offset <= fabs(self->stepsPerX / self->cntsPerX)) + offset = 1; + else + offset = offset / 2; + } +#else if (offset - (int) fabs(self->stepsPerX * self->creep_offset) > (int) fabs(self->stepsPerX / self->cntsPerX)) offset = offset - fabs(self->stepsPerX * self->creep_offset); else { @@ -371,6 +405,7 @@ static int motCreep(pDMC2280Driv self, float target) { else offset = offset / 2; } +#endif self->preseek = 1; } if (self->creep_val > 0) /* moving down restore to negative */ @@ -622,28 +657,19 @@ static void set_lastMotion(pDMC2280Driv self, float steps, float counts) { gettimeofday(&(self->time_lastPos_set), NULL); } -#ifndef DEPRECATED -/** \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[CMDLEN]; - char cmd[CMDLEN]; - - snprintf(cmd, CMDLEN, "MG _TD%c,_TP%c", self->axisLabel, self->axisLabel); - if (FAILURE == DMC2280SendReceive(self, cmd, reply)) - return FAILURE; - - if (2 != sscanf(reply, "%f %f", steps, counts)) - return FAILURE; - return SUCCESS; +static void report_motion(pDMC2280Driv self) { + SConnection *pDumCon; + MotCallback sCall; + if (self->pMot == NULL) + self->pMot = FindMotor(pServ->pSics, self->name); + assert(self->pMot); + pDumCon = SCCreateDummyConnection(pServ->pSics); + MotorGetSoftPosition(self->pMot,pDumCon,&sCall.fVal); + SCDeleteConnection(pDumCon); + sCall.pName = self->pMot->name; + InvokeCallBack(self->pMot->pCall, MOTDRIVE, &sCall); + InvokeCallBack(self->pMot->pCall, MOTEND, &sCall); } -#endif /** \brief Reads absolute encoder. * @@ -671,7 +697,7 @@ static int readAbsEnc(pDMC2280Driv self, float *pos) { * SUCCESS * FAILURE */ -static int ReadThread(pDMC2280Driv self, int thread, float *pos) { +static int readThread(pDMC2280Driv self, int thread, float *pos) { char reply[CMDLEN]; char cmd[CMDLEN]; @@ -730,250 +756,6 @@ static int RunHomeRoutine(pDMC2280Driv self, float newValue) { return SUCCESS; } -#ifndef DEPRECATED -/** - * \brief calculate and issue the motion commands - * - * \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 DMC2280RunCommon(pDMC2280Driv self,float fValue){ - char axis; - char SHx[CMDLEN], BGx[CMDLEN], absPosCmd[CMDLEN]; - int absEncHome, motorHome, newAbsPosn; - float stepsPerX, cntsPerX; - float target; - - 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; - - if (self->preseek) - self->preseek = 0; - else { - if (self->backlash_offset > FLT_EPSILON) { - if (target > self->lastPosition) { - self->preseek = 1; - target += self->backlash_offset; - if (target > self->fUpper) - target = self->fUpper; - } - } - else if (self->backlash_offset < -FLT_EPSILON) { - if (target < self->lastPosition) { - self->preseek = 1; - target += self->backlash_offset; - if (target < self->fLower) - target = self->fLower; - } - } - } - - if (1 == self->abs_encoder) { - absEncHome = self->absEncHome; - cntsPerX = self->cntsPerX; -#if 0 - /* PAF=-((absEncHome-_TPF)/-cntsPerX + target)*stepsPerX + _TDF */ - snprintf(absPosCmd, CMDLEN, - "PA%c=(((%d-_TP%c)/%f)+%f)*%f + _TD%c", - axis, - absEncHome, - axis, - cntsPerX, - target, - stepsPerX, - axis); -#else - /* PAZ=((absEncHome-_TPZ) + (cntsPerX * target)) * stepsPerX / cntsPerX + _TDZ */ - char s_cnts[20]; - char s_trgt[20]; - char s_stps[20]; - int i; - snprintf(s_cnts, sizeof(s_cnts), "%.4f", cntsPerX); - for (i = strlen(s_cnts); i > 0; --i) - if (s_cnts[i-1] == '.') { - s_cnts[i-1] = '\0'; - break; - } - else if (s_cnts[i-1] == '0') - s_cnts[i-1] = '\0'; - else - break; - snprintf(s_trgt, sizeof(s_trgt), "%.4f", target); - for (i = strlen(s_trgt); i > 0; --i) - if (s_trgt[i-1] == '.') { - s_trgt[i-1] = '\0'; - break; - } - else if (s_trgt[i-1] == '0') - s_trgt[i-1] = '\0'; - else - break; - snprintf(s_stps, sizeof(s_stps), "%.4f", stepsPerX); - for (i = strlen(s_stps); i > 0; --i) - if (s_stps[i-1] == '.') { - s_stps[i-1] = '\0'; - break; - } - else if (s_stps[i-1] == '0') - s_stps[i-1] = '\0'; - else - break; - snprintf(absPosCmd, CMDLEN, - "PA%c=((%d-_TP%c)+(%s*%s))*%s/%s+_TD%c", - axis, - absEncHome, - axis, - s_cnts, - s_trgt, - s_stps, - s_cnts, - axis); -#endif - } else { - newAbsPosn = (int)(target * stepsPerX + motorHome + 0.5); - snprintf(absPosCmd, CMDLEN, "PA%c=%d",axis, newAbsPosn); - } - - if (FAILURE == DMC2280Send(self, SHx)) - return HWFault; - if (FAILURE == DMC2280Send(self, absPosCmd)) - return HWFault; - if (FAILURE == DMC2280Send(self, BGx)) - return HWFault; - return OKOK; -} - -/** - * \brief process the airpad status response - */ -static int airpad_callback(pAsyncTxn pCmd) { - char* resp = pCmd->inp_buf; - pDMC2280Driv self = (pDMC2280Driv) pCmd->cntx; - - if (pCmd->txn_status == ATX_TIMEOUT) { - if (self->debug) { - SICSLogWrite(pCmd->out_buf, eStatus); - SICSLogWrite("", eStatus); - } - strncpy(self->lastCmd, pCmd->out_buf, CMDLEN); - self->errorCode = MOTCMDTMO; - } - else { - float fReply; - 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; - 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; - } - } - return 0; -} - -/** - * \brief request the airpad status periodically - */ -static int airpad_timeout(void* ctx, int mode) { - pDMC2280Driv self = (pDMC2280Driv) ctx; - - 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; - } - --self->airpad_counter; - NetWatchRegisterTimer(&self->airpad_timer, - AIR_POLL_TIMER, - airpad_timeout, self); - - if (FAILURE == DMC2280Queue(self, "MG APDONE", airpad_callback)) { - self->errorCode = BADCUSHION; - self->airpad_state = AIRPADS_DOWN; - return 0; - } - 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, - AIR_POLL_TIMER, - airpad_timeout, self); - return 1; -} - -/** - * \brief turn the motor off after a delay - * - * \param context motor data - * \param mode - */ -static int motoff_timeout(void* context, int mode) { - pDMC2280Driv self = (pDMC2280Driv) context; - char cmd[CMDLEN]; - - self->motoff_timer = NULL; - if (self->has_airpads) { - DMC_AirPads(self, 0); - return 0; - } - snprintf(cmd, CMDLEN, "MO%c", self->axisLabel); - DMC2280Send(self, cmd); - return 0; -} -#endif - /** \brief Check if the axis has moved significantly since * the last check. * @@ -1002,12 +784,6 @@ static int checkMotion(void *pData) { steps = self->currSteps; counts = self->currCounts; } -#ifndef DEPRECATED - else { - if (FAILURE == readMotion(self, &steps, &counts)) - return 0; - } -#endif set_lastMotion(pData, steps, counts); return 1; } @@ -1022,12 +798,6 @@ static int checkMotion(void *pData) { steps = self->currSteps; counts = self->currCounts; } -#ifndef DEPRECATED - else { - if (FAILURE == readMotion(self, &steps, &counts)) - return 0; - } -#endif /* If not stepping, then not blocked */ if (fabs(steps - self->lastSteps) < fabs(self->blockage_thresh * self->stepsPerX)) { /* just update the timestamp */ @@ -1076,20 +846,6 @@ static int checkMotion(void *pData) { return 1; } -#ifndef DEPRECATED -/** - * \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; -} -#endif - /** * \brief test if there is a variable of the form VVVx where VVV is the * variable name and x is the axis label. @@ -1130,33 +886,89 @@ static int state_cmd_execute(pDMC2280Driv self, enum commandtype cmd); * * \param self access to the drive data */ -static void request_status(pDMC2280Driv self) { +static int cmdStatus(pDMC2280Driv self) { char cmd[CMDLEN]; - /* TODO: add _RUNx for has_airpads == 3 */ - snprintf(cmd, CMDLEN, "MG {F10.0} _TD%c,_TP%c,_TS%c,_TI0,_TI1,_XQ0", - self->axisLabel, - self->axisLabel, - self->axisLabel); - DMC_SendCmd(self, cmd, state_msg_callback); - return; + if (self->has_airpads == 3) + snprintf(cmd, CMDLEN, "MG {F10.0} _TD%c,_TP%c,_TS%c,_RUN%c,_TI0,_TI1,_XQ0", + self->axisLabel, self->axisLabel, self->axisLabel, self->axisLabel); + else + snprintf(cmd, CMDLEN, "MG {F10.0} _TD%c,_TP%c,_TS%c,_BG%c,_TI0,_TI1,_XQ0", + self->axisLabel, self->axisLabel, self->axisLabel, self->axisLabel); + return DMC_SendCmd(self, cmd, state_msg_callback); } -static int process_status(pDMC2280Driv self, const char* text) { +static int cmdOn(pDMC2280Driv self) { + char cmd[CMDLEN]; + if (self->has_airpads == 1) { + snprintf(cmd, CMDLEN, "FTUBE=1"); + } + else if (self->has_airpads == 2 || self->has_airpads == 3) { + snprintf(cmd, CMDLEN, "REQ%c=1", self->axisLabel); + } + else { + snprintf(cmd, CMDLEN, "SH%c", self->axisLabel); + } + return DMC_SendCmd(self, cmd, state_msg_callback); +} + +static int cmdPosition(pDMC2280Driv self, int target) +{ + char cmd[CMDLEN]; + if (self->has_airpads == 3) + snprintf(cmd, CMDLEN, "DST%c=%d", self->axisLabel, target); + else + snprintf(cmd, CMDLEN, "PA%c=%d", self->axisLabel, target); + return DMC_SendCmd(self, cmd, state_msg_callback); +} + +static int cmdBegin(pDMC2280Driv self) +{ + char cmd[CMDLEN]; + if (self->has_airpads == 3) + snprintf(cmd, CMDLEN, "RUN%c=1", self->axisLabel); + else + snprintf(cmd, CMDLEN, "BG%c", self->axisLabel); + return DMC_SendCmd(self, cmd, state_msg_callback); +} + +static int cmdOff(pDMC2280Driv self) { + char cmd[CMDLEN]; + if (self->has_airpads == 1) { + snprintf(cmd, CMDLEN, "FTUBE=0"); + } + else if (self->has_airpads == 2 || self->has_airpads == 3) { + snprintf(cmd, CMDLEN, "REQ%c=0", self->axisLabel); + } + else { + snprintf(cmd, CMDLEN, "MO%c", self->axisLabel); + } + return DMC_SendCmd(self, cmd, state_msg_callback); +} + +static int rspStatus(pDMC2280Driv self, const char* text) { int iRet, iFlags; int iSteps, iCounts; - int iTI0, iTI1, iXQ0; + int iTI0, iTI1, iXQ0, iBG; /* TODO: add _RUNx for has_airpads == 3 */ - iRet = sscanf(text, "%d %d %d %d %d %d", - &iSteps, &iCounts, &iFlags, + iRet = sscanf(text, "%d %d %d %d %d %d %d", + &iSteps, &iCounts, &iFlags, &iBG, &iTI0, &iTI1, &iXQ0); - if (iRet != 6) + if (iRet != 7) return 0; self->currFlags = iFlags; self->currSteps = iSteps; self->currCounts = iCounts; self->currPosition = motPosit(self); - /* TODO Amplifier Error and Thread0 */ + self->thread0 = iXQ0; + self->input0 = iTI0; + self->input1 = iTI1; + if (self->axisLabel >= 'A' && self->axisLabel >= 'D') + self->ampError = !(1 & (iTI0 >> (self->axisLabel - 'A'))); + else + self->ampError = !(1 & (iTI1 >> (self->axisLabel - 'E'))); + self->threadError = self->thread0 < 0; + self->moving = iBG > 0; return 1; } @@ -1171,7 +983,7 @@ static void DMCState_MotorHalt(pDMC2280Driv self, pEvtEvent event); static void DMCState_OffTimer(pDMC2280Driv self, pEvtEvent event); static void DMCState_MotorStop(pDMC2280Driv self, pEvtEvent event); static void DMCState_MotorOff(pDMC2280Driv self, pEvtEvent event); -static void DMCState_WaitStatus(pDMC2280Driv self, pEvtEvent event); +static void DMCState_Error(pDMC2280Driv self, pEvtEvent event); static char* state_name(StateFunc func) { if (func == DMCState_Unknown) return "DMCState_Unknown"; @@ -1183,7 +995,7 @@ static char* state_name(StateFunc func) { if (func == DMCState_OffTimer) return "DMCState_OffTimer"; if (func == DMCState_MotorStop) return "DMCState_MotorStop"; if (func == DMCState_MotorOff) return "DMCState_MotorOff"; - if (func == DMCState_WaitStatus) return "DMCState_WaitStatus"; + if (func == DMCState_Error) return "DMCState_Error"; return ""; } @@ -1209,6 +1021,9 @@ void str_n_cat(char* s1, int len, const char* s2) { static char* event_name(pEvtEvent event, char* text, int length) { switch (event->event_type) { + case eStateEvent: + snprintf(text, length, "eStateEvent"); + return text; case eTimerEvent: snprintf(text, length, "eTimerEvent"); return text; @@ -1242,8 +1057,11 @@ static void report_event(pDMC2280Driv self, pEvtEvent event) { if (self->debug || self->trace) { char line[CMDLEN]; char text[CMDLEN]; - snprintf(line, CMDLEN, "Motor=%s, State=%s, event=%s", self->name, - state_name(self->myState), event_name(event, text, CMDLEN)); + snprintf(line, CMDLEN, "Motor=%s, State=%s(%d), event=%s", + self->name, + state_name(self->myState), + self->subState, + event_name(event, text, CMDLEN)); if (self->debug) SICSLogWrite(line, eStatus); if (self->trace) @@ -1254,8 +1072,11 @@ static void report_event(pDMC2280Driv self, pEvtEvent event) { static void change_state(pDMC2280Driv self, StateFunc func) { if (self->debug || self->trace) { char line[CMDLEN]; - snprintf(line, CMDLEN, "Motor=%s, OldState=%s, NewState=%s", self->name, - state_name(self->myState), state_name(func)); + snprintf(line, CMDLEN, "Motor=%s, OldState=%s(%d), NewState=%s", + self->name, + state_name(self->myState), + self->subState, + state_name(func)); if (self->debug) SICSLogWrite(line, eStatus); if (self->trace) @@ -1264,23 +1085,30 @@ static void change_state(pDMC2280Driv self, StateFunc func) { self->myPrevState = self->myState; self->myState = func; self->subState = 0; - if (func == DMCState_Idle) { - NetWatchRegisterTimer(&self->state_timer, - MOTOR_POLL_SLOW, - state_tmr_callback, self); - } + EvtEvent evt; + evt.event_type = eStateEvent; + self->myState(self, &evt); } static void unhandled_event(pDMC2280Driv self, pEvtEvent event) { char line[CMDLEN]; char text[CMDLEN]; - snprintf(line, CMDLEN, "Motor=%s, State=%s, unhandled event=%s", + snprintf(line, CMDLEN, "Motor=%s, State=%s(%d), unhandled event=%s", self->name, state_name(self->myState), + self->subState, event_name(event, text, CMDLEN)); SICSLogWrite(line, eStatus); } +static void handle_event(pDMC2280Driv self, pEvtEvent event) { + StateFunc oldState; + if (self->debug || self->trace) + report_event(self, event); + oldState = self->myState; + self->myState(self, event); +} + static int state_msg_callback(pAsyncTxn pCmd) { pDMC2280Driv self = (pDMC2280Driv) pCmd->cntx; @@ -1302,9 +1130,7 @@ static int state_msg_callback(pAsyncTxn pCmd) event.event_type = eMessageEvent; event.event.msg.cmd = pCmd; } - if (self->debug || self->trace) - report_event(self, &event); - self->myState(self, &event); + handle_event(self, &event); return 0; } @@ -1314,9 +1140,7 @@ static int state_tmr_callback(void* ctx, int mode) EvtEvent event; self->state_timer = 0; event.event_type = eTimerEvent; - if (self->debug || self->trace) - report_event(self, &event); - self->myState(self, &event); + handle_event(self, &event); return 0; } @@ -1324,9 +1148,7 @@ static int state_cmd_execute(pDMC2280Driv self, enum commandtype cmd) { EvtEvent event; event.event_type = eCommandEvent; event.event.cmd.cmd_type = cmd; - if (self->debug || self->trace) - report_event(self, &event); - self->myState(self, &event); + handle_event(self, &event); return 0; } @@ -1336,45 +1158,44 @@ static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event) { switch (event->event_type) { case eTimerEvent: + return; + case eStateEvent: /* Set speed */ value = motSpeed(self, self->speed); snprintf(cmd, CMDLEN, "SP%c=%d", self->axisLabel, value); - if (FAILURE == DMC2280Send(self, cmd)) + if (FAILURE == DMC2280Send(self, cmd)) { break; + } /* Set acceleration */ value = motAccel(self, self->accel); snprintf(cmd, CMDLEN, "AC%c=%d", self->axisLabel, value); - if (FAILURE == DMC2280Send(self, cmd)) + if (FAILURE == DMC2280Send(self, cmd)) { break; + } /* Set deceleration */ value = motDecel(self, self->decel); snprintf(cmd, CMDLEN, "DC%c=%d", self->axisLabel, value); - if (FAILURE == DMC2280Send(self, cmd)) + if (FAILURE == DMC2280Send(self, cmd)) { break; - request_status(self); + } + cmdStatus(self); + self->subState = 1; return; case eMessageEvent: do { pAsyncTxn pCmd = event->event.msg.cmd; - if (pCmd->out_buf[0] == 'M') { /* MG */ + if (self->subState == 1) { /* Status Request */ int iRet; - iRet = process_status(self, pCmd->inp_buf); + iRet = rspStatus(self, pCmd->inp_buf); if (iRet == 0) break; set_lastMotion(self, self->currSteps, self->currCounts); - if (self->abs_encoder == 0) { - change_state(self, DMCState_Idle); - return; - } snprintf(cmd, CMDLEN, "LV"); DMC_SendCmd(self, cmd, state_msg_callback); + self->subState = 2; return; } - if (pCmd->out_buf[0] == 'D') { /* DP */ - change_state(self, DMCState_Idle); - return; - } - if (pCmd->out_buf[0] == 'L') { /* LV */ + if (self->subState == 2) { /* LV */ if (has_var_x(self, pCmd->inp_buf, "REQ")) if (has_var_x(self, pCmd->inp_buf, "RSP")) { self->has_airpads = 2; @@ -1387,8 +1208,7 @@ static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event) { change_state(self, DMCState_Idle); return; } - } - while (0); + } while (0); break; case eTimeoutEvent: /* TODO: handle timeout */ @@ -1396,86 +1216,73 @@ static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event) { } unhandled_event(self, event); self->errorCode = STATEERROR; + change_state(self, DMCState_Error); + return; } static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) { - char cmd[CMDLEN]; switch (event->event_type) { + case eStateEvent: + if (self->state_timer) + NetWatchRemoveTimer(self->state_timer); + NetWatchRegisterTimer(&self->state_timer, + MOTOR_POLL_SLOW, + state_tmr_callback, self); + self->subState = 0; + return; case eTimerEvent: - request_status(self); - change_state(self, DMCState_WaitStatus); + cmdStatus(self); + self->subState = 1; return; case eMessageEvent: do { pAsyncTxn pCmd = event->event.msg.cmd; - if (pCmd->out_buf[0] == 'M') { /* MG _XQ0,_TSx */ - float fReply; - int iRet, iFlags; - int fwd_limit_active, rvrs_limit_active, errorlimit; - iRet = sscanf(pCmd->inp_buf, "%f %d", &fReply, &iFlags); - if (iRet != 2) + if (self->subState == 1) { /* Status Response */ + int iRet; + float fDelta; + iRet = rspStatus(self, pCmd->inp_buf); + if (iRet == 0) break; - if (fReply < 0) { - if (self->subState >= MAX_RESTARTS) { - /* TODO: handle fault logging */ - self->driver_status = HWFault; - self->errorCode = THREADZERO; - return; - } - DMC_SendCmd(self, "XQ #THREAD0,0", state_msg_callback); + /* action deferred RUN command */ + if (self->run_flag) { + self->run_flag = 0; + change_state(self, DMCState_MotorStart); return; } - /* Handle limit switches */ - fwd_limit_active = !(iFlags & STATUSFWDLIMIT); - rvrs_limit_active = !(iFlags & STATUSRVRSLIMIT); - errorlimit = (iFlags & STATUSERRORLIMIT); - if (fwd_limit_active && rvrs_limit_active) { - self->errorCode = IMPOSSIBLE_LIM_SW; - self->driver_status = HWFault; - } else if (errorlimit) { - self->errorCode = ERRORLIM; - self->driver_status = HWFault; + /* if the motor moved, update any observers */ + if (self->abs_encoder) + fDelta = fabs(self->currCounts - self->lastCounts); + else + fDelta = fabs(self->currSteps - self->lastSteps); + if (fDelta > 10) { + set_lastMotion(self, self->currSteps, self->currCounts); + report_motion(self); } - if (self->driver_status == HWFault) { - return; - } - if (self->has_airpads == 1) { - snprintf(cmd, CMDLEN, "FTUBE=1"); - } - else if (self->has_airpads == 2 || self->has_airpads == 3) { - snprintf(cmd, CMDLEN, "REQ%c=1", self->axisLabel); - } - else { - snprintf(cmd, CMDLEN, "SH%c", self->axisLabel); - } - DMC_SendCmd(self, cmd, state_msg_callback); - change_state(self, DMCState_MotorStart); + NetWatchRegisterTimer(&self->state_timer, + MOTOR_POLL_SLOW, + state_tmr_callback, self); + self->subState = 0; return; } - if (pCmd->out_buf[0] == 'X') { /* XQ #THREAD0,0 */ - snprintf(cmd, CMDLEN, "MG _XQ0,_TS%c", self->axisLabel); - DMC_SendCmd(self, cmd, state_msg_callback); - self->subState += 1; - } } while (0); break; case eCommandEvent: - if (self->state_timer) { - NetWatchRemoveTimer(self->state_timer); - self->state_timer = 0; - } switch (event->event.cmd.cmd_type) { case CMD_RUN: if (self->pMot == NULL) self->pMot = FindMotor(pServ->pSics, self->name); assert(self->pMot); self->driver_status = HWBusy; - snprintf(cmd, CMDLEN, "MG _XQ0,_TS%c", self->axisLabel); - DMC_SendCmd(self, cmd, state_msg_callback); - self->subState = 0; + if (self->subState == 0) { + change_state(self, DMCState_MotorStart); + } + else { + self->run_flag = 1; + } return; case CMD_HALT: - /* TODO: handle halt command */ + /* handle halt command */ + self->run_flag = 0; return; } break; @@ -1485,41 +1292,78 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) { } unhandled_event(self, event); self->errorCode = STATEERROR; + change_state(self, DMCState_Error); + return; } static void DMCState_MotorStart(pDMC2280Driv self, pEvtEvent event) { char cmd[CMDLEN]; switch (event->event_type) { + case eStateEvent: + if (self->state_timer) { + NetWatchRemoveTimer(self->state_timer); + self->state_timer = 0; + } + /* If Motion Control is off, report HWFault */ + if (DMC2280MotionControl != 1) { + state_cmd_execute(self, CMD_HALT); + if (DMC2280MotionControl == 0) + self->errorCode = MOTIONCONTROLOFF; + else + self->errorCode = MOTIONCONTROLUNK; + self->driver_status = HWFault; + } + else { + int iFlags; + int fwd_limit_active, rvrs_limit_active, errorlimit; + /* Handle limit switches from latest status response */ + iFlags = self->currFlags; + fwd_limit_active = !(iFlags & STATUSFWDLIMIT); + rvrs_limit_active = !(iFlags & STATUSRVRSLIMIT); + errorlimit = (iFlags & STATUSERRORLIMIT); + if (fwd_limit_active && rvrs_limit_active) { + self->errorCode = IMPOSSIBLE_LIM_SW; + self->driver_status = HWFault; + } else if (errorlimit) { + self->errorCode = ERRORLIM; + self->driver_status = HWFault; + } + } + if (self->driver_status == HWFault) { + change_state(self, DMCState_Idle); + return; + } + cmdOn(self); + self->subState = 1; + return; case eTimerEvent: if (self->has_airpads == 2 || self->has_airpads == 3) snprintf(cmd, CMDLEN, "MG RSP%c", self->axisLabel); - else + else if (self->has_airpads == 1) snprintf(cmd, CMDLEN, "MG APDONE"); DMC_SendCmd(self, cmd, state_msg_callback); + self->subState = 2; return; case eMessageEvent: do { pAsyncTxn pCmd = event->event.msg.cmd; - if (pCmd->out_buf[0] == 'S') { /* SH */ - NetWatchRegisterTimer(&self->state_timer, - ON_SETTLE_TIMER, - state_tmr_callback, self); - return; + if (self->subState == 1) { + if (self->has_airpads == 0) { /* SH */ + change_state(self, DMCState_MotorOn); + return; + } + else { + NetWatchRegisterTimer(&self->state_timer, + AIR_POLL_TIMER, + state_tmr_callback, self); + self->subState = 0; + return; + } } - else if (pCmd->out_buf[0] == 'F' || /* FTUBE */ - pCmd->out_buf[0] == 'R') { /* REQx= */ - NetWatchRegisterTimer(&self->state_timer, - AIR_POLL_TIMER, - state_tmr_callback, self); - return; - } - else if (pCmd->out_buf[0] == 'M') { /* MG APDONE/RSPx */ + else if (self->subState == 2) { /* MG APDONE/RSPx */ float fReply; fReply = (float) atof(pCmd->inp_buf); if (fReply > 0) { - NetWatchRegisterTimer(&self->state_timer, - ON_SETTLE_TIMER, - state_tmr_callback, self); change_state(self, DMCState_MotorOn); return; } @@ -1527,6 +1371,7 @@ static void DMCState_MotorStart(pDMC2280Driv self, pEvtEvent event) { NetWatchRegisterTimer(&self->state_timer, AIR_POLL_TIMER, state_tmr_callback, self); + self->subState = 0; return; } } while (0); @@ -1538,27 +1383,35 @@ static void DMCState_MotorStart(pDMC2280Driv self, pEvtEvent event) { } unhandled_event(self, event); self->errorCode = STATEERROR; + change_state(self, DMCState_Error); + return; } static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event) { - char cmd[CMDLEN]; switch (event->event_type) { + case eStateEvent: + NetWatchRegisterTimer(&self->state_timer, + ON_SETTLE_TIMER, + state_tmr_callback, self); + return; case eTimerEvent: - request_status(self); + cmdStatus(self); + self->subState = 1; return; case eMessageEvent: do { pAsyncTxn pCmd = event->event.msg.cmd; - if (pCmd->out_buf[0] == 'M') { /* MG */ + if (self->subState == 1) { /* Status Response */ int iRet, absolute; float target; - iRet = process_status(self, pCmd->inp_buf); + iRet = rspStatus(self, pCmd->inp_buf); if (iRet == 0) break; set_lastMotion(self, self->currSteps, self->currCounts); /* compute position for PA command */ target = self->fTarget; self->preseek = 0; + self->fPreseek = self->fTarget; if (self->backlash_offset) { if (self->backlash_offset > 0) { /* @@ -1571,6 +1424,7 @@ static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event) { target += self->backlash_offset; if (target > self->fUpper) target = self->fUpper; + self->fPreseek = target; } } else if (self->backlash_offset < 0) { @@ -1584,21 +1438,26 @@ static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event) { target += self->backlash_offset; if (target < self->fLower) target = self->fLower; + self->fPreseek = target; } } } + if (self->debug) { + char line[CMDLEN]; + snprintf(line, CMDLEN, "Motor=%s, Pos=%f, Preseek=%f, Target=%f\n", + self->name, + self->currPosition, + self->fPreseek, + self->fTarget); + SICSLogWrite(line, eStatus); + } self->creep_val = 0; absolute = motCreep(self, target); - /* TODO set DSTx for has_airpads == 3 */ - snprintf(cmd, CMDLEN, "PA%c=%d", self->axisLabel, absolute); - DMC_SendCmd(self, cmd, state_msg_callback); + cmdPosition(self, absolute); + self->subState = 2; return; } - else if (pCmd->out_buf[0] == 'P') { /* PA */ - self->stepCount = 1; - /* TODO set RUNx for has_airpads == 3 */ - snprintf(cmd, CMDLEN, "BG%c", self->axisLabel); - DMC_SendCmd(self, cmd, state_msg_callback); + else if (self->subState == 2) { /* PA */ change_state(self, DMCState_Moving); return; } @@ -1611,33 +1470,40 @@ static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event) { } unhandled_event(self, event); self->errorCode = STATEERROR; + change_state(self, DMCState_Error); + return; } static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) { - char cmd[CMDLEN]; switch (event->event_type) { + case eStateEvent: + cmdBegin(self); + self->stepCount = 1; + self->subState = 1; + return; case eTimerEvent: - request_status(self); + cmdStatus(self); + self->subState = 2; return; case eMessageEvent: do { pAsyncTxn pCmd = event->event.msg.cmd; - if (pCmd->out_buf[0] == 'B') { /* BG */ + if (self->subState == 1) { /* BG */ NetWatchRegisterTimer(&self->state_timer, MOTOR_POLL_FAST, state_tmr_callback, self); return; } - else if (pCmd->out_buf[0] == 'P') { /* PA */ - snprintf(cmd, CMDLEN, "BG%c", self->axisLabel); - DMC_SendCmd(self, cmd, state_msg_callback); + else if (self->subState == 3) { /* PA */ + cmdBegin(self); self->stepCount++; + self->subState = 1; return; } - else if (pCmd->out_buf[0] == 'M') { /* MG */ + else if (self->subState == 2) { /* MG */ int iRet, iFlags; bool moving, fwd_limit_active, rvrs_limit_active, errorlimit; - iRet = process_status(self, pCmd->inp_buf); + iRet = rspStatus(self, pCmd->inp_buf); if (iRet == 0) break; iFlags = self->currFlags; @@ -1645,25 +1511,26 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) { if (moving) { /* If Motion Control is off, report HWFault */ if (DMC2280MotionControl != 1) { - state_cmd_execute(self, CMD_HALT); if (DMC2280MotionControl == 0) self->errorCode = MOTIONCONTROLOFF; else self->errorCode = MOTIONCONTROLUNK; self->driver_status = HWFault; + change_state(self, DMCState_MotorHalt); return; } if (self->abs_encoder && checkMotion(self) == 0) { /* handle blocked */ - state_cmd_execute(self, CMD_HALT); self->errorCode = BLOCKED; self->driver_status = HWFault; + change_state(self, DMCState_MotorHalt); return; } NetWatchRegisterTimer(&self->state_timer, MOTOR_POLL_FAST, state_tmr_callback, self); + self->subState = 0; return; } /* Motor has stopped */ @@ -1685,7 +1552,7 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) { self->driver_status = HWFault; } if (self->driver_status == HWFault) { - state_cmd_execute(self, CMD_HALT); + change_state(self, DMCState_MotorHalt); return; } /* @@ -1696,6 +1563,7 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) { int absolute; self->preseek = 0; target = self->fTarget; + self->fPreseek = target; /* if we are not creeping it is backlash correction */ /* TODO: ensure minimum progress so it doesn't stall */ /* TODO: handle edge conditions like limits */ @@ -1709,6 +1577,7 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) { target += self->backlash_offset + precision; if (target > self->fUpper) target = self->fUpper; + self->fPreseek = target; } } else if (self->backlash_offset < 0) { @@ -1717,6 +1586,7 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) { target += self->backlash_offset - precision; if (target < self->fLower) target = self->fLower; + self->fPreseek = target; } } @@ -1724,7 +1594,7 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) { if (self->preseek && self->stepCount > 10) { if (self->debug) { char line[CMDLEN]; - snprintf(line, CMDLEN, "Motor=%s preseek stopped stepcount = %d", + snprintf(line, CMDLEN, "Motor=%s preseek stopped, stepcount = %d", self->name, self->stepCount); SICSLogWrite(line, eStatus); } @@ -1735,12 +1605,28 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) { } else { /* change of direction, reset motion check */ + if (self->debug) { + char line[CMDLEN]; + snprintf(line, CMDLEN, "Motor=%s changed direction", + self->name); + SICSLogWrite(line, eStatus); + } set_lastMotion(self, self->currSteps, self->currCounts); absolute = motCreep(self, target); } } else absolute = motCreep(self, target); + + if (self->debug && self->preseek) { + char line[CMDLEN]; + snprintf(line, CMDLEN, "Motor=%s, Pos=%f, Preseek=%f, Target=%f\n", + self->name, + self->currPosition, + self->fPreseek, + self->fTarget); + SICSLogWrite(line, eStatus); + } /* * If we are still iterating, continue */ @@ -1748,15 +1634,15 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) { if (absolute == self->currSteps) { if (self->debug) { char line[CMDLEN]; - snprintf(line, CMDLEN, "Motor=%s motion stopped absolute = %d", + snprintf(line, CMDLEN, "Motor=%s motion stopped, absolute = %d", self->name, absolute); SICSLogWrite(line, eStatus); } self->preseek = 0; } else { - snprintf(cmd, CMDLEN, "PA%c=%d", self->axisLabel, absolute); - DMC_SendCmd(self, cmd, state_msg_callback); + cmdPosition(self, absolute); + self->subState = 3; return; } } @@ -1791,8 +1677,6 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) { switch (event->event.cmd.cmd_type) { case CMD_HALT: /* handle halt command, send message */ - snprintf(cmd, CMDLEN, "ST%c", self->axisLabel); - DMC_SendCmd(self, cmd, state_msg_callback); change_state(self, DMCState_MotorHalt); return; } @@ -1801,32 +1685,41 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) { strncpy(self->lastCmd, event->event.msg.cmd->out_buf, CMDLEN); self->errorCode = MOTCMDTMO; self->driver_status = HWFault; - state_cmd_execute(self, CMD_HALT); + change_state(self, DMCState_MotorHalt); return; } unhandled_event(self, event); self->errorCode = STATEERROR; + change_state(self, DMCState_Error); + return; } -static void DMCState_MotorHalt(pDMC2280Driv self, pEvtEvent event) -{ +static void DMCState_MotorHalt(pDMC2280Driv self, pEvtEvent event) { char cmd[CMDLEN]; switch (event->event_type) { + case eStateEvent: + if (self->state_timer) + NetWatchRemoveTimer(self->state_timer); + self->state_timer = 0; + snprintf(cmd, CMDLEN, "ST%c", self->axisLabel); + DMC_SendCmd(self, cmd, state_msg_callback); + return; case eTimerEvent: - request_status(self); + cmdStatus(self); + self->subState = 1; return; case eMessageEvent: do { pAsyncTxn pCmd = event->event.msg.cmd; - if (pCmd->out_buf[0] == 'S') { /* ST */ + if (self->subState == 0) { /* ST */ NetWatchRegisterTimer(&self->state_timer, MOTOR_POLL_FAST, state_tmr_callback, self); return; } - else if (pCmd->out_buf[0] == 'M') { /* MG */ + else if (self->subState == 1) { /* Status Response */ int iRet, iFlags; - iRet = process_status(self, pCmd->inp_buf); + iRet = rspStatus(self, pCmd->inp_buf); if (iRet == 0) break; iFlags = self->currFlags; @@ -1836,24 +1729,8 @@ static void DMCState_MotorHalt(pDMC2280Driv self, pEvtEvent event) state_tmr_callback, self); return; } - if (self->has_airpads == 1) { - snprintf(cmd, CMDLEN, "FTUBE=0"); - DMC_SendCmd(self, cmd, state_msg_callback); - change_state(self, DMCState_MotorStop); - return; - } - else if (self->has_airpads == 2 || self->has_airpads == 3) { - snprintf(cmd, CMDLEN, "REQ%c=0", self->axisLabel); - DMC_SendCmd(self, cmd, state_msg_callback); - change_state(self, DMCState_MotorStop); - return; - } - else { - snprintf(cmd, CMDLEN, "MO%c", self->axisLabel); - DMC_SendCmd(self, cmd, state_msg_callback); - change_state(self, DMCState_MotorStop); - return; - } + change_state(self, DMCState_MotorStop); + return; } } while (0); break; @@ -1863,80 +1740,70 @@ static void DMCState_MotorHalt(pDMC2280Driv self, pEvtEvent event) } unhandled_event(self, event); self->errorCode = STATEERROR; + change_state(self, DMCState_Error); + return; } static void DMCState_OffTimer(pDMC2280Driv self, pEvtEvent event) { - char cmd[CMDLEN]; switch (event->event_type) { + case eStateEvent: + return; case eTimerEvent: - if (self->has_airpads == 1) { - snprintf(cmd, CMDLEN, "FTUBE=0"); - DMC_SendCmd(self, cmd, state_msg_callback); - change_state(self, DMCState_MotorStop); - } - else if (self->has_airpads == 2 || self->has_airpads == 3) { - snprintf(cmd, CMDLEN, "REQ%c=0", self->axisLabel); - DMC_SendCmd(self, cmd, state_msg_callback); - change_state(self, DMCState_MotorStop); - return; - } - else { - snprintf(cmd, CMDLEN, "MO%c", self->axisLabel); - DMC_SendCmd(self, cmd, state_msg_callback); - change_state(self, DMCState_MotorStop); - } + change_state(self, DMCState_MotorStop); return; case eCommandEvent: switch (event->event.cmd.cmd_type) { case CMD_RUN: /* handle run command, convert to motor on timer expired */ - NetWatchRemoveTimer(self->state_timer); + if (self->state_timer) + NetWatchRemoveTimer(self->state_timer); self->state_timer = 0; self->driver_status = HWBusy; change_state(self, DMCState_MotorOn); - event->event_type = eTimerEvent; - self->myState(self, event); return; case CMD_HALT: /* handle halt command, convert to motor off timer expired */ - NetWatchRemoveTimer(self->state_timer); + if (self->state_timer) + NetWatchRemoveTimer(self->state_timer); self->state_timer = 0; - event->event_type = eTimerEvent; - self->myState(self, event); + change_state(self, DMCState_MotorStop); return; } return; } unhandled_event(self, event); self->errorCode = STATEERROR; + change_state(self, DMCState_Error); + return; } static void DMCState_MotorStop(pDMC2280Driv self, pEvtEvent event) { char cmd[CMDLEN]; switch (event->event_type) { + case eStateEvent: + cmdOff(self); + return; case eTimerEvent: if (self->has_airpads == 2 || self->has_airpads == 3) snprintf(cmd, CMDLEN, "MG RSP%c", self->axisLabel); else snprintf(cmd, CMDLEN, "MG APDONE"); DMC_SendCmd(self, cmd, state_msg_callback); + self->subState = 1; return; case eMessageEvent: do { pAsyncTxn pCmd = event->event.msg.cmd; - if (pCmd->out_buf[0] == 'F' || /* FTUBE */ - pCmd->out_buf[0] == 'R') { /* REQx= */ + if (self->subState == 0) { /* Off command */ + if (self->has_airpads == 0) { + change_state(self, DMCState_MotorOff); + return; + } } - else if (pCmd->out_buf[0] == 'M') { /* MG APDONE/RSPx or MO */ + else if (self->subState == 1) { /* MG APDONE/RSPx */ float fReply; - if (pCmd->out_buf[1] == 'O') /* MO */ - fReply = 1; - else - fReply = (float) atof(pCmd->inp_buf); + fReply = (float) atof(pCmd->inp_buf); if (fReply == 0) { - NetWatchRegisterTimer(&self->state_timer, - ON_SETTLE_TIMER, - state_tmr_callback, self); change_state(self, DMCState_MotorOff); return; } @@ -1962,16 +1829,23 @@ static void DMCState_MotorStop(pDMC2280Driv self, pEvtEvent event) { } unhandled_event(self, event); self->errorCode = STATEERROR; + change_state(self, DMCState_Error); + return; } static void DMCState_MotorOff(pDMC2280Driv self, pEvtEvent event) { switch (event->event_type) { + case eStateEvent: + NetWatchRegisterTimer(&self->state_timer, + ON_SETTLE_TIMER, + state_tmr_callback, self); + return; case eTimerEvent: case eMessageEvent: /* progress to IDLE */ - change_state(self, DMCState_Idle); if (self->driver_status == HWBusy) self->driver_status = HWIdle; + change_state(self, DMCState_Idle); return; case eCommandEvent: switch (event->event.cmd.cmd_type) { @@ -1989,66 +1863,34 @@ static void DMCState_MotorOff(pDMC2280Driv self, pEvtEvent event) { } unhandled_event(self, event); self->errorCode = STATEERROR; + change_state(self, DMCState_Error); + return; } -static void DMCState_WaitStatus(pDMC2280Driv self, pEvtEvent event) { +static void DMCState_Error(pDMC2280Driv self, pEvtEvent event) { switch (event->event_type) { + case eStateEvent: + return; case eTimerEvent: case eMessageEvent: - do { - pAsyncTxn pCmd = event->event.msg.cmd; - if (pCmd->out_buf[0] == 'M') { /* MG */ - int iRet; - iRet = process_status(self, pCmd->inp_buf); - if (iRet == 0) - break; - change_state(self, DMCState_Idle); - if (self->run_flag) { - self->run_flag = 0; - state_cmd_execute(self, CMD_RUN); - } - else { - float fDelta; - if (self->abs_encoder) - fDelta = fabs(self->currCounts - self->lastCounts); - else - fDelta = fabs(self->currSteps - self->lastSteps); - if (fDelta > 10) { - SConnection *pDumCon; - MotCallback sCall; - set_lastMotion(self, self->currSteps, self->currCounts); - if (self->pMot == NULL) - self->pMot = FindMotor(pServ->pSics, self->name); - assert(self->pMot); - pDumCon = SCCreateDummyConnection(pServ->pSics); - MotorGetSoftPosition(self->pMot,pDumCon,&sCall.fVal); - SCDeleteConnection(pDumCon); - sCall.pName = self->pMot->name; - InvokeCallBack(self->pMot->pCall, MOTDRIVE, &sCall); - InvokeCallBack(self->pMot->pCall, MOTEND, &sCall); - } - } - return; - } - } while (0); - break; + return; case eCommandEvent: switch (event->event.cmd.cmd_type) { case CMD_RUN: /* TODO: handle run command */ - self->driver_status = HWBusy; - self->run_flag = 1; return; case CMD_HALT: /* TODO: handle halt command */ return; } + break; case eTimeoutEvent: /* TODO handle message timeout */ break; } unhandled_event(self, event); self->errorCode = STATEERROR; + return; } /** \brief Reads motor position, implements the GetPosition @@ -2062,36 +1904,16 @@ static void DMCState_WaitStatus(pDMC2280Driv self, pEvtEvent event) { * */ static int DMC2280GetPos(void *pData, float *fPos){ pDMC2280Driv self = NULL; -#ifndef DEPRECATED - char reply[CMDLEN]; - char cmd[CMDLEN]; - float absEncPos, motorPos; - - reply[0]='\0'; -#endif self = (pDMC2280Driv)pData; assert(self != NULL); if (self->has_fsm) { - if (self->myState == DMCState_Unknown) - SicsWait(1); +// if (self->myState == DMCState_Unknown) +// SicsWait(1); *fPos = motPosit(self); return OKOK; } -#ifndef DEPRECATED - if (1 == self->abs_encoder) { - 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; - } -#endif return OKOK; } @@ -2138,29 +1960,13 @@ static int DMC2280Run(void *pData,float fValue){ self->fTarget = fValue; - if (self->has_fsm) { - /* TODO: FIXME and handle RUN command everywhere */ - while (self->myState == DMCState_MotorStop - || self->myState == DMCState_MotorOff) { - SicsWait(1); - } - state_cmd_execute(self, CMD_RUN); - return 1; + /* TODO: FIXME and handle RUN command everywhere */ + while (self->myState == DMCState_MotorStop + || self->myState == DMCState_MotorOff) { + SicsWait(1); } - -#ifndef DEPRECATED - /* - * 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); -#else - assert(self->has_fsm); - return 0; -#endif + state_cmd_execute(self, CMD_RUN); + return 1; } @@ -2177,147 +1983,13 @@ static int DMC2280Run(void *pData,float fValue){ */ static int DMC2280Status(void *pData){ pDMC2280Driv self = NULL; -#ifndef DEPRECATED - char cmd[CMDLEN]; - int switches; - char switchesAscii[CMDLEN]; - bool moving, fwd_limit_active, rvrs_limit_active, errorlimit; -#endif self = (pDMC2280Driv)pData; assert(self != NULL); - if (self->has_fsm) { - return self->driver_status; - } -#ifndef DEPRECATED - /* - * If we are waiting for the motor or airpads then we - * are busy - */ - if (self->motoff_timer || self->airpad_timer) - return HWBusy; - - /* Make sure that speed, accel and decel are set correctly */ - /* ckSpeedAccelDecel(self); */ - /* Get status of switches - * see TS (Tell Switches) in Galil manc2xx.pdf */ - snprintf(cmd, CMDLEN, "TS%c", self->axisLabel); - if (FAILURE == DMC2280SendReceive(self, cmd, switchesAscii)) - return HWFault; - sscanf(switchesAscii, "%d", &switches); - moving = (switches & STATUSMOVING)>0; - fwd_limit_active = !((switches & STATUSFWDLIMIT)>0); - rvrs_limit_active = !((switches & STATUSRVRSLIMIT)>0); - errorlimit = (switches & STATUSERRORLIMIT)>0; - - if (fwd_limit_active && rvrs_limit_active) { - self->errorCode = IMPOSSIBLE_LIM_SW; - return HWFault; - } - if (moving) { - int iRet; - /* If Motion Control is off, report HWFault */ - if (DMC2280MotionControl != 1) { - if (DMC2280MotionControl == 0) - self->errorCode = MOTIONCONTROLOFF; - else - self->errorCode = MOTIONCONTROLUNK; - return HWFault; - } - /* If pos hasn't changed since last - * check then stop and scream */ -#if 0 - iRet = checkPosition(pData); -#else - iRet = checkMotion(pData); -#endif - if (iRet == 0) { - DMC2280Halt(pData); - self->errorCode = BLOCKED; - return HWFault; - } else { - self->errorCode = BADBSY; - return HWBusy; - } - } else { - /* If motor stopped check limits and error status */ - if (fwd_limit_active) { - self->errorCode = FWDLIM; - return HWFault; - } else if (rvrs_limit_active) { - self->errorCode = RVRSLIM; - return HWFault; - } else if (errorlimit) { - self->errorCode = ERRORLIM; - return HWFault; - } - /* - * When we get here, the motion has completed - */ - if (self->preseek) { - DMC2280RunCommon(self, self->fTarget); - return HWBusy; - } - /* 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->motoff_timer, - self->motOffDelay, - motoff_timeout, self); - return HWIdle; - } - if (!DMC_AirPads(self, 0)) - return HWFault; - 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->motoff_timer, - self->motOffDelay, - motoff_timeout, self); - return HWIdle; -#endif - } else { - snprintf(cmd, CMDLEN, "MO%c", self->axisLabel); - } - DMC2280Send(self, cmd); - } - return HWIdle; - } -#else - assert(self->has_fsm); - return 0; -#endif + return self->driver_status; } + /** \brief DMC2280 implementation of the GetError * method in the MotorDriver interface. * @@ -2486,32 +2158,12 @@ static int DMC2280Fix(void *pData, int iCode,/*@unused@*/ float fValue){ */ static int DMC2280Halt(void *pData){ pDMC2280Driv self = NULL; -#ifndef DEPRECATED - char cmd[CMDLEN]; -#endif self = (pDMC2280Driv)pData; assert(self != NULL); - if (self->has_fsm) { - state_cmd_execute(self, CMD_HALT); - return 1; - } -#ifndef DEPRECATED - /* Stop motor */ - 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; - else - return 1; -#else - assert(self->has_fsm); - return 0; -#endif + state_cmd_execute(self, CMD_HALT); + return 1; } /** \brief Fetches the value of the named parameter, @@ -2644,21 +2296,21 @@ static int DMC2280GetPar(void *pData, char *name, } } if(strcasecmp(name,"thread0") == 0) - return ReadThread(self, 0, fValue); + return readThread(self, 0, fValue); if(strcasecmp(name,"thread1") == 0) - return ReadThread(self, 1, fValue); + return readThread(self, 1, fValue); if(strcasecmp(name,"thread2") == 0) - return ReadThread(self, 2, fValue); + return readThread(self, 2, fValue); if(strcasecmp(name,"thread3") == 0) - return ReadThread(self, 3, fValue); + return readThread(self, 3, fValue); if(strcasecmp(name,"thread4") == 0) - return ReadThread(self, 4, fValue); + return readThread(self, 4, fValue); if(strcasecmp(name,"thread5") == 0) - return ReadThread(self, 5, fValue); + return readThread(self, 5, fValue); if(strcasecmp(name,"thread6") == 0) - return ReadThread(self, 6, fValue); + return readThread(self, 6, fValue); if(strcasecmp(name,"thread7") == 0) - return ReadThread(self, 7, fValue); + return readThread(self, 7, fValue); return 0; } @@ -3160,7 +2812,7 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) { pNew->blockage_ratio = 2.0; /* maximum ratio observed/expected */ pNew->blockage_fail = 1; /* fail the motor */ pNew->backlash_offset = 0.0; - pNew->myState = DMCState_Unknown; + pNew->myState = NULL; /* PARAMETERS: Fetch parameter values */ @@ -3313,32 +2965,7 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) { } } - if (pNew->has_fsm) { - /* Schedule a timer event as soon as possible */ - NetWatchRegisterTimer(&pNew->state_timer, - 0, - state_tmr_callback, pNew); - } -#ifndef DEPRECATED - else { - char cmd[CMDLEN]; - /* 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 - */ - } -#endif + change_state(pNew, DMCState_Unknown); return (MotorDriver *)pNew; } @@ -3488,8 +3115,10 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData, } else if(self->has_fsm && strcasecmp("state", argv[1]) == 0) { char line[132]; - snprintf(line, 132, "%s.state = %s (timer=%s)", - self->name, state_name(self->myState), + snprintf(line, 132, "%s.state = %s(%d) (timer=%s)", + self->name, + state_name(self->myState), + self->subState, self->state_timer ? "active" : "inactive"); SCWrite(pCon, line, eValue); return 1;