From 5d311bf61f79504e8e1dc2f2e010ed57ebf60d9f Mon Sep 17 00:00:00 2001 From: Douglas Clowes Date: Thu, 11 Oct 2007 18:00:02 +1000 Subject: [PATCH] A bit more rationalisation r2183 | dcl | 2007-10-11 18:00:02 +1000 (Thu, 11 Oct 2007) | 2 lines --- site_ansto/motor_dmc2280.c | 101 +++++++++++++++++++++++++++---------- 1 file changed, 73 insertions(+), 28 deletions(-) diff --git a/site_ansto/motor_dmc2280.c b/site_ansto/motor_dmc2280.c index 5c15d392..60a0466c 100644 --- a/site_ansto/motor_dmc2280.c +++ b/site_ansto/motor_dmc2280.c @@ -78,6 +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 + static pAsyncProtocol DMC2280_Protocol = NULL; /*----------------------------------------------------------------------- @@ -178,6 +185,7 @@ struct __MoDriv { int posit_count; bool posit_check; unsigned long long *posit_array; + int variables; }; int DMC2280MotionControl = 1; /* defaults to enabled */ @@ -1068,6 +1076,12 @@ static int cmdStatus(pDMC2280Driv self) { return DMC_SendCmd(self, cmd, state_msg_callback); } +static int cmdVars(pDMC2280Driv self) { + char cmd[CMDLEN]; + snprintf(cmd, CMDLEN, "LV"); + return DMC_SendCmd(self, cmd, state_msg_callback); +} + static int cmdOn(pDMC2280Driv self) { char cmd[CMDLEN]; if (self->has_airpads == 1) { @@ -1102,6 +1116,26 @@ static int cmdBegin(pDMC2280Driv self) return DMC_SendCmd(self, cmd, state_msg_callback); } +static int cmdPoll(pDMC2280Driv self) +{ + char cmd[CMDLEN]; + if (self->has_airpads == 2 || self->has_airpads == 3) + snprintf(cmd, CMDLEN, "MG RSP%c", self->axisLabel); + else if (self->has_airpads == 1) + snprintf(cmd, CMDLEN, "MG APDONE"); + return DMC_SendCmd(self, cmd, state_msg_callback); +} + +static int 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_SendCmd(self, cmd, state_msg_callback); +} + static int cmdOff(pDMC2280Driv self) { char cmd[CMDLEN]; if (self->has_airpads == 1) { @@ -1142,6 +1176,32 @@ static int rspStatus(pDMC2280Driv self, const char* text) { return 1; } +static int rspVars(pDMC2280Driv self, const char* text) { + self->variables = 0; + if (has_var_x(self, text, "REQ")) + self->variables |= VAR_REQ; + if (has_var_x(self, text, "RSP")) + self->variables |= VAR_RSP; + if (has_var_x(self, text, "RUN")) + self->variables |= VAR_RUN; + if (has_var_x(self, text, "DST")) + self->variables |= VAR_DST; + if (has_var_x(self, text, "POS")) + self->variables |= VAR_POS; + if (has_var_x(self, text, "HLT")) + self->variables |= VAR_HLT; + if ((self->variables & VAR_REQ) && + (self->variables & VAR_RSP)) { + self->has_airpads = 2; + if ((self->variables & VAR_RUN) && + (self->variables & VAR_DST) && + (self->variables & VAR_POS)) { + self->has_airpads = 3; + } + } + return 1; +} + /* State Functions */ static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event); @@ -1365,21 +1425,12 @@ static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event) { if (self->pMot) { self->pMot->fTarget = self->pMot->fPosition = motPosit(self); } - snprintf(cmd, CMDLEN, "LV"); - DMC_SendCmd(self, cmd, state_msg_callback); + cmdVars(self); self->subState = 2; return; } - if (self->subState == 2) { /* LV */ - if (has_var_x(self, pCmd->inp_buf, "REQ")) - if (has_var_x(self, pCmd->inp_buf, "RSP")) { - self->has_airpads = 2; - if (has_var_x(self, pCmd->inp_buf, "RUN")) - if (has_var_x(self, pCmd->inp_buf, "DST")) - if (has_var_x(self, pCmd->inp_buf, "POS")) { - self->has_airpads = 3; - } - } + if (self->subState == 2) { /* Vars */ + rspVars(self, pCmd->inp_buf); change_state(self, DMCState_Idle); return; } @@ -1512,11 +1563,7 @@ static void DMCState_MotorStart(pDMC2280Driv self, pEvtEvent event) { self->subState = 1; return; case eTimerEvent: - if (self->has_airpads == 2 || self->has_airpads == 3) - snprintf(cmd, CMDLEN, "MG RSP%c", self->axisLabel); - else if (self->has_airpads == 1) - snprintf(cmd, CMDLEN, "MG APDONE"); - DMC_SendCmd(self, cmd, state_msg_callback); + cmdPoll(self); self->subState = 2; return; case eMessageEvent: @@ -1535,7 +1582,7 @@ static void DMCState_MotorStart(pDMC2280Driv self, pEvtEvent event) { return; } } - else if (self->subState == 2) { /* MG APDONE/RSPx */ + else if (self->subState == 2) { /* Poll */ float fReply; fReply = (float) atof(pCmd->inp_buf); if (fReply > 0) { @@ -1582,6 +1629,7 @@ static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event) { iRet = rspStatus(self, pCmd->inp_buf); if (iRet == 0) break; + /* TODO AMPERR */ set_lastMotion(self, self->currSteps, self->currCounts); /* compute position for PA command */ target = self->fTarget; @@ -1604,7 +1652,7 @@ static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event) { } else if (self->backlash_offset < 0) { /* - * We want to be moving from to low high , + * We want to be moving from low to high, * if the target is lower than current * we must pre-seek to the lower side */ @@ -1681,6 +1729,7 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) { iRet = rspStatus(self, pCmd->inp_buf); if (iRet == 0) break; + /* TODO AMPERR */ iFlags = self->currFlags; moving = (iFlags & STATUSMOVING); if (moving) { @@ -1765,8 +1814,8 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) { } } - /* limit the maximum number of tries */ if (self->preseek && self->stepCount > 10) { + /* limit the maximum number of tries */ if (self->debug) { char line[CMDLEN]; snprintf(line, CMDLEN, "Motor=%s preseek stopped, stepcount = %d", @@ -1774,6 +1823,7 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) { SICSLogWrite(line, eStatus); } self->preseek = 0; + absolute = motCreep(self, target); } else if (self->preseek) { absolute = motCreep(self, target); @@ -1876,8 +1926,7 @@ static void DMCState_MotorHalt(pDMC2280Driv self, pEvtEvent event) { if (self->state_timer) NetWatchRemoveTimer(self->state_timer); self->state_timer = 0; - snprintf(cmd, CMDLEN, "ST%c", self->axisLabel); - DMC_SendCmd(self, cmd, state_msg_callback); + cmdHalt(self); return; case eTimerEvent: cmdStatus(self); @@ -1959,11 +2008,7 @@ static void DMCState_MotorStop(pDMC2280Driv self, pEvtEvent event) { cmdOff(self); return; case eTimerEvent: - if (self->has_airpads == 2 || self->has_airpads == 3) - snprintf(cmd, CMDLEN, "MG RSP%c", self->axisLabel); - else - snprintf(cmd, CMDLEN, "MG APDONE"); - DMC_SendCmd(self, cmd, state_msg_callback); + cmdPoll(self); self->subState = 1; return; case eMessageEvent: @@ -1975,7 +2020,7 @@ static void DMCState_MotorStop(pDMC2280Driv self, pEvtEvent event) { return; } } - else if (self->subState == 1) { /* MG APDONE/RSPx */ + else if (self->subState == 1) { /* Poll */ float fReply; fReply = (float) atof(pCmd->inp_buf); if (fReply == 0) {