diff --git a/site_ansto/motor_dmc2280.c b/site_ansto/motor_dmc2280.c index eb1d1a3a..3ec72255 100644 --- a/site_ansto/motor_dmc2280.c +++ b/site_ansto/motor_dmc2280.c @@ -220,6 +220,7 @@ int DMC2280MotionControl = 1; /* defaults to enabled */ #define MOTCMDTMO -19 /* Motor Command Timeout */ #define STATEERROR -20 #define THREADZERO -21 /* Thread zero not running */ +//#define AMPERROR -22 /* Amplifier OK not asserted */ /*--------------------------------------------------------------------*/ #define STATUSMOVING 128 /* Motor is moving */ #define STATUSERRORLIMIT 64 /* Number of errors exceed limit */ @@ -1178,7 +1179,7 @@ static bool has_var_x(pDMC2280Driv self, const char* vars, const char* name) { return false; /* found partial (ZVVVx=) only */ } -/* State Callbacks */ +/* State Machine Events */ static int state_msg_callback(pAsyncTxn pCmd); static int state_tmr_callback(void* ctx, int mode); @@ -1193,7 +1194,7 @@ static int cmdStatus(pDMC2280Driv self) { char cmd[CMDLEN]; if (self->has_airpads == 3) - snprintf(cmd, CMDLEN, "MG {F10.0} _TD%c,_TP%c,_TS%c,_RUN%c,_TI0,_TI1,_XQ0", + 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", @@ -1292,7 +1293,7 @@ static int rspStatus(pDMC2280Driv self, const char* text) { self->thread0 = iXQ0; self->input0 = iTI0; self->input1 = iTI1; - if (self->axisLabel >= 'A' && self->axisLabel >= 'D') + if (self->axisLabel >= 'A' && self->axisLabel <= 'D') self->ampError = !(1 & (iTI0 >> (self->axisLabel - 'A'))); else self->ampError = !(1 & (iTI1 >> (self->axisLabel - 'E'))); @@ -1664,6 +1665,16 @@ static void DMCState_MotorStart(pDMC2280Driv self, pEvtEvent event) { self->errorCode = MOTIONCONTROLUNK; self->driver_status = HWFault; } + else if (self->threadError) { + self->errorCode = THREADZERO; + self->driver_status = HWFault; + } +#ifdef AMPERROR + else if (self->ampError) { + self->errorCode = AMPERROR; + self->driver_status = HWFault; + } +#endif else { int iFlags; int fwd_limit_active, rvrs_limit_active, errorlimit; @@ -1754,7 +1765,20 @@ static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event) { iRet = rspStatus(self, pCmd->inp_buf); if (iRet == 0) break; - /* TODO AMPERR */ + if (self->threadError) { + self->errorCode = THREADZERO; + self->driver_status = HWFault; + change_state(self, DMCState_Error); + return; + } +#ifdef AMPERROR + else if (self->ampError) { + self->errorCode = AMPERROR; + self->driver_status = HWFault; + change_state(self, DMCState_Error); + return; + } +#endif set_lastMotion(self, self->currSteps, self->currCounts); /* compute position for PA command */ target = self->fTarget; @@ -1854,7 +1878,20 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) { iRet = rspStatus(self, pCmd->inp_buf); if (iRet == 0) break; - /* TODO AMPERR */ + if (self->threadError) { + self->errorCode = THREADZERO; + self->driver_status = HWFault; + change_state(self, DMCState_Error); + return; + } +#ifdef AMPERROR + else if (self->ampError) { + self->errorCode = AMPERROR; + self->driver_status = HWFault; + change_state(self, DMCState_Error); + return; + } +#endif iFlags = self->currFlags; moving = (iFlags & STATUSMOVING); if (moving) { @@ -2427,6 +2464,11 @@ static void DMC2280Error(void *pData, int *iCode, char *error, int errLen){ case THREADZERO: strncpy(error, "THREAD ZERO NOT RUNNING ON CONTROLLER", (size_t)errLen); break; +#ifdef AMPERROR + case AMPERROR: + strncpy(error, "DRIVE AMPLIFIER ERROR", (size_t)errLen); + break; +#endif default: /* FIXME What's the default */ snprintf(error, (size_t)errLen, "Unknown Motor Error: %d", self->errorCode); @@ -2461,6 +2503,9 @@ static int DMC2280Fix(void *pData, int iCode,/*@unused@*/ float fValue){ case MOTIONCONTROLUNK: case MOTCMDTMO: case THREADZERO: +#ifdef AMPERROR + case AMPERROR: +#endif return MOTFAIL; case POSFAULT: #if HAS_RS232