diff --git a/site_ansto/motor_dmc2280.c b/site_ansto/motor_dmc2280.c index c5fdd9d4..a3d780ad 100644 --- a/site_ansto/motor_dmc2280.c +++ b/site_ansto/motor_dmc2280.c @@ -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;