From 04568a34f2d9a93300f06bf6469e5e4e64c1cff3 Mon Sep 17 00:00:00 2001 From: Douglas Clowes Date: Mon, 10 Mar 2008 08:19:23 +1100 Subject: [PATCH] Bugfixes from commissioning. Use _SCx, send STx and HLTx=1. Fix missing TRACE output. Change 'has_airpads' to 'protocol'. r2389 | dcl | 2008-03-10 08:19:23 +1100 (Mon, 10 Mar 2008) | 2 lines --- site_ansto/motor_dmc2280.c | 190 ++++++++++++++++++++++++------------- 1 file changed, 124 insertions(+), 66 deletions(-) diff --git a/site_ansto/motor_dmc2280.c b/site_ansto/motor_dmc2280.c index 47790008..a76ad7c5 100644 --- a/site_ansto/motor_dmc2280.c +++ b/site_ansto/motor_dmc2280.c @@ -89,6 +89,7 @@ struct EvtEvent_s { #define VAR_ENC (1<<6) #define VAR_SWI (1<<7) #define VAR_HOM (1<<8) +#define VAR_STP (1<<9) static pAsyncProtocol DMC2280_Protocol = NULL; @@ -154,8 +155,8 @@ struct __MoDriv { int lastSteps; int lastCounts; int thread0; /**< last read of _XQ0 */ - unsigned short int input0; /**< last read of _TI0 */ - unsigned short int input1; /**< last read of _TI1 */ + unsigned short int stopCode; /**< last read of _SCx */ + unsigned short int inputByte; /**< last read of _TIx */ bool ampError; /**< amplifier error */ bool runError; /**< motor error */ bool threadError; /**< thread error */ @@ -165,7 +166,7 @@ struct __MoDriv { float blockage_thresh; /**< motion threshold for blockage checking */ float blockage_ratio; /**< ratio steps/counts must be between 1/this and this */ int blockage_fail; /**< flag =1 if we should fail the motor */ - int has_airpads; /**< Flag = 1 if there is are airpads for this motor */ + int protocol; /**< protocol version 0..3 */ float backlash_offset; /**< signed offset to drive from */ double fTarget; /**< target passed from SICS to timer callback */ double fPreseek; /**< preseek target when preseek is active */ @@ -267,6 +268,7 @@ int DMC2280MotionControl = 1; /* defaults to enabled */ /* State Machine Events */ +static int state_snd_callback(pAsyncTxn pCmd); static int state_msg_callback(pAsyncTxn pCmd); static int state_tmr_callback(void* ctx, int mode); static int state_cmd_execute(pDMC2280Driv self, enum commandtype cmd); @@ -913,7 +915,7 @@ static int SendCallback(pAsyncTxn pCmd) { static int DMC_Send(pDMC2280Driv self, char *command) { return AsyncUnitSendTxn(self->asyncUnit, command, strlen(command), - SendCallback, self, CMDLEN); + state_snd_callback, self, CMDLEN); } /** @@ -1210,16 +1212,23 @@ static bool has_var_x(pDMC2280Driv self, const char* vars, const char* name) { static int cmdStatus(pDMC2280Driv self) { char cmd[CMDLEN]; char encoder = self->axisLabel; + int io_byte = 0; if (self->encoderAxis && !(self->variables & VAR_ENC)) encoder = self->encoderAxis; + if (self->axisLabel >= 'A' && self->axisLabel <= 'D') + io_byte = 0; + else + io_byte = 1; - /* TODO: Use POSx, ENCx, RUNx, SWIx if it has these variables */ - snprintf(cmd, CMDLEN, "MG {F10.0} %s%c,%s%c,%s%c,%s%c,_TI0,_TI1,_XQ0", + /* Use POSx, ENCx, RUNx, SWIx if it has these variables */ + snprintf(cmd, CMDLEN, "MG {F10.0} %s%c,%s%c,%s%c,%s%c,%s%c,_TI%d,_XQ0", (self->variables & VAR_POS) ? "POS" : "_TD", self->axisLabel, (self->variables & VAR_ENC) ? "ENC" : "_TP", encoder, (self->variables & VAR_SWI) ? "SWI" : "_TS", self->axisLabel, - (self->variables & VAR_RUN) ? "RUN" : "_BG", self->axisLabel); + (self->variables & VAR_RUN) ? "RUN" : "_BG", self->axisLabel, + (self->variables & VAR_STP) ? "STP" : "_SC", self->axisLabel, + io_byte); return DMC_SendReq(self, cmd); } @@ -1231,10 +1240,10 @@ static int cmdVars(pDMC2280Driv self) { static int cmdOn(pDMC2280Driv self) { char cmd[CMDLEN]; - if (self->has_airpads == 1) { + if (self->protocol == 1) { snprintf(cmd, CMDLEN, "FTUBE=1"); } - else if (self->has_airpads == 2 || self->has_airpads == 3) { + else if (self->protocol == 2 || self->protocol == 3) { snprintf(cmd, CMDLEN, "REQ%c=1", self->axisLabel); } else { @@ -1245,7 +1254,7 @@ static int cmdOn(pDMC2280Driv self) { static int cmdPosition(pDMC2280Driv self, int target) { char cmd[CMDLEN]; - if (self->has_airpads == 3) + if (self->protocol == 3) snprintf(cmd, CMDLEN, "DST%c=%d", self->axisLabel, target); else snprintf(cmd, CMDLEN, "PA%c=%d", self->axisLabel, target); @@ -1254,7 +1263,7 @@ static int cmdPosition(pDMC2280Driv self, int target) { static int cmdBegin(pDMC2280Driv self) { char cmd[CMDLEN]; - if (self->has_airpads == 3) + if (self->protocol == 3) snprintf(cmd, CMDLEN, "RUN%c=1", self->axisLabel); else snprintf(cmd, CMDLEN, "BG%c", self->axisLabel); @@ -1263,28 +1272,29 @@ static int cmdBegin(pDMC2280Driv self) { static int cmdPoll(pDMC2280Driv self) { char cmd[CMDLEN]; - if (self->has_airpads == 2 || self->has_airpads == 3) + if (self->protocol == 2 || self->protocol == 3) snprintf(cmd, CMDLEN, "MG RSP%c", self->axisLabel); - else if (self->has_airpads == 1) + else if (self->protocol == 1) snprintf(cmd, CMDLEN, "MG APDONE"); return DMC_SendReq(self, cmd); } -static int cmdHalt(pDMC2280Driv self) { +static void cmdHalt(pDMC2280Driv self) { char cmd[CMDLEN]; - if (self->variables & VAR_HLT) + snprintf(cmd, CMDLEN, "ST%c", self->axisLabel); + (void) DMC_Send(self, cmd); + if (self->variables & VAR_HLT) { snprintf(cmd, CMDLEN, "HLT%c=1", self->axisLabel); - else - snprintf(cmd, CMDLEN, "ST%c", self->axisLabel); - return DMC_SendReq(self, cmd); + (void) DMC_Send(self, cmd); + } } static int cmdOff(pDMC2280Driv self) { char cmd[CMDLEN]; - if (self->has_airpads == 1) { + if (self->protocol == 1) { snprintf(cmd, CMDLEN, "FTUBE=0"); } - else if (self->has_airpads == 2 || self->has_airpads == 3) { + else if (self->protocol == 2 || self->protocol == 3) { snprintf(cmd, CMDLEN, "REQ%c=0", self->axisLabel); } else { @@ -1296,11 +1306,10 @@ static int cmdOff(pDMC2280Driv self) { static int rspStatus(pDMC2280Driv self, const char* text) { int iRet, iFlags; int iSteps, iCounts; - int iTI0, iTI1, iXQ0, iBG; - /* TODO: add _RUNx for has_airpads == 3 */ + int iIOByte, iStopCode, iXQ0, iBG; iRet = sscanf(text, "%d %d %d %d %d %d %d", &iSteps, &iCounts, &iFlags, &iBG, - &iTI0, &iTI1, &iXQ0); + &iStopCode, &iIOByte, &iXQ0); if (iRet != 7) return 0; self->currFlags = iFlags; @@ -1308,21 +1317,27 @@ static int rspStatus(pDMC2280Driv self, const char* text) { self->currCounts = iCounts; self->currPosition = motPosit(self); self->thread0 = iXQ0; - self->input0 = iTI0; - self->input1 = iTI1; + self->stopCode = iStopCode; + self->inputByte = iIOByte; #ifdef AMPERROR if (self->variables & VAR_RUN) self->ampError = 0; else if (self->axisLabel >= 'A' && self->axisLabel <= 'D') - self->ampError = !(1 & (iTI0 >> (self->axisLabel - 'A'))); + self->ampError = !(1 & (iIOByte >> (self->axisLabel - 'A'))); else - self->ampError = !(1 & (iTI1 >> (self->axisLabel - 'E'))); + self->ampError = !(1 & (iIOByte >> (self->axisLabel - 'E'))); #endif self->runError = iBG < 0 ? iBG : 0; if (iBG < 0) snprintf(self->dmc2280Error, CMDLEN, "MOTOR CONTROLLER RUN ERROR: %d", iBG); self->threadError = self->thread0 < 0; self->moving = iBG > 0; + if (self->protocol == 3 && !(self->variables & VAR_STP)) { + if (self->moving) + self->stopCode = 0; + else + self->stopCode = 1; + } return 1; } @@ -1346,13 +1361,15 @@ static int rspVars(pDMC2280Driv self, const char* text) { self->variables |= VAR_ENC; if (has_var_x(self, text, "SWI")) self->variables |= VAR_SWI; + if (has_var_x(self, text, "STP")) + self->variables |= VAR_STP; if ((self->variables & VAR_REQ) && (self->variables & VAR_RSP)) { - self->has_airpads = 2; + self->protocol = 2; if ((self->variables & VAR_RUN) && (self->variables & VAR_DST) && (self->variables & VAR_POS)) { - self->has_airpads = 3; + self->protocol = 3; } } return 1; @@ -1361,8 +1378,8 @@ static int rspVars(pDMC2280Driv self, const char* text) { static int rspPoll(pDMC2280Driv self, const char* text) { int iReply = atoi(text); if (iReply < 0) - snprintf(self->dmc2280Error, CMDLEN, - "MOTOR CONTROLLER REQ ERROR: %d", iReply); + snprintf(self->dmc2280Error, CMDLEN, "MOTOR CONTROLLER REQ ERROR: %d", + iReply); return iReply; } @@ -1550,23 +1567,46 @@ static void handle_event(pDMC2280Driv self, pEvtEvent event) { self->myState(self, event); } +static int state_snd_callback(pAsyncTxn pCmd) { + pDMC2280Driv self = (pDMC2280Driv) pCmd->cntx; + SendCallback(pCmd); +#if defined(STATE_TRACE) && (STATE_TRACE > 0) + char* line; + char text[CMDLEN]; + gettimeofday(&self->state_trace_time[self->state_trace_idx], NULL); + line = &self->state_trace_text[self->state_trace_idx++][0]; + if (self->state_trace_idx >= STATE_TRACE) + self->state_trace_idx = 0; +#else + char line[CMDLEN]; + char text[CMDLEN]; +#endif + text[0] = '\0'; + str_n_cat(text, CMDLEN, pCmd->out_buf); + str_n_cat(text, CMDLEN, "|"); + str_n_cat(text, CMDLEN, pCmd->inp_buf); + snprintf(line, CMDLEN, "Motor=%s, State=%s(%d), send complete=%s", + self->name, + state_name(self->myState), + self->subState, + text); + if (self->debug) + SICSLogWrite(line, eStatus); + if (self->trace) + SCWrite(self->trace, line, eStatus); + return 0; +} + static int state_msg_callback(pAsyncTxn pCmd) { pDMC2280Driv self = (pDMC2280Driv) pCmd->cntx; EvtEvent event; + SendCallback(pCmd); if (pCmd->txn_status == ATX_TIMEOUT) { - if (self->debug) { - SICSLogWrite(pCmd->out_buf, eStatus); - SICSLogWrite("", eStatus); - } event.event_type = eTimeoutEvent; event.event.msg.cmd = pCmd; } else { - if (self->debug) { - SICSLogWrite(pCmd->out_buf, eStatus); - SICSLogWrite(pCmd->inp_buf, eStatus); - } event.event_type = eMessageEvent; event.event.msg.cmd = pCmd; } @@ -1600,6 +1640,7 @@ static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event) { case eStateEvent: self->run_flag = 0; self->driver_status = HWIdle; + self->errorCode = 0; /* Set speed */ value = motSpeed(self, self->speed); snprintf(cmd, CMDLEN, "SP%c=%d", self->axisLabel, value); @@ -1690,7 +1731,7 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) { if (self->variables & VAR_RUN) { char cmd[CMDLEN]; snprintf(cmd, CMDLEN, "RUN%c=0", self->axisLabel); - DMC_Send(self, cmd); + (void) DMC_Send(self, cmd); } NetWatchRegisterTimer(&self->state_timer, 0, @@ -1840,7 +1881,7 @@ static void DMCState_MotorStart(pDMC2280Driv self, pEvtEvent event) { do { pAsyncTxn pCmd = event->event.msg.cmd; if (self->subState == 1) { - if (self->has_airpads == 0) { /* cmdOn */ + if (self->protocol == 0) { /* cmdOn */ change_state(self, DMCState_MotorOn); return; } @@ -2075,6 +2116,7 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) { do { pAsyncTxn pCmd = event->event.msg.cmd; if (self->subState == 1) { /* BG */ + /* TODO: Check if BG worked (reply != '?') */ NetWatchRegisterTimer(&self->state_timer, MOTOR_POLL_FAST, state_tmr_callback, self); @@ -2112,6 +2154,27 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) { return; } #endif + if (self->stopCode != 0 && self->stopCode != 1) { + if (self->stopCode == 2) { + self->errorCode = FWDLIM; + self->driver_status = HWFault; + change_state(self, DMCState_OffTimer); + return; + } else if (self->stopCode == 3) { + self->errorCode = RVRSLIM; + self->driver_status = HWFault; + change_state(self, DMCState_OffTimer); + return; + } + else { + self->errorCode = RUNERROR; + snprintf(self->dmc2280Error, CMDLEN, "BAD Stop Code %d", + self->stopCode); + self->driver_status = HWFault; + change_state(self, DMCState_OffTimer); + return; + } + } if (self->moving) { /* If Motion Control is off, report HWFault */ if (DMC2280MotionControl != 1) { @@ -2146,18 +2209,12 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) { if (fwd_limit_active && rvrs_limit_active) { self->errorCode = IMPOSSIBLE_LIM_SW; self->driver_status = HWFault; - } else if (fwd_limit_active) { - self->errorCode = FWDLIM; - self->driver_status = HWFault; - } else if (rvrs_limit_active) { - self->errorCode = RVRSLIM; - self->driver_status = HWFault; } else if (errorlimit) { self->errorCode = ERRORLIM; self->driver_status = HWFault; } if (self->driver_status == HWFault) { - change_state(self, DMCState_MotorHalt); + change_state(self, DMCState_OffTimer); return; } /* @@ -2310,7 +2367,7 @@ static void DMCState_MotorHalt(pDMC2280Driv self, pEvtEvent event) { NetWatchRemoveTimer(self->state_timer); self->state_timer = 0; cmdHalt(self); - return; + /* Note fall through */ case eTimerEvent: cmdStatus(self); self->subState = 1; @@ -2318,13 +2375,7 @@ static void DMCState_MotorHalt(pDMC2280Driv self, pEvtEvent event) { case eMessageEvent: do { pAsyncTxn pCmd = event->event.msg.cmd; - if (self->subState == 0) { /* ST */ - NetWatchRegisterTimer(&self->state_timer, - MOTOR_POLL_FAST, - state_tmr_callback, self); - return; - } - else if (self->subState == 1) { /* Status Response */ + if (self->subState == 1) { /* Status Response */ int iRet; iRet = rspStatus(self, pCmd->inp_buf); if (iRet == 0) @@ -2373,7 +2424,8 @@ static void DMCState_OffTimer(pDMC2280Driv self, pEvtEvent event) { switch (event->event_type) { case eStateEvent: if (self->run_flag == 0) { - self->driver_status = HWIdle; + if (self->driver_status == HWBusy) + self->driver_status = HWIdle; if (self->motOffDelay) { NetWatchRegisterTimer(&self->state_timer, self->motOffDelay, @@ -2443,7 +2495,7 @@ static void DMCState_MotorStop(pDMC2280Driv self, pEvtEvent event) { do { pAsyncTxn pCmd = event->event.msg.cmd; if (self->subState == 0) { /* Off command */ - if (self->has_airpads == 0) { + if (self->protocol == 0) { change_state(self, DMCState_Idle); return; } @@ -2473,6 +2525,8 @@ static void DMCState_MotorStop(pDMC2280Driv self, pEvtEvent event) { switch (event->event.cmd.cmd_type) { case CMD_RUN: /* handle run command */ + if (self->driver_status == HWIdle) + self->driver_status = HWBusy; if (self->waitResponse == false) { self->run_flag = 0; change_state(self, DMCState_MotorStart); @@ -2882,8 +2936,8 @@ static int DMC2280GetPar(void *pData, char *name, *fValue = self->settle; return 1; } - if(strcasecmp(name,AIRPADS) == 0) { - *fValue = self->has_airpads; + if(strcasecmp(name,AIRPADS) == 0 || strcasecmp(name,"protocol") == 0) { + *fValue = self->protocol; return 1; } if(strcasecmp(name,BLOCKAGE_CHECK_INTERVAL) == 0) { @@ -3055,11 +3109,11 @@ static int DMC2280SetPar(void *pData, SConnection *pCon, } /* Set airpads, managers only */ - if(strcasecmp(name,AIRPADS) == 0) { + if(strcasecmp(name,AIRPADS) == 0 || strcasecmp(name,"protocol") == 0) { if(!SCMatchRights(pCon,usMugger)) return 1; else { - self->has_airpads = newValue; + self->protocol = newValue; return 1; } } @@ -3294,7 +3348,7 @@ static void DMC2280List(void *pData, char *name, SConnection *pCon){ SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.Backlash_offset = %f\n", name, self->backlash_offset); SCWrite(pCon, buffer, eStatus); - snprintf(buffer, BUFFLEN, "%s.AirPads = %d\n", name, self->has_airpads); + snprintf(buffer, BUFFLEN, "%s.Protocol = %d\n", name, self->protocol); SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.absEnc = %d\n", name, self->abs_encoder); SCWrite(pCon, buffer, eStatus); @@ -3625,10 +3679,14 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) { } /* AIRPADS: this motor need airpads */ - if ((pPtr=getParam(pCon, interp, params,"airpads",_OPTIONAL)) == NULL) - pNew->has_airpads=0; + if ((pPtr=getParam(pCon, interp, params,"airpads",_OPTIONAL)) == NULL) { + if ((pPtr=getParam(pCon, interp, params,"protocol",_OPTIONAL)) == NULL) + pNew->protocol=0; + else + sscanf(pPtr,"%d",&(pNew->protocol)); + } else { - sscanf(pPtr,"%d",&(pNew->has_airpads)); + sscanf(pPtr,"%d",&(pNew->protocol)); } /* ABSENC: If the parameter requires an abs enc add it to the else block */