linse/sics
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add run, halt, error action commands

Browse Source
This commit is contained in:
Douglas Clowes
2013-07-22 12:15:37 +10:00
parent 2a7d0a0699
commit ea0a9759e1
1 changed files with 119 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

122
site_ansto/motor_dmc2280.c
View File

@@ -30,6 +30,7 @@
#include <dynstring.h>
#include <time.h>
#include <arpa/inet.h>
#include <errno.h>
#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",