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

Bug fixes and improved error handling, more on controller variables

Browse Source 
r2232 | dcl | 2007-11-06 08:50:18 +1100 (Tue, 06 Nov 2007) | 2 lines
This commit is contained in:
Douglas Clowes
2007-11-06 08:50:18 +11:00
parent f4f1c5f152
commit ebf70bfe5c
1 changed files with 78 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

103
site_ansto/motor_dmc2280.c
View File

@@ -78,12 +78,13 @@ struct EvtEvent_s {
} event;
};
#define VAR_REQ 1
#define VAR_RSP 2
#define VAR_RUN 4
#define VAR_DST 8
#define VAR_POS 16
#define VAR_HLT 32
#define VAR_REQ (1<<0)
#define VAR_RSP (1<<1)
#define VAR_RUN (1<<2)
#define VAR_DST (1<<3)
#define VAR_POS (1<<4)
#define VAR_HLT (1<<5)
#define VAR_ENC (1<<6)
static pAsyncProtocol DMC2280_Protocol = NULL;
@@ -151,6 +152,7 @@ struct __MoDriv {
unsigned short int input0; /**< last read of _TI0 */
unsigned short int input1; /**< last read of _TI1 */
bool ampError; /**< amplifier error */
bool runError; /**< motor error */
bool threadError; /**< thread error */
bool moving; /**< true if moving */
struct timeval time_lastPos_set; /**< Time when lastPosition was set */
@@ -221,6 +223,7 @@ int DMC2280MotionControl = 1; /* defaults to enabled */
#define STATEERROR -20
#define THREADZERO -21 /* Thread zero not running */
//#define AMPERROR -22 /* Amplifier OK not asserted */
#define RUNERROR -23 /* Error from RUN variable */
/*--------------------------------------------------------------------*/
#define STATUSMOVING 128 /* Motor is moving */
#define STATUSERRORLIMIT 64 /* Number of errors exceed limit */
@@ -1173,7 +1176,7 @@ static bool has_var_x(pDMC2280Driv self, const char* vars, const char* name) {
if (p == NULL) /* not found at all */
return false;
if (p == vars) /* found at start of first line */
return false;
return true;
if (p[-1] == '\n') /* found at start of another line */
return true;
return false; /* found partial (ZVVVx=) only */
@@ -1190,6 +1193,19 @@ static int state_cmd_execute(pDMC2280Driv self, enum commandtype cmd);
*
* \param self access to the drive data
*/
#if 1
static int cmdStatus(pDMC2280Driv self) {
char cmd[CMDLEN];
/* TODO: Use POSx, ENCx, RUNx if it has these variables */
snprintf(cmd, CMDLEN, "MG {F10.0} %s%c,%s%c,_TS%c,%s%c,_TI0,_TI1,_XQ0",
(self->variables & VAR_POS) ? "POS" : "_TD", self->axisLabel,
(self->variables & VAR_ENC) ? "ENC" : "_TP", self->axisLabel,
self->axisLabel,
(self->variables & VAR_RUN) ? "RUN" : "_BG", self->axisLabel);
return DMC_SendCmd(self, cmd, state_msg_callback);
}
#else
static int cmdStatus(pDMC2280Driv self) {
char cmd[CMDLEN];
@@ -1201,6 +1217,7 @@ static int cmdStatus(pDMC2280Driv self) {
self->axisLabel, self->axisLabel, self->axisLabel, self->axisLabel);
return DMC_SendCmd(self, cmd, state_msg_callback);
}
#endif
static int cmdVars(pDMC2280Driv self) {
char cmd[CMDLEN];
@@ -1293,10 +1310,17 @@ static int rspStatus(pDMC2280Driv self, const char* text) {
self->thread0 = iXQ0;
self->input0 = iTI0;
self->input1 = iTI1;
if (self->axisLabel >= 'A' && self->axisLabel <= 'D')
#ifdef AMPERROR
if (self->variables & VAR_RUN)
self->ampError = 0;
else if (self->axisLabel >= 'A' && self->axisLabel <= 'D')
self->ampError = !(1 & (iTI0 >> (self->axisLabel - 'A')));
else
self->ampError = !(1 & (iTI1 >> (self->axisLabel - 'E')));
#endif
self->runError = iBG < 0 ? iBG : 0;
if (iBG < 0)
snprintf(self->dmc2280Error, CMDLEN, "MOTOR CONTROLLER RUN ERROR: %d", iBG);
self->threadError = self->thread0 < 0;
self->moving = iBG > 0;
return 1;
@@ -1316,6 +1340,8 @@ static int rspVars(pDMC2280Driv self, const char* text) {
self->variables |= VAR_POS;
if (has_var_x(self, text, "HLT"))
self->variables |= VAR_HLT;
if (has_var_x(self, text, "ENC"))
self->variables |= VAR_ENC;
if ((self->variables & VAR_REQ) &&
(self->variables & VAR_RSP)) {
self->has_airpads = 2;
@@ -1577,9 +1603,15 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) {
case eStateEvent:
if (self->state_timer)
NetWatchRemoveTimer(self->state_timer);
NetWatchRegisterTimer(&self->state_timer,
MOTOR_POLL_SLOW,
state_tmr_callback, self);
if ((self->variables & VAR_RUN) && self->runError) {
char cmd[CMDLEN];
snprintf(cmd, CMDLEN, "RUN%c=0", self->axisLabel);
DMC_SendCmd(self, cmd, state_msg_callback);
}
else
NetWatchRegisterTimer(&self->state_timer,
MOTOR_POLL_SLOW,
state_tmr_callback, self);
self->subState = 0;
return;
case eTimerEvent:
@@ -1589,6 +1621,12 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) {
case eMessageEvent:
do {
pAsyncTxn pCmd = event->event.msg.cmd;
if (self->subState == 0) { /* RUNx reset */
NetWatchRegisterTimer(&self->state_timer,
MOTOR_POLL_SLOW,
state_tmr_callback, self);
return;
}
if (self->subState == 1) { /* Status Response */
int iRet;
float fDelta;
@@ -1649,7 +1687,6 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) {
}
static void DMCState_MotorStart(pDMC2280Driv self, pEvtEvent event) {
char cmd[CMDLEN];
switch (event->event_type) {
case eStateEvent:
if (self->state_timer) {
@@ -1669,6 +1706,10 @@ static void DMCState_MotorStart(pDMC2280Driv self, pEvtEvent event) {
self->errorCode = THREADZERO;
self->driver_status = HWFault;
}
else if (self->runError) {
self->errorCode = RUNERROR;
self->driver_status = HWFault;
}
#ifdef AMPERROR
else if (self->ampError) {
self->errorCode = AMPERROR;
@@ -1768,14 +1809,20 @@ static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event) {
if (self->threadError) {
self->errorCode = THREADZERO;
self->driver_status = HWFault;
change_state(self, DMCState_Error);
change_state(self, DMCState_MotorStop);
return;
}
else if (self->runError) {
self->errorCode = RUNERROR;
self->driver_status = HWFault;
change_state(self, DMCState_MotorStop);
return;
}
#ifdef AMPERROR
else if (self->ampError) {
self->errorCode = AMPERROR;
self->driver_status = HWFault;
change_state(self, DMCState_Error);
change_state(self, DMCState_MotorStop);
return;
}
#endif
@@ -1874,27 +1921,31 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
}
else if (self->subState == 2) { /* MG */
int iRet, iFlags;
bool moving, fwd_limit_active, rvrs_limit_active, errorlimit;
bool fwd_limit_active, rvrs_limit_active, errorlimit;
iRet = rspStatus(self, pCmd->inp_buf);
if (iRet == 0)
break;
if (self->threadError) {
self->errorCode = THREADZERO;
self->driver_status = HWFault;
change_state(self, DMCState_Error);
change_state(self, DMCState_MotorHalt);
return;
}
else if (self->runError) {
self->errorCode = RUNERROR;
self->driver_status = HWFault;
change_state(self, DMCState_MotorHalt);
return;
}
#ifdef AMPERROR
else if (self->ampError) {
self->errorCode = AMPERROR;
self->driver_status = HWFault;
change_state(self, DMCState_Error);
change_state(self, DMCState_MotorHalt);
return;
}
#endif
iFlags = self->currFlags;
moving = (iFlags & STATUSMOVING);
if (moving) {
if (self->moving) {
/* If Motion Control is off, report HWFault */
if (DMC2280MotionControl != 1) {
if (DMC2280MotionControl == 0)
@@ -1921,6 +1972,7 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
}
/* Motor has stopped */
/* Handle limit switches */
iFlags = self->currFlags;
fwd_limit_active = !(iFlags & STATUSFWDLIMIT);
rvrs_limit_active = !(iFlags & STATUSRVRSLIMIT);
errorlimit = (iFlags & STATUSERRORLIMIT);
@@ -2082,7 +2134,6 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
}
static void DMCState_MotorHalt(pDMC2280Driv self, pEvtEvent event) {
char cmd[CMDLEN];
switch (event->event_type) {
case eStateEvent:
if (self->state_timer)
@@ -2104,12 +2155,11 @@ static void DMCState_MotorHalt(pDMC2280Driv self, pEvtEvent event) {
return;
}
else if (self->subState == 1) { /* Status Response */
int iRet, iFlags;
int iRet;
iRet = rspStatus(self, pCmd->inp_buf);
if (iRet == 0)
break;
iFlags = self->currFlags;
if (iFlags & STATUSMOVING) {
if (self->moving) {
NetWatchRegisterTimer(&self->state_timer,
MOTOR_POLL_FAST,
state_tmr_callback, self);
@@ -2164,7 +2214,6 @@ static void DMCState_OffTimer(pDMC2280Driv self, pEvtEvent event) {
}
static void DMCState_MotorStop(pDMC2280Driv self, pEvtEvent event) {
char cmd[CMDLEN];
switch (event->event_type) {
case eStateEvent:
cmdOff(self);
@@ -2469,6 +2518,9 @@ static void DMC2280Error(void *pData, int *iCode, char *error, int errLen){
strncpy(error, "DRIVE AMPLIFIER ERROR", (size_t)errLen);
break;
#endif
case RUNERROR:
strncpy(error, self->dmc2280Error, (size_t)errLen);
break;
default:
/* FIXME What's the default */
snprintf(error, (size_t)errLen, "Unknown Motor Error: %d", self->errorCode);
@@ -2506,6 +2558,7 @@ static int DMC2280Fix(void *pData, int iCode,/*@unused@*/ float fValue){
#ifdef AMPERROR
case AMPERROR:
#endif
case RUNERROR:
return MOTFAIL;
case POSFAULT:
#if HAS_RS232