diff --git a/site_ansto/motor_dmc2280.c b/site_ansto/motor_dmc2280.c index 3ec72255..4bf76474 100644 --- a/site_ansto/motor_dmc2280.c +++ b/site_ansto/motor_dmc2280.c @@ -78,12 +78,13 @@ struct EvtEvent_s { } event; }; -#define VAR_REQ 1 -#define VAR_RSP 2 -#define VAR_RUN 4 -#define VAR_DST 8 -#define VAR_POS 16 -#define VAR_HLT 32 +#define VAR_REQ (1<<0) +#define VAR_RSP (1<<1) +#define VAR_RUN (1<<2) +#define VAR_DST (1<<3) +#define VAR_POS (1<<4) +#define VAR_HLT (1<<5) +#define VAR_ENC (1<<6) static pAsyncProtocol DMC2280_Protocol = NULL; @@ -151,6 +152,7 @@ struct __MoDriv { unsigned short int input0; /**< last read of _TI0 */ unsigned short int input1; /**< last read of _TI1 */ bool ampError; /**< amplifier error */ + bool runError; /**< motor error */ bool threadError; /**< thread error */ bool moving; /**< true if moving */ struct timeval time_lastPos_set; /**< Time when lastPosition was set */ @@ -221,6 +223,7 @@ int DMC2280MotionControl = 1; /* defaults to enabled */ #define STATEERROR -20 #define THREADZERO -21 /* Thread zero not running */ //#define AMPERROR -22 /* Amplifier OK not asserted */ +#define RUNERROR -23 /* Error from RUN variable */ /*--------------------------------------------------------------------*/ #define STATUSMOVING 128 /* Motor is moving */ #define STATUSERRORLIMIT 64 /* Number of errors exceed limit */ @@ -1173,7 +1176,7 @@ static bool has_var_x(pDMC2280Driv self, const char* vars, const char* name) { if (p == NULL) /* not found at all */ return false; if (p == vars) /* found at start of first line */ - return false; + return true; if (p[-1] == '\n') /* found at start of another line */ return true; return false; /* found partial (ZVVVx=) only */ @@ -1190,6 +1193,19 @@ static int state_cmd_execute(pDMC2280Driv self, enum commandtype cmd); * * \param self access to the drive data */ +#if 1 +static int cmdStatus(pDMC2280Driv self) { + char cmd[CMDLEN]; + + /* TODO: Use POSx, ENCx, RUNx if it has these variables */ + snprintf(cmd, CMDLEN, "MG {F10.0} %s%c,%s%c,_TS%c,%s%c,_TI0,_TI1,_XQ0", + (self->variables & VAR_POS) ? "POS" : "_TD", self->axisLabel, + (self->variables & VAR_ENC) ? "ENC" : "_TP", self->axisLabel, + self->axisLabel, + (self->variables & VAR_RUN) ? "RUN" : "_BG", self->axisLabel); + return DMC_SendCmd(self, cmd, state_msg_callback); +} +#else static int cmdStatus(pDMC2280Driv self) { char cmd[CMDLEN]; @@ -1201,6 +1217,7 @@ static int cmdStatus(pDMC2280Driv self) { self->axisLabel, self->axisLabel, self->axisLabel, self->axisLabel); return DMC_SendCmd(self, cmd, state_msg_callback); } +#endif static int cmdVars(pDMC2280Driv self) { char cmd[CMDLEN]; @@ -1293,10 +1310,17 @@ static int rspStatus(pDMC2280Driv self, const char* text) { self->thread0 = iXQ0; self->input0 = iTI0; self->input1 = iTI1; - if (self->axisLabel >= 'A' && self->axisLabel <= 'D') +#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'))); else self->ampError = !(1 & (iTI1 >> (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; return 1; @@ -1316,6 +1340,8 @@ static int rspVars(pDMC2280Driv self, const char* text) { self->variables |= VAR_POS; if (has_var_x(self, text, "HLT")) self->variables |= VAR_HLT; + if (has_var_x(self, text, "ENC")) + self->variables |= VAR_ENC; if ((self->variables & VAR_REQ) && (self->variables & VAR_RSP)) { self->has_airpads = 2; @@ -1577,9 +1603,15 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) { case eStateEvent: if (self->state_timer) NetWatchRemoveTimer(self->state_timer); - NetWatchRegisterTimer(&self->state_timer, - MOTOR_POLL_SLOW, - state_tmr_callback, self); + if ((self->variables & VAR_RUN) && self->runError) { + char cmd[CMDLEN]; + snprintf(cmd, CMDLEN, "RUN%c=0", self->axisLabel); + DMC_SendCmd(self, cmd, state_msg_callback); + } + else + NetWatchRegisterTimer(&self->state_timer, + MOTOR_POLL_SLOW, + state_tmr_callback, self); self->subState = 0; return; case eTimerEvent: @@ -1589,6 +1621,12 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) { case eMessageEvent: do { pAsyncTxn pCmd = event->event.msg.cmd; + if (self->subState == 0) { /* RUNx reset */ + NetWatchRegisterTimer(&self->state_timer, + MOTOR_POLL_SLOW, + state_tmr_callback, self); + return; + } if (self->subState == 1) { /* Status Response */ int iRet; float fDelta; @@ -1649,7 +1687,6 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) { } static void DMCState_MotorStart(pDMC2280Driv self, pEvtEvent event) { - char cmd[CMDLEN]; switch (event->event_type) { case eStateEvent: if (self->state_timer) { @@ -1669,6 +1706,10 @@ static void DMCState_MotorStart(pDMC2280Driv self, pEvtEvent event) { self->errorCode = THREADZERO; self->driver_status = HWFault; } + else if (self->runError) { + self->errorCode = RUNERROR; + self->driver_status = HWFault; + } #ifdef AMPERROR else if (self->ampError) { self->errorCode = AMPERROR; @@ -1768,14 +1809,20 @@ static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event) { if (self->threadError) { self->errorCode = THREADZERO; self->driver_status = HWFault; - change_state(self, DMCState_Error); + change_state(self, DMCState_MotorStop); + return; + } + else if (self->runError) { + self->errorCode = RUNERROR; + self->driver_status = HWFault; + change_state(self, DMCState_MotorStop); return; } #ifdef AMPERROR else if (self->ampError) { self->errorCode = AMPERROR; self->driver_status = HWFault; - change_state(self, DMCState_Error); + change_state(self, DMCState_MotorStop); return; } #endif @@ -1874,27 +1921,31 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) { } else if (self->subState == 2) { /* MG */ int iRet, iFlags; - bool moving, fwd_limit_active, rvrs_limit_active, errorlimit; + bool fwd_limit_active, rvrs_limit_active, errorlimit; iRet = rspStatus(self, pCmd->inp_buf); if (iRet == 0) break; if (self->threadError) { self->errorCode = THREADZERO; self->driver_status = HWFault; - change_state(self, DMCState_Error); + change_state(self, DMCState_MotorHalt); + return; + } + else if (self->runError) { + self->errorCode = RUNERROR; + self->driver_status = HWFault; + change_state(self, DMCState_MotorHalt); return; } #ifdef AMPERROR else if (self->ampError) { self->errorCode = AMPERROR; self->driver_status = HWFault; - change_state(self, DMCState_Error); + change_state(self, DMCState_MotorHalt); return; } #endif - iFlags = self->currFlags; - moving = (iFlags & STATUSMOVING); - if (moving) { + if (self->moving) { /* If Motion Control is off, report HWFault */ if (DMC2280MotionControl != 1) { if (DMC2280MotionControl == 0) @@ -1921,6 +1972,7 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) { } /* Motor has stopped */ /* Handle limit switches */ + iFlags = self->currFlags; fwd_limit_active = !(iFlags & STATUSFWDLIMIT); rvrs_limit_active = !(iFlags & STATUSRVRSLIMIT); errorlimit = (iFlags & STATUSERRORLIMIT); @@ -2082,7 +2134,6 @@ static void DMCState_Moving(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) @@ -2104,12 +2155,11 @@ static void DMCState_MotorHalt(pDMC2280Driv self, pEvtEvent event) { return; } else if (self->subState == 1) { /* Status Response */ - int iRet, iFlags; + int iRet; iRet = rspStatus(self, pCmd->inp_buf); if (iRet == 0) break; - iFlags = self->currFlags; - if (iFlags & STATUSMOVING) { + if (self->moving) { NetWatchRegisterTimer(&self->state_timer, MOTOR_POLL_FAST, state_tmr_callback, self); @@ -2164,7 +2214,6 @@ static void DMCState_OffTimer(pDMC2280Driv self, pEvtEvent event) { } static void DMCState_MotorStop(pDMC2280Driv self, pEvtEvent event) { - char cmd[CMDLEN]; switch (event->event_type) { case eStateEvent: cmdOff(self); @@ -2469,6 +2518,9 @@ static void DMC2280Error(void *pData, int *iCode, char *error, int errLen){ strncpy(error, "DRIVE AMPLIFIER ERROR", (size_t)errLen); break; #endif + case RUNERROR: + strncpy(error, self->dmc2280Error, (size_t)errLen); + break; default: /* FIXME What's the default */ snprintf(error, (size_t)errLen, "Unknown Motor Error: %d", self->errorCode); @@ -2506,6 +2558,7 @@ static int DMC2280Fix(void *pData, int iCode,/*@unused@*/ float fValue){ #ifdef AMPERROR case AMPERROR: #endif + case RUNERROR: return MOTFAIL; case POSFAULT: #if HAS_RS232