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

SICS-597: Don't drive a motor if it is within precision of the target.

Browse Source
This commit is contained in:
Ferdi Franceschini
2013-04-27 18:16:30 +10:00
parent 44579ee8aa
commit a6cfbe8b7f
1 changed files with 9 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
motor.c
View File

@ -689,8 +689,8 @@ static int MotorGetHardPositionImpl(pMotor self, SConnection * pCon,
/*---------------------------------------------------------------------------*/ /*---------------------------------------------------------------------------*/
static long MotorRunImpl(void *sulf, SConnection * pCon, float fNew) static long MotorRunImpl(void *sulf, SConnection * pCon, float fNew)
{ {
float fHard; float fHard, fCurrHard, fNewHard, fZero;
int i, iRet, iCode; int i, iRet, iCode, iSign;
char pBueffel[512]; char pBueffel[512];
char pError[132]; char pError[132];
pMotor self; pMotor self;
@ -709,6 +709,13 @@ static long MotorRunImpl(void *sulf, SConnection * pCon, float fNew)
SCSetInterrupt(pCon, eAbortBatch); SCSetInterrupt(pCon, eAbortBatch);
return 0; return 0;
} }
self->pDriver->GetPosition(self->pDriver, &fCurrHard);
iSign = ObVal(self->ParArray, SIGN);
fZero = ObVal(self->ParArray, SZERO);
fNewHard = iSign * (fNew + fZero);
if (absf(fCurrHard - fNewHard) <= ObVal(self->ParArray, PREC)) {
return OKOK;
}
/* check boundaries first */ /* check boundaries first */
iRet = MotorCheckBoundaryImpl(self, fNew, &fHard, pError, 131); iRet = MotorCheckBoundaryImpl(self, fNew, &fHard, pError, 131);