diff --git a/confvirtualmot.c b/confvirtualmot.c index b42523b6..7e99b2f0 100644 --- a/confvirtualmot.c +++ b/confvirtualmot.c @@ -23,6 +23,7 @@ extern char *stptok(char *s, char *t, int len, char *brk); extern int CheckMotiMatch(const void* context, const void* pUserData); +extern double DoubleTime(); /*--------------------------------------------------------------------------- An internal data structure which holds information required to control @@ -191,6 +192,7 @@ static long ConfSetValue(void *pData, SConnection *pCon, float newValue){ self->targetValue = newValue; self->targetReached = 0; self->posCount = 0; + self->last_report_time = 0.0; status = startMotorList(self,pCon); if(status != OKOK){ @@ -314,10 +316,10 @@ static int ConfCheckStatus(void *pData, SConnection *pCon){ InvokeCallBack(self->pCall, MOTDRIVE, &event); } else if (result == HWBusy) { #if 1 - double current_time, skip_time, DoubleTime(void); + double current_time, skip_time; current_time = DoubleTime(); skip_time = 0.500; - if (self->last_report_time + skip_time >= current_time) + if (self->last_report_time + skip_time <= current_time) #else self->posCount++; if(self->posCount >= 10/*ObVal(self->ParArray,MOVECOUNT)*/) @@ -469,6 +471,7 @@ int MakeConfigurableVirtualMotor(SConnection *pCon, SicsInterp *pSics, pNew->name = strdup(argv[1]); pNew->posCount = 0; + pNew->last_report_time = 0.0; pNew->pDes = CreateDescriptor("ConfigurableVirtualMotor"); pNew->pDriv = CreateDrivableInterface(); pNew->motorList = LLDcreate(sizeof(RealMotor)); diff --git a/motor.c b/motor.c index 97ba01ec..0b98eab2 100644 --- a/motor.c +++ b/motor.c @@ -76,6 +76,7 @@ #define IGNOREFAULT 10 #define MOVECOUNT 11 +extern double DoubleTime(void); /*-------------------------------------------------------------------------*/ static void *MotorGetInterface(void *pData, int iID) @@ -369,10 +370,10 @@ static int evaluateStatus(pMotor self, SConnection *pCon) static void handleMoveCallback(pMotor self, SConnection *pCon) { #if 1 - double current_time, skip_time, DoubleTime(void); + double current_time, skip_time; current_time = DoubleTime(); skip_time = 0.001 * ObVal(self->ParArray,MOVECOUNT); - if(self->last_report_time + skip_time >= current_time) + if(self->last_report_time + skip_time <= current_time) #else self->posCount++; if(self->posCount >= ObVal(self->ParArray,MOVECOUNT)) @@ -808,6 +809,7 @@ extern void KillPiPiezo(void *pData); self->moving = 1; self->fTarget = fHard; InvokeCallBack(self->pCall,HDBVAL,self); + self->last_report_time = 0.0; self->posCount = 0; iRet = self->pDriver->RunTo(self->pDriver,fHard); if(iRet != OKOK)