diff --git a/site_ansto/motor_dmc2280.c b/site_ansto/motor_dmc2280.c index 694a76b9..fb59caca 100644 --- a/site_ansto/motor_dmc2280.c +++ b/site_ansto/motor_dmc2280.c @@ -30,6 +30,7 @@ #include #include #include +#include #include "anstoutil.h" #include "action.h" @@ -334,6 +335,16 @@ static int DMC2280Halt(void *pData); static int DMC2280SetPar(void *pData, SConnection *pCon, char *name, float newValue); +static double getMotorParam(pDMC2280Driv self, char *param) { + int iRet = 0; + float fParam = 0.0; + iRet = MotorGetPar(self->pMot, param, &fParam); + if (!iRet) { + SICSLogPrintf(eLogError, "Motor parameter not found: \"%s\"", param); + } + return fParam; +} + static bool check_positions(pDMC2280Driv self) { char line[CMDLEN]; int missing = 0; @@ -3062,6 +3073,8 @@ static void DMCState_StepMove(pDMC2280Driv self, pEvtEvent event) { return; } else if (self->subState == 2) { /* Status */ int iRet; + /* update the stop time now in case of error exit */ + self->moveStopTime = DoubleTime(); /* parse the status response and set error codes */ iRet = rspStatus(self, pCmd->inp_buf); /* if the parse failed break out of here */ @@ -3073,7 +3086,6 @@ static void DMCState_StepMove(pDMC2280Driv self, pEvtEvent event) { /* * We get here when the motor stops normally */ - self->moveStopTime = DoubleTime(); if (self->doSettle) { self->doSettle = false; DMC_SetTimer(self, self->settle); @@ -3229,6 +3241,8 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) { } else if (self->subState == 2 || self->subState == 4) { /* Status */ int iRet; + /* update the stop time now in case of error exit */ + self->moveStopTime = DoubleTime(); /* parse the status response and set error codes */ iRet = rspStatus(self, pCmd->inp_buf); /* if the parse failed break out of here */ @@ -3368,7 +3382,6 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) { } /* We get here when the motor stops normally */ - self->moveStopTime = DoubleTime(); if (true /*self->debug*/) { double units = self->currPosition - self->origPosition; long int steps = self->currSteps - self->origSteps; @@ -3808,7 +3821,7 @@ static void DMC2280Error(void *pData, int *iCode, char *error, int errLen){ strncpy(error, "Crashed into forward limit switch", (size_t)errLen); break; case POSFAULT: - strncpy(error, "Positioning fault detected", (size_t)errLen); + strncpy(error, "Positioning fault, overrun detected", (size_t)errLen); break; case BADCUSHION: strncpy(error, "Air cushion problem", (size_t)errLen); @@ -4490,6 +4503,12 @@ static void DMC2280StrList(pDMC2280Driv self, char *name, SConnection *pCon){ SCWrite(pCon, buffer, eStatus); } } + if (self->errorCode) { + int iCode; + char error[CMDLEN]; + DMC2280Error(self, &iCode, error, CMDLEN); + SCPrintf(pCon, eStatus, "%s.error = %d:%s", self->name, iCode, error); + } return; } /** \brief List the motor parameters to the client. @@ -5134,6 +5153,103 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData, SCWrite(pCon, line, eValue); return 1; } + else if(strcasecmp("error", argv[1]) == 0) { + if (self->errorCode) { + int iCode; + char error[CMDLEN]; + DMC2280Error(self, &iCode, error, CMDLEN); + SCPrintf(pCon, eStatus, "%s.error = %d:%s", self->name, iCode, error); + } else { + SCPrintf(pCon, eStatus, "%s.error = %d:%s", self->name, 0, "No error"); + } + return 1; + } + else if(strcasecmp("run", argv[1]) == 0) { + if (argc > 2) { + float fLower, fUpper, fZero, fSign, fFixed, fLim; + double fSoft; + char *endPtr; + errno = 0; + fSoft = strtod(argv[2], &endPtr); + if (endPtr == argv[2] || errno == ERANGE) { + self->errorCode = BADPAR; + SCPrintf(pCon, eWarning, "Motor %s bad run parameter %s", self->name, argv[2]); + return 1; + } + /* Reject Infinity and Not a Number (NaN) */ + switch (fpclassify(fSoft)) { + case FP_ZERO: + case FP_NORMAL: + break; + default: + self->errorCode = BADPAR; + SCPrintf(pCon, eWarning, "Motor %s bad run parameter %s", self->name, argv[2]); + return 1; + } + MotorGetPar(self->pMot, "softlowerlim", &fLower); + MotorGetPar(self->pMot, "softupperlim", &fUpper); + MotorGetPar(self->pMot, "softzero", &fZero); + MotorGetPar(self->pMot, "sign", &fSign); + MotorGetPar(self->pMot, "fixed", &fFixed); + if (fFixed >= 0) { + SCPrintf(pCon, eWarning, "Motor %s is Fixed", self->name); + } else if (fSoft > fUpper) { + SCPrintf(pCon, eWarning, "%g violates upper software limit %g on %s", + fSoft, fUpper, self->name); + } else if (fSoft < fLower) { + SCPrintf(pCon, eWarning, "%g violates lower software limit %g on %s", + fSoft, fLower, self->name); + } else { + double fHard = fSoft; + fHard += fZero; + fHard *= fSign; + if (fHard > self->fUpper) { + SCPrintf(pCon, eWarning, "%g violates upper hardware limit %g (%g) on %s", + fSoft, self->fUpper * fSign + fZero, self->fUpper, self->name); + } else if (fHard < self->fLower) { + SCPrintf(pCon, eWarning, "%g violates lower hardware limit %g (%g) on %s", + fSoft, self->fLower * fSign + fZero, self->fLower, self->name); + } else { + self->fTarget = fHard; + SCPrintf(pCon, eLog, "Running %s to soft=%g, hard=%g", self->name, fSoft, fHard); + state_cmd_execute(self, CMD_RUN); + } + } + } + else { + char *txt = NULL; + if (OKOK == self->driver_status) + txt = "OKOK"; + else if (HWIdle == self->driver_status) + txt = "HWIdle"; + else if (HWBusy == self->driver_status) + txt = "HWBusy"; + else if (HWFault == self->driver_status) + txt = "HWFault"; + else if (HWPosFault == self->driver_status) + txt = "HWPosFault"; + else if (HWCrash == self->driver_status) + txt = "HWCrash"; + else if (NOMEMORY == self->driver_status) + txt = "NOMEMORY"; + else if (HWNoBeam == self->driver_status) + txt = "HWNoBeam"; + else if (HWPause == self->driver_status) + txt = "HWPause"; + else if (HWWarn == self->driver_status) + txt = "HWWarn"; + else if (HWRedo == self->driver_status) + txt = "HWRedo"; + else + txt = "HWUnknown"; + SCPrintf(pCon, eValue, "%s.run = %s", self->name, txt); + } + return 1; + } + else if(strcasecmp("halt", argv[1]) == 0) { + state_cmd_execute(self, CMD_HALT); + return 1; + } else if(self->abs_encoder && strcasecmp("stats", argv[1]) == 0) { char line[132]; snprintf(line, CMDLEN, "%s.stats=%s samples=%.0f, slope=%f, intercept=%f, r=%f, stepsPerX=%f",