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

Move the motor status handling out of line to reduce clutter and promote reuse

Browse Source
This commit is contained in:
Douglas Clowes
2013-07-11 17:12:14 +10:00
parent 6e5c0730e3
commit 6c220b4b55
1 changed files with 133 additions and 109 deletions
Download Patch File Download Diff File Expand all files Collapse all files

242
site_ansto/motor_dmc2280.c
View File

@@ -1818,6 +1818,136 @@ static void change_state(pDMC2280Driv self, StateFunc func) {
self->myState(self, &evt);
}
/*
* Move the status handling out of line to reduce clutter.
* If we can handle the status response and take the action then,
* return true to the caller else return false.
*/
static bool motorHandleStatus(pDMC2280Driv self) {
int iFlags;
bool fwd_limit_active, rvrs_limit_active, errorlimit;
iFlags = self->currFlags;
fwd_limit_active = !(iFlags & STATUSFWDLIMIT);
rvrs_limit_active = !(iFlags & STATUSRVRSLIMIT);
errorlimit = (iFlags & STATUSERRORLIMIT);
if (self->threadError) {
self->errorCode = THREADZERO;
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return true;
}
else if (self->runError) {
self->errorCode = RUNERROR;
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return true;
}
#ifdef AMPERROR
else if (self->ampError) {
self->errorCode = AMPERROR;
self->driver_status = HWFault;
change_state(self, DMCState_MotorHalt);
return true;
}
#endif
if (self->stopCode != 0 && self->stopCode != 1) {
/*
* Handle abnormal stop codes
*/
if (self->stopCode == 2) {
self->errorCode = FWDLIM;
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return true;
}
else if (self->stopCode == 3) {
self->errorCode = RVRSLIM;
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return true;
}
else if (self->stopCode == 4) {
if (fwd_limit_active && rvrs_limit_active) {
self->errorCode = IMPOSSIBLE_LIM_SW;
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return true;
}
else if (fwd_limit_active) {
self->errorCode = FWDLIM;
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return true;
}
else if (rvrs_limit_active) {
self->errorCode = RVRSLIM;
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return true;
}
}
self->errorCode = RUNERROR;
snprintf(self->dmc2280Error, CMDLEN, "BAD Stop Code %d",
self->stopCode);
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return true;
}
if (self->moving) {
/* If Motion Control is off, report HWFault */
if (DMC2280MotionControl != 1) {
if (DMC2280MotionControl == 0)
self->errorCode = MOTIONCONTROLOFF;
else
self->errorCode = MOTIONCONTROLUNK;
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return true;
}
if (self->abs_encoder) {
if (checkMotion(self) == 0) {
/* handle blocked */
self->errorCode = BLOCKED;
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return true;
}
if (checkPosition(self) == 0) {
/* handle runaway */
self->errorCode = POSFAULT; /* recoverable fault */
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return true;
}
}
/*
* The motor is still moving and everything seems OK,
* set the poll timer and continue
*/
self->subState = 2;
DMC_SetTimer(self, self->motorPollFast);
return true;
}
/*
* The motor has stopped, check the limit switches
*/
if (fwd_limit_active && rvrs_limit_active) {
self->errorCode = IMPOSSIBLE_LIM_SW;
self->driver_status = HWFault;
} else if (errorlimit) {
self->errorCode = ERRORLIM;
self->driver_status = HWFault;
}
if (self->driver_status == HWFault) {
change_state(self, DMCState_OffTimer);
return true;
}
/*
* The motor has stopped and everything seems OK
*/
return false;
}
static int state_snd_callback(pAsyncTxn pCmd) {
pDMC2280Driv self = (pDMC2280Driv) pCmd->cntx;
SendCallback(pCmd);
@@ -2434,121 +2564,15 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
return;
}
else if (self->subState == 2 || self->subState == 4) { /* Status */
int iRet, iFlags;
bool fwd_limit_active, rvrs_limit_active, errorlimit;
int iRet;
/* parse the status response and set error codes */
iRet = rspStatus(self, pCmd->inp_buf);
/* if the parse failed break out of here */
if (iRet == 0)
break;
iFlags = self->currFlags;
fwd_limit_active = !(iFlags & STATUSFWDLIMIT);
rvrs_limit_active = !(iFlags & STATUSRVRSLIMIT);
errorlimit = (iFlags & STATUSERRORLIMIT);
if (self->threadError) {
self->errorCode = THREADZERO;
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
/* if the status response can be handled here, return */
if (motorHandleStatus(self))
return;
}
else if (self->runError) {
self->errorCode = RUNERROR;
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return;
}
#ifdef AMPERROR
else if (self->ampError) {
self->errorCode = AMPERROR;
self->driver_status = HWFault;
change_state(self, DMCState_MotorHalt);
return;
}
#endif
if (self->stopCode != 0 && self->stopCode != 1) {
if (self->stopCode == 2) {
self->errorCode = FWDLIM;
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return;
}
else if (self->stopCode == 3) {
self->errorCode = RVRSLIM;
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->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return;
}
else if (fwd_limit_active) {
self->errorCode = FWDLIM;
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return;
}
else if (rvrs_limit_active) {
self->errorCode = RVRSLIM;
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return;
}
}
self->errorCode = RUNERROR;
snprintf(self->dmc2280Error, CMDLEN, "BAD Stop Code %d",
self->stopCode);
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return;
}
if (self->moving) {
/* If Motion Control is off, report HWFault */
if (DMC2280MotionControl != 1) {
if (DMC2280MotionControl == 0)
self->errorCode = MOTIONCONTROLOFF;
else
self->errorCode = MOTIONCONTROLUNK;
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return;
}
if (self->abs_encoder) {
if (checkMotion(self) == 0) {
/* handle blocked */
self->errorCode = BLOCKED;
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return;
}
if (checkPosition(self) == 0) {
/* handle runaway */
self->errorCode = POSFAULT; /* recoverable fault */
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return;
}
}
self->subState = 2;
DMC_SetTimer(self, self->motorPollFast);
return;
}
/* Motor has stopped */
/* Handle limit switches */
if (fwd_limit_active && rvrs_limit_active) {
self->errorCode = IMPOSSIBLE_LIM_SW;
self->driver_status = HWFault;
} else if (errorlimit) {
self->errorCode = ERRORLIM;
self->driver_status = HWFault;
}
if (self->driver_status == HWFault) {
change_state(self, DMCState_OffTimer);
return;
}
/*
* If we get here, the motor has stopped and everything seems OK.
* We must decide on the next course of action.