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

Set Motor status to HWBusy in State_Unknown

Browse Source 
Discard outstanding message response on entering State_Unknown (timing window)
Don't break if it times out while waiting
Defer various faults until motor has stopped
Rename absenc to absencoder in list command (if has encoder)
Modify absenc to report integer encoder value
Allow manager to set various sensitive values in motor debug mode
Make motor "reset" command wait until motor reinitialised (timing window)

r2520 | dcl | 2008-05-12 11:05:35 +1000 (Mon, 12 May 2008) | 9 lines
This commit is contained in:
Douglas Clowes
2008-05-12 11:05:35 +10:00
parent 2a469d6b89
commit 445a332370
1 changed files with 129 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files

202
site_ansto/motor_dmc2280.c
View File

@@ -1018,7 +1018,7 @@ static void report_motion(pDMC2280Driv self) {
* SUCCESS
* FAILURE
*/
static int readAbsEnc(pDMC2280Driv self, float *pos) {
static int readAbsEnc(pDMC2280Driv self, long *pos) {
char reply[CMDLEN];
char cmd[CMDLEN];
@@ -1026,7 +1026,7 @@ static int readAbsEnc(pDMC2280Driv self, float *pos) {
if (FAILURE == DMC_SendReceive(self, cmd, reply))
return FAILURE;
*pos = (double) atol(reply);
*pos = atol(reply);
return SUCCESS;
}
@@ -1772,8 +1772,14 @@ static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event) {
switch (event->event_type) {
case eStateEvent:
self->run_flag = 0;
self->driver_status = HWIdle;
self->driver_status = HWBusy;
self->errorCode = 0;
/* handle pending message event */
if (self->waitResponse) {
self->subState = 0;
/* FIXME: FIXME_DOUG */
return;
}
/* Set speed */
value = motSpeed(self, self->speed);
snprintf(cmd, CMDLEN, "SP%c=%d", self->axisLabel, value);
@@ -1808,6 +1814,10 @@ static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event) {
case eMessageEvent:
do {
pAsyncTxn pCmd = event->event.msg.cmd;
if (self->subState == 0) { /* waitResponse */
change_state(self, DMCState_Unknown);
return;
}
if (self->subState == 1) { /* Vars */
rspVars(self, pCmd->inp_buf);
cmdStatus(self);
@@ -1841,7 +1851,7 @@ static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event) {
case eTimeoutEvent:
/* handle timeout */
change_state(self, DMCState_Unknown);
break;
return;
}
unhandled_event(self, event);
self->errorCode = STATEERROR;
@@ -2246,7 +2256,7 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
if (pCmd->inp_buf[0] == '?') {
/* TODO: what happens when BGx fails? */
self->errorCode = BADCMD;
self->driver_status = HWFault;
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return;
}
@@ -2272,13 +2282,13 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
errorlimit = (iFlags & STATUSERRORLIMIT);
if (self->threadError) {
self->errorCode = THREADZERO;
self->driver_status = HWFault;
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return;
}
else if (self->runError) {
self->errorCode = RUNERROR;
self->driver_status = HWFault;
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return;
}
@@ -2293,30 +2303,32 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
if (self->stopCode != 0 && self->stopCode != 1) {
if (self->stopCode == 2) {
self->errorCode = FWDLIM;
self->driver_status = HWFault;
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return;
}
else if (self->stopCode == 3) {
self->errorCode = RVRSLIM;
self->driver_status = HWFault;
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return;
}
else if (self->stopCode == 4) {
if (fwd_limit_active && rvrs_limit_active) {
self->errorCode = IMPOSSIBLE_LIM_SW;
self->driver_status = HWFault;
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return;
}
else if (fwd_limit_active) {
self->errorCode = FWDLIM;
self->driver_status = HWFault;
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return;
}
else if (rvrs_limit_active) {
self->errorCode = RVRSLIM;
self->driver_status = HWFault;
}
if (self->driver_status == HWFault) {
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return;
}
@@ -2324,7 +2336,7 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
self->errorCode = RUNERROR;
snprintf(self->dmc2280Error, CMDLEN, "BAD Stop Code %d",
self->stopCode);
self->driver_status = HWFault;
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return;
}
@@ -2335,7 +2347,7 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
self->errorCode = MOTIONCONTROLOFF;
else
self->errorCode = MOTIONCONTROLUNK;
self->driver_status = HWFault;
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return;
}
@@ -2343,14 +2355,14 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
if (checkMotion(self) == 0) {
/* handle blocked */
self->errorCode = BLOCKED;
self->driver_status = HWFault;
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return;
}
if (checkPosition(self) == 0) {
/* handle runaway */
self->errorCode = POSFAULT;
self->faultPending = true; /* defer recoverable fault */
self->errorCode = POSFAULT; /* recoverable fault */
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return;
}
@@ -3142,12 +3154,24 @@ static int DMC2280GetPar(void *pData, char *name,
return 1;
}
}
if (strcasecmp(name,"absencoder") == 0) {
if (self->abs_encoder)
*fValue = 1.0;
else
*fValue = 0.0;
return 1;
}
if (self->abs_encoder != 0) {
if (strcasecmp(name,"absenc") == 0) {
if (readAbsEnc(self, fValue) == SUCCESS)
long lValue;
if (readAbsEnc(self, &lValue) == SUCCESS) {
*fValue = (double) lValue;
return 1;
else
}
else {
*fValue = -1.0;
return 0;
}
}
if (strcasecmp(name,"absenchome") == 0) {
*fValue = self->absEncHome;
@@ -3215,27 +3239,6 @@ static int DMC2280SetPar(void *pData, SConnection *pCon,
self = (pDMC2280Driv)pData;
/* XXX Maybe move this to a configuration parameter.*/
/* Set home, managers only. Users should set softposition */
if(strcasecmp(name,HOME) == 0) {
if(!SCMatchRights(pCon,usMugger))
return 1;
else {
if ( (self->fLower - newValue) > FLT_EPSILON) {
snprintf(pError, ERRLEN,"ERROR:'%s %s' must be greater than or equal to %f", self->name, HOME, self->fLower);
SCWrite(pCon, pError, eError);
return 1;
}
if ( (newValue - self->fUpper) > FLT_EPSILON) {
snprintf(pError, ERRLEN,"ERROR:'%s %s' must be less than or equal to %f", self->name, HOME, self->fUpper);
SCWrite(pCon, pError, eError);
return 1;
}
self->fHome = newValue;
return 1;
}
}
if(strcasecmp(name,SETPOS) == 0) {
float oldZero, newZero;
if (self->pMot == NULL)
@@ -3436,6 +3439,56 @@ static int DMC2280SetPar(void *pData, SConnection *pCon,
return 0; /* FIXME should signal a HWFault */
return 1;
}
if (strcasecmp("hardlowerlim", name) == 0) {
/* Debug Managers only */
if (!(self->debug && SCMatchRights(pCon, usMugger)))
return 0;
self->fLower = newValue;
return 1;
}
if (strcasecmp("hardupperlim", name) == 0) {
/* Debug Managers only */
if (!(self->debug && SCMatchRights(pCon, usMugger)))
return 0;
self->fUpper = newValue;
return 1;
}
if (self->abs_encoder && strcasecmp("stepsPerX", name) == 0) {
/* Debug Managers only */
if (!(self->debug && SCMatchRights(pCon, usMugger)))
return 0;
self->stepsPerX = newValue;
return 1;
}
if (self->debug && strcasecmp("cntsPerX", name) == 0) {
/* Debug Managers only */
if (!(self->debug && SCMatchRights(pCon, usMugger)))
return 0;
self->cntsPerX = newValue;
return 1;
}
/* XXX Maybe move this to a configuration parameter.*/
/* Set home, managers only. Users should set softposition */
if(strcasecmp(name, HOME) == 0) {
/* Debug Managers only */
if(!(self->debug && SCMatchRights(pCon,usMugger)))
return 1;
else {
if ( (self->fLower - newValue) > FLT_EPSILON) {
snprintf(pError, ERRLEN,"ERROR:'%s %s' must be greater than or equal to %f", self->name, HOME, self->fLower);
SCWrite(pCon, pError, eError);
return 1;
}
if ( (newValue - self->fUpper) > FLT_EPSILON) {
snprintf(pError, ERRLEN,"ERROR:'%s %s' must be less than or equal to %f", self->name, HOME, self->fUpper);
SCWrite(pCon, pError, eError);
return 1;
}
self->fHome = newValue;
return 1;
}
}
if (self->posit_count > 0) {
int index;
@@ -3517,7 +3570,7 @@ static void DMC2280List(void *pData, char *name, SConnection *pCon){
SCWrite(pCon, buffer, eStatus);
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);
snprintf(buffer, BUFFLEN, "%s.absEncoder = %d\n", name, self->abs_encoder);
SCWrite(pCon, buffer, eStatus);
if (self->abs_encoder) {
snprintf(buffer, BUFFLEN, "%s.absEncHome = %d\n", name, self->absEncHome);
@@ -4082,6 +4135,10 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
self->state_timer = 0;
self->driver_status = HWIdle;
change_state(self, DMCState_Unknown);
while (self->myState == DMCState_Unknown) {
pTaskMan pTasker = GetTasker();
TaskYield(pTasker);
}
/* Handle further in generic motor driver */
return MotorAction(pCon, pSics, pData, argc, argv);
}
@@ -4191,38 +4248,37 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
}
return 1;
}
else if (self->debug && strcasecmp("hardlowerlim", argv[1]) == 0) {
/* Managers only */
if (!SCMatchRights(pCon, usMugger))
return 0;
float fValue = strtod(argv[2], NULL);
self->fLower = fValue;
return 1;
}
else if (self->debug && strcasecmp("hardupperlim", argv[1]) == 0) {
/* Managers only */
if (!SCMatchRights(pCon, usMugger))
return 0;
float fValue = strtod(argv[2], NULL);
self->fUpper = fValue;
return 1;
}
else if (self->debug && strcasecmp("stepsPerX", argv[1]) == 0) {
/* Managers only */
if (!SCMatchRights(pCon, usMugger))
return 0;
float fValue = strtod(argv[2], NULL);
self->stepsPerX = fValue;
return 1;
}
else if (self->debug && strcasecmp("cntsPerX", argv[1]) == 0) {
/* Managers only */
if (!SCMatchRights(pCon, usMugger))
return 0;
float fValue = strtod(argv[2], NULL);
self->cntsPerX = fValue;
if (self->abs_encoder && strcasecmp("absEncHome", argv[1]) == 0) {
if (argc > 2) {
/* Debug Managers only */
if (!(self->debug && SCMatchRights(pCon, usMugger)))
return 0;
long lValue = strtol(argv[2], NULL, 10);
self->absEncHome = lValue;
return 1;
}
else {
char line[132];
snprintf(line, 132, "%s.absEncHome = %d.000000",
self->name,
self->absEncHome);
SCWrite(pCon, line, eValue);
return 1;
}
}
if (self->abs_encoder && strcasecmp("absenc", argv[1]) == 0) {
long lValue;
if (readAbsEnc(self, &lValue) != SUCCESS) {
lValue = -1;
}
char line[132];
snprintf(line, 132, "%s.absEnc = %ld.000000",
self->name,
lValue);
SCWrite(pCon, line, eValue);
return 1;
}
if (self->posit_count > 0) {
if(strcasecmp("posit2count", argv[1]) == 0) {
char line[132];