diff --git a/site_ansto/motor_dmc2280.c b/site_ansto/motor_dmc2280.c index 08db73c6..7209021e 100644 --- a/site_ansto/motor_dmc2280.c +++ b/site_ansto/motor_dmc2280.c @@ -1640,6 +1640,20 @@ static int cmdOn(pDMC2280Driv self) { return DMC_SendReq(self, cmd); } +static int cmdZero(pDMC2280Driv self) { + char cmd[CMDLEN]; + if (self->abs_encoder == 0) { + SICSLogPrintf(eError, "motor=%s, Attempt to zero non-absolute motor", self->name); + snprintf(cmd, CMDLEN, "TD%c", self->axisLabel); + } else if (self->protocol == 3) { + SICSLogPrintf(eError, "motor=%s, Attempt to zero protocol 3 motor", self->name); + snprintf(cmd, CMDLEN, "TD%c", self->axisLabel); + } else { + snprintf(cmd, CMDLEN, "DP%c=0", self->axisLabel); + } + return DMC_SendReq(self, cmd); +} + static int cmdPosition(pDMC2280Driv self, int target) { char cmd[CMDLEN]; self->lastPosition = motPosit(self); @@ -2575,8 +2589,13 @@ static void DMCState_MotorStart(pDMC2280Driv self, pEvtEvent event) { change_state(self, DMCState_Idle); return; } - cmdOn(self); - self->subState = 1; + if (self->abs_encoder && self->rotary_bits > 0) { + cmdZero(self); + self->subState = 3; + } else { + cmdOn(self); + self->subState = 1; + } return; case eTimerEvent: cmdPoll(self); @@ -2625,6 +2644,23 @@ static void DMCState_MotorStart(pDMC2280Driv self, pEvtEvent event) { self->subState = 0; return; } + else if (self->subState == 3) { /* Zero */ + cmdStatus(self); + self->status_valid = false; + self->subState = 4; + } + else if (self->subState == 4) { /* Status after Zero */ + int iRet; + iRet = rspStatus(self, pCmd->inp_buf); + if (iRet == 0) + break; + if (fabs(self->stepsPerX) < fabs(self->currSteps)) { + SICSLogPrintf(eError, "motor=%s, cmdZero failed %d", self->name, self->currSteps); + } + set_lastMotion(self, self->currSteps, self->currCounts); + cmdOn(self); + self->subState = 1; + } } while (0); break; case eCommandEvent: