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

SICS-761 Zero the motor step counter for rotary encoders.

Browse Source
This commit is contained in:
Douglas Clowes
2014-07-15 12:02:44 +10:00
parent 8849dc90c5
commit 22b34395c1
1 changed files with 38 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

40
site_ansto/motor_dmc2280.c
View File

@ -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: