diff --git a/oscillate.c b/oscillate.c index b65ba11e..b6edcc5c 100644 --- a/oscillate.c +++ b/oscillate.c @@ -13,19 +13,25 @@ #include "task.h" #include "commandlog.h" #include "oscillate.h" +#include "status.h" #define ABS(x) (x < 0 ? -(x) : (x)) /*================== real work =========================================*/ static void StopOscillation(pOscillator self) { + int savedStatus; + assert(self != NULL); if (self->taskID > 0) { self->pMot->pDriver->Halt(self->pMot->pDriver); self->stopFlag = 1; self->taskID = -1; } + savedStatus = GetStatus(); /* fool status check in ObParSet (avoid "Cannot change parameter while running" message */ + SetStatus(eEager); MotorSetPar(self->pMot, self->pCon, "accesscode", usUser); + SetStatus(savedStatus); if (self->debug > 0) { WriteToCommandLog("oscillator>> ", "Stopping"); } @@ -68,6 +74,7 @@ static int OscillationTask(void *data) assert(self); if (self->stopFlag == 1) { + finishDriving(self->pMot, self->pCon); return 0; } @@ -104,10 +111,16 @@ static int OscillationTask(void *data) case HWBusy: break; case HWIdle: + if (self->cycles > 0 && self->currcycle == self->cycles) { + StopOscillation(self); + return 1; + } pos = getNextPos(self); status = MotorRun(self->pMot, self->pCon, pos); if (status == OKOK) { self->pMot->pDrivInt->iErrorCount = 0; + if (self->cycles > 0) + (self->currcycle)++; } if (self->debug > 0) { snprintf(message, 131, "Started oscillation to %f, ret code = %d", @@ -122,7 +135,7 @@ static int OscillationTask(void *data) static int StartOscillation(pOscillator self, SConnection * pCon) { float fval; - int status; + int status, savedStatus; char error[80], pBueffel[255]; assert(self); @@ -138,7 +151,10 @@ static int StartOscillation(pOscillator self, SConnection * pCon) self->lowerLimit += .5; MotorGetPar(self->pMot, "softupperlim", &self->upperLimit); self->upperLimit -= .5; + savedStatus = GetStatus(); /* fool status check in ObParSet (avoid "Cannot change parameter while running" message */ + SetStatus(eEager); MotorSetPar(self->pMot, self->pCon, "accesscode", (float) usInternal); + SetStatus(savedStatus); self->nextTargetFlag = 0; self->errorCount = 0; self->stopFlag = 0; @@ -221,6 +237,8 @@ int MakeOscillator(SConnection * pCon, SicsInterp * pSics, void *pData, memset(pNew, 0, sizeof(Oscillator)); pNew->pDes = CreateDescriptor("Oscillator"); pNew->pMot = pMot; + pNew->cycles = 0; + pNew->currcycle = 0; pNew->pCon = SCCreateDummyConnection(pSics); if (!pNew->pDes || !pNew->pCon) { SCWrite(pCon, "ERROR: out of memory creating oscillator", eError); @@ -259,6 +277,12 @@ int OscillatorWrapper(SConnection * pCon, SicsInterp * pSics, void *pData, } strtolower(argv[1]); + if (argc == 3) { + self->cycles = 2 * atoi(argv[2]); + } else { + self->cycles = 0; + } + self->currcycle = 0; if (strcmp(argv[1], "start") == 0) { return StartOscillation(self, pCon); } else if (strcmp(argv[1], "stop") == 0) {