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

Bugfixes from commissioning. Use _SCx, send STx and HLTx=1. Fix missing TRACE output. Change 'has_airpads' to 'protocol'.

Browse Source 
r2389 | dcl | 2008-03-10 08:19:23 +1100 (Mon, 10 Mar 2008) | 2 lines
This commit is contained in:
Douglas Clowes
2008-03-10 08:19:23 +11:00
parent 106159ca58
commit 04568a34f2
1 changed files with 124 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files

188
site_ansto/motor_dmc2280.c
View File

@@ -89,6 +89,7 @@ struct EvtEvent_s {
#define VAR_ENC (1<<6)
#define VAR_SWI (1<<7)
#define VAR_HOM (1<<8)
#define VAR_STP (1<<9)
static pAsyncProtocol DMC2280_Protocol = NULL;
@@ -154,8 +155,8 @@ struct __MoDriv {
int lastSteps;
int lastCounts;
int thread0; /**< last read of _XQ0 */
unsigned short int input0; /**< last read of _TI0 */
unsigned short int input1; /**< last read of _TI1 */
unsigned short int stopCode; /**< last read of _SCx */
unsigned short int inputByte; /**< last read of _TIx */
bool ampError; /**< amplifier error */
bool runError; /**< motor error */
bool threadError; /**< thread error */
@@ -165,7 +166,7 @@ struct __MoDriv {
float blockage_thresh; /**< motion threshold for blockage checking */
float blockage_ratio; /**< ratio steps/counts must be between 1/this and this */
int blockage_fail; /**< flag =1 if we should fail the motor */
int has_airpads; /**< Flag = 1 if there is are airpads for this motor */
int protocol; /**< protocol version 0..3 */
float backlash_offset; /**< signed offset to drive from */
double fTarget; /**< target passed from SICS to timer callback */
double fPreseek; /**< preseek target when preseek is active */
@@ -267,6 +268,7 @@ int DMC2280MotionControl = 1; /* defaults to enabled */
/* State Machine Events */
static int state_snd_callback(pAsyncTxn pCmd);
static int state_msg_callback(pAsyncTxn pCmd);
static int state_tmr_callback(void* ctx, int mode);
static int state_cmd_execute(pDMC2280Driv self, enum commandtype cmd);
@@ -913,7 +915,7 @@ static int SendCallback(pAsyncTxn pCmd) {
static int DMC_Send(pDMC2280Driv self, char *command) {
return AsyncUnitSendTxn(self->asyncUnit,
command, strlen(command),
SendCallback, self, CMDLEN);
state_snd_callback, self, CMDLEN);
}
/**
@@ -1210,16 +1212,23 @@ static bool has_var_x(pDMC2280Driv self, const char* vars, const char* name) {
static int cmdStatus(pDMC2280Driv self) {
char cmd[CMDLEN];
char encoder = self->axisLabel;
int io_byte = 0;
if (self->encoderAxis && !(self->variables & VAR_ENC))
encoder = self->encoderAxis;
if (self->axisLabel >= 'A' && self->axisLabel <= 'D')
io_byte = 0;
else
io_byte = 1;
/* TODO: Use POSx, ENCx, RUNx, SWIx if it has these variables */
snprintf(cmd, CMDLEN, "MG {F10.0} %s%c,%s%c,%s%c,%s%c,_TI0,_TI1,_XQ0",
/* Use POSx, ENCx, RUNx, SWIx if it has these variables */
snprintf(cmd, CMDLEN, "MG {F10.0} %s%c,%s%c,%s%c,%s%c,%s%c,_TI%d,_XQ0",
(self->variables & VAR_POS) ? "POS" : "_TD", self->axisLabel,
(self->variables & VAR_ENC) ? "ENC" : "_TP", encoder,
(self->variables & VAR_SWI) ? "SWI" : "_TS", self->axisLabel,
(self->variables & VAR_RUN) ? "RUN" : "_BG", self->axisLabel);
(self->variables & VAR_RUN) ? "RUN" : "_BG", self->axisLabel,
(self->variables & VAR_STP) ? "STP" : "_SC", self->axisLabel,
io_byte);
return DMC_SendReq(self, cmd);
}
@@ -1231,10 +1240,10 @@ static int cmdVars(pDMC2280Driv self) {
static int cmdOn(pDMC2280Driv self) {
char cmd[CMDLEN];
if (self->has_airpads == 1) {
if (self->protocol == 1) {
snprintf(cmd, CMDLEN, "FTUBE=1");
}
else if (self->has_airpads == 2 || self->has_airpads == 3) {
else if (self->protocol == 2 || self->protocol == 3) {
snprintf(cmd, CMDLEN, "REQ%c=1", self->axisLabel);
}
else {
@@ -1245,7 +1254,7 @@ static int cmdOn(pDMC2280Driv self) {
static int cmdPosition(pDMC2280Driv self, int target) {
char cmd[CMDLEN];
if (self->has_airpads == 3)
if (self->protocol == 3)
snprintf(cmd, CMDLEN, "DST%c=%d", self->axisLabel, target);
else
snprintf(cmd, CMDLEN, "PA%c=%d", self->axisLabel, target);
@@ -1254,7 +1263,7 @@ static int cmdPosition(pDMC2280Driv self, int target) {
static int cmdBegin(pDMC2280Driv self) {
char cmd[CMDLEN];
if (self->has_airpads == 3)
if (self->protocol == 3)
snprintf(cmd, CMDLEN, "RUN%c=1", self->axisLabel);
else
snprintf(cmd, CMDLEN, "BG%c", self->axisLabel);
@@ -1263,28 +1272,29 @@ static int cmdBegin(pDMC2280Driv self) {
static int cmdPoll(pDMC2280Driv self) {
char cmd[CMDLEN];
if (self->has_airpads == 2 || self->has_airpads == 3)
if (self->protocol == 2 || self->protocol == 3)
snprintf(cmd, CMDLEN, "MG RSP%c", self->axisLabel);
else if (self->has_airpads == 1)
else if (self->protocol == 1)
snprintf(cmd, CMDLEN, "MG APDONE");
return DMC_SendReq(self, cmd);
}
static int cmdHalt(pDMC2280Driv self) {
static void cmdHalt(pDMC2280Driv self) {
char cmd[CMDLEN];
if (self->variables & VAR_HLT)
snprintf(cmd, CMDLEN, "HLT%c=1", self->axisLabel);
else
snprintf(cmd, CMDLEN, "ST%c", self->axisLabel);
return DMC_SendReq(self, cmd);
(void) DMC_Send(self, cmd);
if (self->variables & VAR_HLT) {
snprintf(cmd, CMDLEN, "HLT%c=1", self->axisLabel);
(void) DMC_Send(self, cmd);
}
}
static int cmdOff(pDMC2280Driv self) {
char cmd[CMDLEN];
if (self->has_airpads == 1) {
if (self->protocol == 1) {
snprintf(cmd, CMDLEN, "FTUBE=0");
}
else if (self->has_airpads == 2 || self->has_airpads == 3) {
else if (self->protocol == 2 || self->protocol == 3) {
snprintf(cmd, CMDLEN, "REQ%c=0", self->axisLabel);
}
else {
@@ -1296,11 +1306,10 @@ static int cmdOff(pDMC2280Driv self) {
static int rspStatus(pDMC2280Driv self, const char* text) {
int iRet, iFlags;
int iSteps, iCounts;
int iTI0, iTI1, iXQ0, iBG;
/* TODO: add _RUNx for has_airpads == 3 */
int iIOByte, iStopCode, iXQ0, iBG;
iRet = sscanf(text, "%d %d %d %d %d %d %d",
&iSteps, &iCounts, &iFlags, &iBG,
&iTI0, &iTI1, &iXQ0);
&iStopCode, &iIOByte, &iXQ0);
if (iRet != 7)
return 0;
self->currFlags = iFlags;
@@ -1308,21 +1317,27 @@ static int rspStatus(pDMC2280Driv self, const char* text) {
self->currCounts = iCounts;
self->currPosition = motPosit(self);
self->thread0 = iXQ0;
self->input0 = iTI0;
self->input1 = iTI1;
self->stopCode = iStopCode;
self->inputByte = iIOByte;
#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')));
self->ampError = !(1 & (iIOByte >> (self->axisLabel - 'A')));
else
self->ampError = !(1 & (iTI1 >> (self->axisLabel - 'E')));
self->ampError = !(1 & (iIOByte >> (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;
if (self->protocol == 3 && !(self->variables & VAR_STP)) {
if (self->moving)
self->stopCode = 0;
else
self->stopCode = 1;
}
return 1;
}
@@ -1346,13 +1361,15 @@ static int rspVars(pDMC2280Driv self, const char* text) {
self->variables |= VAR_ENC;
if (has_var_x(self, text, "SWI"))
self->variables |= VAR_SWI;
if (has_var_x(self, text, "STP"))
self->variables |= VAR_STP;
if ((self->variables & VAR_REQ) &&
(self->variables & VAR_RSP)) {
self->has_airpads = 2;
self->protocol = 2;
if ((self->variables & VAR_RUN) &&
(self->variables & VAR_DST) &&
(self->variables & VAR_POS)) {
self->has_airpads = 3;
self->protocol = 3;
}
}
return 1;
@@ -1361,8 +1378,8 @@ static int rspVars(pDMC2280Driv self, const char* text) {
static int rspPoll(pDMC2280Driv self, const char* text) {
int iReply = atoi(text);
if (iReply < 0)
snprintf(self->dmc2280Error, CMDLEN,
"MOTOR CONTROLLER REQ ERROR: %d", iReply);
snprintf(self->dmc2280Error, CMDLEN, "MOTOR CONTROLLER REQ ERROR: %d",
iReply);
return iReply;
}
@@ -1550,23 +1567,46 @@ static void handle_event(pDMC2280Driv self, pEvtEvent event) {
self->myState(self, event);
}
static int state_snd_callback(pAsyncTxn pCmd) {
pDMC2280Driv self = (pDMC2280Driv) pCmd->cntx;
SendCallback(pCmd);
#if defined(STATE_TRACE) && (STATE_TRACE > 0)
char* line;
char text[CMDLEN];
gettimeofday(&self->state_trace_time[self->state_trace_idx], NULL);
line = &self->state_trace_text[self->state_trace_idx++][0];
if (self->state_trace_idx >= STATE_TRACE)
self->state_trace_idx = 0;
#else
char line[CMDLEN];
char text[CMDLEN];
#endif
text[0] = '\0';
str_n_cat(text, CMDLEN, pCmd->out_buf);
str_n_cat(text, CMDLEN, "|");
str_n_cat(text, CMDLEN, pCmd->inp_buf);
snprintf(line, CMDLEN, "Motor=%s, State=%s(%d), send complete=%s",
self->name,
state_name(self->myState),
self->subState,
text);
if (self->debug)
SICSLogWrite(line, eStatus);
if (self->trace)
SCWrite(self->trace, line, eStatus);
return 0;
}
static int state_msg_callback(pAsyncTxn pCmd) {
pDMC2280Driv self = (pDMC2280Driv) pCmd->cntx;
EvtEvent event;
SendCallback(pCmd);
if (pCmd->txn_status == ATX_TIMEOUT) {
if (self->debug) {
SICSLogWrite(pCmd->out_buf, eStatus);
SICSLogWrite("<TIMEOUT>", eStatus);
}
event.event_type = eTimeoutEvent;
event.event.msg.cmd = pCmd;
}
else {
if (self->debug) {
SICSLogWrite(pCmd->out_buf, eStatus);
SICSLogWrite(pCmd->inp_buf, eStatus);
}
event.event_type = eMessageEvent;
event.event.msg.cmd = pCmd;
}
@@ -1600,6 +1640,7 @@ static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event) {
case eStateEvent:
self->run_flag = 0;
self->driver_status = HWIdle;
self->errorCode = 0;
/* Set speed */
value = motSpeed(self, self->speed);
snprintf(cmd, CMDLEN, "SP%c=%d", self->axisLabel, value);
@@ -1690,7 +1731,7 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) {
if (self->variables & VAR_RUN) {
char cmd[CMDLEN];
snprintf(cmd, CMDLEN, "RUN%c=0", self->axisLabel);
DMC_Send(self, cmd);
(void) DMC_Send(self, cmd);
}
NetWatchRegisterTimer(&self->state_timer,
0,
@@ -1840,7 +1881,7 @@ static void DMCState_MotorStart(pDMC2280Driv self, pEvtEvent event) {
do {
pAsyncTxn pCmd = event->event.msg.cmd;
if (self->subState == 1) {
if (self->has_airpads == 0) { /* cmdOn */
if (self->protocol == 0) { /* cmdOn */
change_state(self, DMCState_MotorOn);
return;
}
@@ -2075,6 +2116,7 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
do {
pAsyncTxn pCmd = event->event.msg.cmd;
if (self->subState == 1) { /* BG */
/* TODO: Check if BG worked (reply != '?') */
NetWatchRegisterTimer(&self->state_timer,
MOTOR_POLL_FAST,
state_tmr_callback, self);
@@ -2112,6 +2154,27 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
return;
}
#endif
if (self->stopCode != 0 && self->stopCode != 1) {
if (self->stopCode == 2) {
self->errorCode = FWDLIM;
self->driver_status = HWFault;
change_state(self, DMCState_OffTimer);
return;
} else if (self->stopCode == 3) {
self->errorCode = RVRSLIM;
self->driver_status = HWFault;
change_state(self, DMCState_OffTimer);
return;
}
else {
self->errorCode = RUNERROR;
snprintf(self->dmc2280Error, CMDLEN, "BAD Stop Code %d",
self->stopCode);
self->driver_status = HWFault;
change_state(self, DMCState_OffTimer);
return;
}
}
if (self->moving) {
/* If Motion Control is off, report HWFault */
if (DMC2280MotionControl != 1) {
@@ -2146,18 +2209,12 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
if (fwd_limit_active && rvrs_limit_active) {
self->errorCode = IMPOSSIBLE_LIM_SW;
self->driver_status = HWFault;
} else if (fwd_limit_active) {
self->errorCode = FWDLIM;
self->driver_status = HWFault;
} else if (rvrs_limit_active) {
self->errorCode = RVRSLIM;
self->driver_status = HWFault;
} else if (errorlimit) {
self->errorCode = ERRORLIM;
self->driver_status = HWFault;
}
if (self->driver_status == HWFault) {
change_state(self, DMCState_MotorHalt);
change_state(self, DMCState_OffTimer);
return;
}
/*
@@ -2310,7 +2367,7 @@ static void DMCState_MotorHalt(pDMC2280Driv self, pEvtEvent event) {
NetWatchRemoveTimer(self->state_timer);
self->state_timer = 0;
cmdHalt(self);
return;
/* Note fall through */
case eTimerEvent:
cmdStatus(self);
self->subState = 1;
@@ -2318,13 +2375,7 @@ static void DMCState_MotorHalt(pDMC2280Driv self, pEvtEvent event) {
case eMessageEvent:
do {
pAsyncTxn pCmd = event->event.msg.cmd;
if (self->subState == 0) { /* ST */
NetWatchRegisterTimer(&self->state_timer,
MOTOR_POLL_FAST,
state_tmr_callback, self);
return;
}
else if (self->subState == 1) { /* Status Response */
if (self->subState == 1) { /* Status Response */
int iRet;
iRet = rspStatus(self, pCmd->inp_buf);
if (iRet == 0)
@@ -2373,6 +2424,7 @@ static void DMCState_OffTimer(pDMC2280Driv self, pEvtEvent event) {
switch (event->event_type) {
case eStateEvent:
if (self->run_flag == 0) {
if (self->driver_status == HWBusy)
self->driver_status = HWIdle;
if (self->motOffDelay) {
NetWatchRegisterTimer(&self->state_timer,
@@ -2443,7 +2495,7 @@ static void DMCState_MotorStop(pDMC2280Driv self, pEvtEvent event) {
do {
pAsyncTxn pCmd = event->event.msg.cmd;
if (self->subState == 0) { /* Off command */
if (self->has_airpads == 0) {
if (self->protocol == 0) {
change_state(self, DMCState_Idle);
return;
}
@@ -2473,6 +2525,8 @@ static void DMCState_MotorStop(pDMC2280Driv self, pEvtEvent event) {
switch (event->event.cmd.cmd_type) {
case CMD_RUN:
/* handle run command */
if (self->driver_status == HWIdle)
self->driver_status = HWBusy;
if (self->waitResponse == false) {
self->run_flag = 0;
change_state(self, DMCState_MotorStart);
@@ -2882,8 +2936,8 @@ static int DMC2280GetPar(void *pData, char *name,
*fValue = self->settle;
return 1;
}
if(strcasecmp(name,AIRPADS) == 0) {
*fValue = self->has_airpads;
if(strcasecmp(name,AIRPADS) == 0 || strcasecmp(name,"protocol") == 0) {
*fValue = self->protocol;
return 1;
}
if(strcasecmp(name,BLOCKAGE_CHECK_INTERVAL) == 0) {
@@ -3055,11 +3109,11 @@ static int DMC2280SetPar(void *pData, SConnection *pCon,
}
/* Set airpads, managers only */
if(strcasecmp(name,AIRPADS) == 0) {
if(strcasecmp(name,AIRPADS) == 0 || strcasecmp(name,"protocol") == 0) {
if(!SCMatchRights(pCon,usMugger))
return 1;
else {
self->has_airpads = newValue;
self->protocol = newValue;
return 1;
}
}
@@ -3294,7 +3348,7 @@ static void DMC2280List(void *pData, char *name, SConnection *pCon){
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.Backlash_offset = %f\n", name, self->backlash_offset);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.AirPads = %d\n", name, self->has_airpads);
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);
SCWrite(pCon, buffer, eStatus);
@@ -3625,10 +3679,14 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) {
}
/* AIRPADS: this motor need airpads */
if ((pPtr=getParam(pCon, interp, params,"airpads",_OPTIONAL)) == NULL)
pNew->has_airpads=0;
if ((pPtr=getParam(pCon, interp, params,"airpads",_OPTIONAL)) == NULL) {
if ((pPtr=getParam(pCon, interp, params,"protocol",_OPTIONAL)) == NULL)
pNew->protocol=0;
else
sscanf(pPtr,"%d",&(pNew->protocol));
}
else {
sscanf(pPtr,"%d",&(pNew->has_airpads));
sscanf(pPtr,"%d",&(pNew->protocol));
}
/* ABSENC: If the parameter requires an abs enc add it to the else block */