diff --git a/polterwrite.c b/polterwrite.c index c4aebd1..885bda3 100644 --- a/polterwrite.c +++ b/polterwrite.c @@ -335,7 +335,11 @@ static void PoldiStart(pPolterdi self, SConnection * pCon) for (i = 0; i < iLength; i++) { fTime2[i] = fTime[i] / 2.; } - sprintf(pBueffel, "%d", iLength); + if(iLength > 2){ + sprintf(pBueffel, "%d", iLength); + } else { + sprintf(pBueffel, "%d", 1); + } NXDupdate(hdict, "timebin", pBueffel); NXDputalias(hfil, hdict, "dtime", fTime2); free(fTime2); diff --git a/tabledrive.c b/tabledrive.c index d6329fc..6700652 100644 --- a/tabledrive.c +++ b/tabledrive.c @@ -367,18 +367,21 @@ static float calculateNextStep(pTableDrive self) /*------------------------------------------------------------------------*/ static int runToNextStep(pTableDrive self, SConnection * pCon, float value) { - int status, test; + int status; tdMotor moti; float target; + long motID; + self->groupID = GetTaskGroupID(pServ->pTasker); status = LLDnodePtr2First(self->motorTable); while (status != 0) { LLDnodeDataTo(self->motorTable, &moti); target = findTarget(moti, value); - test = MotorRun(moti.pMot, pCon, target); - if (test != OKOK) { - return test; + motID = StartDriveTask(moti.pMot,pCon,moti.motorName,target); + if(motID < 0){ + return HWFault; } + AddTaskToGroup(pServ->pTasker,motID,self->groupID); status = LLDnodePtr2Next(self->motorTable); } return 1; @@ -391,18 +394,11 @@ static int checkRunning(pTableDrive self, SConnection * pCon) int test; tdMotor moti; - status = LLDnodePtr2First(self->motorTable); - while (status != 0) { - LLDnodeDataTo(self->motorTable, &moti); - test = moti.pMot->pDrivInt->CheckStatus(moti.pMot, pCon); - if (test != HWIdle && test != OKOK) { - return test; - } - status = LLDnodePtr2Next(self->motorTable); - } + if(isTaskGroupRunning(pServ->pTasker,self->groupID)){ + return HWBusy; + } return HWIdle; } - /*------------------------------------------------------------------------*/ static void showPositions(pTableDrive self, SConnection * pCon, char *pBueffel, int buffLen) @@ -529,11 +525,13 @@ static int startASDS(pTableDrive self, SConnection * pCon, int target) int status; char name[132]; float targetValue; + long motID; if (self == NULL || self->motorTable < 0) { return 0; } + self->groupID = GetTaskGroupID(pServ->pTasker); status = LLDnodePtr2First(self->motorTable); while (status != 0) { LLDnodeDataTo(self->motorTable, &moti); @@ -545,10 +543,12 @@ static int startASDS(pTableDrive self, SConnection * pCon, int target) } else { targetValue = findTarget(moti, self->targetPosition); } - status = MotorRun(moti.pMot, pCon, targetValue); - if (status != OKOK) { - return status; + motID = StartDriveTask(moti.pMot,pCon,moti.motorName,targetValue); + if(motID < 0){ + return HWFault; } + AddTaskToGroup(pServ->pTasker,motID,self->groupID); + } status = LLDnodePtr2Next(self->motorTable); } @@ -562,11 +562,13 @@ static int startOthers(pTableDrive self, SConnection * pCon) int status; char name[132]; float targetValue; + long motID; if (self == NULL || self->motorTable < 0) { return 0; } + self->groupID = GetTaskGroupID(pServ->pTasker); status = LLDnodePtr2First(self->motorTable); while (status != 0) { LLDnodeDataTo(self->motorTable, &moti); @@ -574,10 +576,11 @@ static int startOthers(pTableDrive self, SConnection * pCon) strtolower(name); if (strstr(name, "ds") == NULL && strstr(name, "as") == NULL) { targetValue = findTarget(moti, self->targetPosition); - status = MotorRun(moti.pMot, pCon, targetValue); - if (status != OKOK) { - return status; + motID = StartDriveTask(moti.pMot,pCon,moti.motorName,targetValue); + if(motID < 0){ + return HWFault; } + AddTaskToGroup(pServ->pTasker,motID,self->groupID); } status = LLDnodePtr2Next(self->motorTable); } diff --git a/tabledrive.h b/tabledrive.h index 3648c4e..eced49c 100644 --- a/tabledrive.h +++ b/tabledrive.h @@ -39,6 +39,7 @@ typedef struct { int debug; tdMotor oriMotor; int oriInvalid; + long groupID; } TableDrive, *pTableDrive; /*-------------------------------------------------------------------------*/