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

Change Flight Tube to use REQx/RSPx instead of FTUBE/APDONE variables. Now workks for RSD Sample raise dual motor as well.

Browse Source 
Move DMC_Notify and reset state machine on reconnect.

r1996 | dcl | 2007-06-01 16:50:24 +1000 (Fri, 01 Jun 2007) | 3 lines
This commit is contained in:
Douglas Clowes
2007-06-01 16:50:24 +10:00
parent 6e919f84ff
commit d785f48aa7
1 changed files with 87 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

117
site_ansto/motor_dmc2280.c
View File

@@ -478,26 +478,6 @@ static int DMC_SendCmd(pDMC2280Driv self,
callback, self, CMDLEN);
}
static void DMC_Notify(void* context, int event)
{
pDMC2280Driv self = (pDMC2280Driv) context;
char line[132];
switch (event) {
case AQU_DISCONNECT:
snprintf(line, 132, "Disconnect on Motor '%s'", self->name);
SICSLogWrite(line, eStatus);
/* TODO: disconnect */
break;
case AQU_RECONNECT:
snprintf(line, 132, "Reconnect on Motor '%s'", self->name);
SICSLogWrite(line, eStatus);
/* TODO: reconnect */
break;
}
return;
}
/**
* \brief SendCallback is the callback for the general command.
*/
@@ -1319,9 +1299,13 @@ static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event) {
change_state(self, DMCState_Idle);
return;
}
#if 0
value = ((self->currCounts - self->absEncHome) / self->cntsPerX) * self->stepsPerX;
self->currSteps = value;
snprintf(cmd, CMDLEN, "DP%c=%d", self->axisLabel, value);
#else
snprintf(cmd, CMDLEN, "LV");
#endif
DMC_SendCmd(self, cmd, state_msg_callback);
return;
}
@@ -1329,6 +1313,18 @@ static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event) {
change_state(self, DMCState_Idle);
return;
}
if (pCmd->out_buf[0] == 'L') { /* LV */
char req[12];
char rdy[12];
snprintf(req, 12, "REQ%c=", self->axisLabel);
snprintf(rdy, 12, "RSP%c=", self->axisLabel);
if (strstr(pCmd->inp_buf, req) &&
strstr(pCmd->inp_buf, rdy)) {
self->has_airpads = 2;
}
change_state(self, DMCState_Idle);
return;
}
}
while (0);
break;
@@ -1369,7 +1365,6 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) {
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;
@@ -1377,11 +1372,16 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) {
if (self->driver_status == HWFault) {
return;
}
if (self->has_airpads) {
if (self->has_airpads == 1) {
snprintf(cmd, CMDLEN, "FTUBE=1");
DMC_SendCmd(self, cmd, state_msg_callback);
change_state(self, DMCState_AirOn);
}
else if (self->has_airpads == 2) {
snprintf(cmd, CMDLEN, "REQ%c=1", self->axisLabel);
DMC_SendCmd(self, cmd, state_msg_callback);
change_state(self, DMCState_AirOn);
}
else {
snprintf(cmd, CMDLEN, "SH%c", self->axisLabel);
DMC_SendCmd(self, cmd, state_msg_callback);
@@ -1420,20 +1420,26 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) {
}
static void DMCState_AirOn(pDMC2280Driv self, pEvtEvent event) {
char cmd[CMDLEN];
switch (event->event_type) {
case eTimerEvent:
DMC_SendCmd(self, "MG APDONE", state_msg_callback);
if (self->has_airpads == 2)
snprintf(cmd, CMDLEN, "MG RSP%c", self->axisLabel);
else
snprintf(cmd, CMDLEN, "MG APDONE");
DMC_SendCmd(self, cmd, state_msg_callback);
return;
case eMessageEvent:
do {
pAsyncTxn pCmd = event->event.msg.cmd;
if (pCmd->out_buf[0] == 'F') { /* FTUBE */
if (pCmd->out_buf[0] == 'F' || /* FTUBE */
pCmd->out_buf[0] == 'R') { /* REQx= */
NetWatchRegisterTimer(&self->state_timer,
AIR_POLL_TIMER,
state_tmr_callback, self);
return;
}
else if (pCmd->out_buf[0] == 'M') { /* MG APDONE */
else if (pCmd->out_buf[0] == 'M') { /* MG APDONE/RSPx */
float fReply;
fReply = (float) atof(pCmd->inp_buf);
if (fReply > 0) {
@@ -1772,12 +1778,18 @@ static void DMCState_MotorHalt(pDMC2280Driv self, pEvtEvent event)
state_tmr_callback, self);
return;
}
if (self->has_airpads) {
if (self->has_airpads == 1) {
snprintf(cmd, CMDLEN, "FTUBE=0");
DMC_SendCmd(self, cmd, state_msg_callback);
change_state(self, DMCState_AirOff);
return;
}
else if (self->has_airpads == 2) {
snprintf(cmd, CMDLEN, "REQ%c=0", self->axisLabel);
DMC_SendCmd(self, cmd, state_msg_callback);
change_state(self, DMCState_AirOff);
return;
}
else {
snprintf(cmd, CMDLEN, "MO%c", self->axisLabel);
DMC_SendCmd(self, cmd, state_msg_callback);
@@ -1799,11 +1811,17 @@ static void DMCState_OffTimer(pDMC2280Driv self, pEvtEvent event) {
char cmd[CMDLEN];
switch (event->event_type) {
case eTimerEvent:
if (self->has_airpads) {
if (self->has_airpads == 1) {
snprintf(cmd, CMDLEN, "FTUBE=0");
DMC_SendCmd(self, cmd, state_msg_callback);
change_state(self, DMCState_AirOff);
}
else if (self->has_airpads == 2) {
snprintf(cmd, CMDLEN, "REQ%c=0", self->axisLabel);
DMC_SendCmd(self, cmd, state_msg_callback);
change_state(self, DMCState_AirOff);
return;
}
else {
snprintf(cmd, CMDLEN, "MO%c", self->axisLabel);
DMC_SendCmd(self, cmd, state_msg_callback);
@@ -1836,16 +1854,22 @@ static void DMCState_OffTimer(pDMC2280Driv self, pEvtEvent event) {
}
static void DMCState_AirOff(pDMC2280Driv self, pEvtEvent event) {
char cmd[CMDLEN];
switch (event->event_type) {
case eTimerEvent:
DMC_SendCmd(self, "MG APDONE", state_msg_callback);
if (self->has_airpads == 2)
snprintf(cmd, CMDLEN, "MG RSP%c", self->axisLabel);
else
snprintf(cmd, CMDLEN, "MG APDONE");
DMC_SendCmd(self, cmd, state_msg_callback);
return;
case eMessageEvent:
do {
pAsyncTxn pCmd = event->event.msg.cmd;
if (pCmd->out_buf[0] == 'F') { /* FTUBE */
if (pCmd->out_buf[0] == 'F' || /* FTUBE */
pCmd->out_buf[0] == 'R') { /* REQx= */
}
else if (pCmd->out_buf[0] == 'M') { /* MG APDONE */
else if (pCmd->out_buf[0] == 'M') { /* MG APDONE/RSPx */
float fReply;
fReply = (float) atof(pCmd->inp_buf);
if (fReply == 0) {
@@ -2851,6 +2875,38 @@ static void DMC2280List(void *pData, char *name, SConnection *pCon){
SCWrite(pCon, buffer, eStatus);
return;
}
static void DMC_Notify(void* context, int event)
{
pDMC2280Driv self = (pDMC2280Driv) context;
char line[132];
switch (event) {
case AQU_DISCONNECT:
snprintf(line, 132, "Disconnect on Motor '%s'", self->name);
SICSLogWrite(line, eStatus);
/* TODO: disconnect */
break;
case AQU_RECONNECT:
snprintf(line, 132, "Reconnect on Motor '%s'", self->name);
SICSLogWrite(line, eStatus);
/* TODO: reconnect */
if (self->has_fsm) {
/* Reset the state machine */
if (self->state_timer)
NetWatchRemoveTimer(self->state_timer);
self->state_timer = 0;
change_state(self, DMCState_Unknown);
/* Schedule a timer event as soon as possible */
NetWatchRegisterTimer(&self->state_timer,
0,
state_tmr_callback, self);
}
break;
}
return;
}
/** \brief Free memory if motor is removed
* \param *pData (rw) provides access to the motor's data structure
*/
@@ -3270,6 +3326,7 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
}
else if(strcasecmp("reset", argv[1]) == 0) {
if (self->has_fsm) {
/* Reset the state machine */
if (self->state_timer)
NetWatchRemoveTimer(self->state_timer);
self->state_timer = 0;