- Adapted indenation to new agreed upon system
- Fixed bad status in poldi zug driver
This commit is contained in:
koennecke
435
ptasdrive.c
435
ptasdrive.c
@ -62,284 +62,233 @@ extern char *stptok(const char *s, char *tok, size_t toklen, char *brk);
|
||||
A1-A6, curvature and currents motors.
|
||||
|
||||
|
||||
-------------------------------------------------------------------------*/
|
||||
-------------------------------------------------------------------------*/
|
||||
#define NUM 1
|
||||
#define TXT 2
|
||||
|
||||
int TASDrive(SConnection *pCon, SicsInterp *pSics, void *pData,
|
||||
int argc, char *argv[])
|
||||
int TASDrive(SConnection * pCon, SicsInterp * pSics, void *pData,
|
||||
int argc, char *argv[])
|
||||
{
|
||||
pTASdata self = NULL;
|
||||
int iTAS = 0;
|
||||
float tasTargets[20], oldPos, oldEnergy[MAXEVAR];
|
||||
unsigned char tasTargetMask[20], tasMask[MAXEVAR];
|
||||
char *pPtr, pToken[20], pLine[256];
|
||||
int varPointer, i, motorPointer, status, rStatus, lastToken;
|
||||
char pBueffel[256];
|
||||
unsigned char motorMask[MAXMOT];
|
||||
float newPositions[MAXMOT];
|
||||
pMotor pMot;
|
||||
int qhPars;
|
||||
pTASdata self = NULL;
|
||||
int iTAS = 0;
|
||||
float tasTargets[20], oldPos, oldEnergy[MAXEVAR];
|
||||
unsigned char tasTargetMask[20], tasMask[MAXEVAR];
|
||||
char *pPtr, pToken[20], pLine[256];
|
||||
int varPointer, i, motorPointer, status, rStatus, lastToken;
|
||||
char pBueffel[256];
|
||||
unsigned char motorMask[MAXMOT];
|
||||
float newPositions[MAXMOT];
|
||||
pMotor pMot;
|
||||
int qhPars;
|
||||
|
||||
|
||||
assert(pCon);
|
||||
assert(pSics);
|
||||
self = (pTASdata)pData;
|
||||
assert(self);
|
||||
assert(pCon);
|
||||
assert(pSics);
|
||||
self = (pTASdata) pData;
|
||||
assert(self);
|
||||
|
||||
/*
|
||||
/*
|
||||
check authorization
|
||||
*/
|
||||
if(!SCMatchRights(pCon,usUser))
|
||||
return 0;
|
||||
if (!SCMatchRights(pCon, usUser))
|
||||
return 0;
|
||||
|
||||
|
||||
/* Initialize */
|
||||
Arg2Text(argc, argv,pLine,255);
|
||||
strtolower(pLine);
|
||||
lastToken = NUM;
|
||||
pPtr = pLine + strlen(argv[0]); /* step over command */
|
||||
for(i = 0; i < 10; i++)
|
||||
{
|
||||
tasMask[i] = 0;
|
||||
motorMask[i] = 0;
|
||||
motorMask[10+i] = 0;
|
||||
tasTargets[i] = .0;
|
||||
tasTargets[i+10] = .0;
|
||||
oldEnergy[i] = .0;
|
||||
}
|
||||
tasMask[10] = 0;
|
||||
tasMask[11] = 0;
|
||||
/* Initialize */
|
||||
Arg2Text(argc, argv, pLine, 255);
|
||||
strtolower(pLine);
|
||||
lastToken = NUM;
|
||||
pPtr = pLine + strlen(argv[0]); /* step over command */
|
||||
for (i = 0; i < 10; i++) {
|
||||
tasMask[i] = 0;
|
||||
motorMask[i] = 0;
|
||||
motorMask[10 + i] = 0;
|
||||
tasTargets[i] = .0;
|
||||
tasTargets[i + 10] = .0;
|
||||
oldEnergy[i] = .0;
|
||||
}
|
||||
tasMask[10] = 0;
|
||||
tasMask[11] = 0;
|
||||
|
||||
for(i = 0; i < MAXMOT; i++)
|
||||
{
|
||||
motorMask[i] = 0;
|
||||
}
|
||||
varPointer = -1;
|
||||
motorPointer = -1;
|
||||
rStatus = 1;
|
||||
qhPars = 0;
|
||||
|
||||
/* parse loop */
|
||||
while(pPtr != NULL)
|
||||
{
|
||||
pPtr = stptok(pPtr,pToken,20," ,=");
|
||||
if(strlen(pToken) < 1 || pPtr == NULL )
|
||||
continue;
|
||||
for (i = 0; i < MAXMOT; i++) {
|
||||
motorMask[i] = 0;
|
||||
}
|
||||
varPointer = -1;
|
||||
motorPointer = -1;
|
||||
rStatus = 1;
|
||||
qhPars = 0;
|
||||
|
||||
if(tasNumeric(pToken)) /* numbers */
|
||||
{
|
||||
if(lastToken == NUM)
|
||||
{
|
||||
/* handle storage order logic */
|
||||
if(varPointer > -1 && qhPars > 1)
|
||||
{
|
||||
varPointer++; qhPars--;
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf(pBueffel,"ERROR: parse error at %s, %s",
|
||||
pToken,"need parameter to drive");
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
/* parse loop */
|
||||
while (pPtr != NULL) {
|
||||
pPtr = stptok(pPtr, pToken, 20, " ,=");
|
||||
if (strlen(pToken) < 1 || pPtr == NULL)
|
||||
continue;
|
||||
|
||||
if (tasNumeric(pToken)) { /* numbers */
|
||||
if (lastToken == NUM) {
|
||||
/* handle storage order logic */
|
||||
if (varPointer > -1 && qhPars > 1) {
|
||||
varPointer++;
|
||||
qhPars--;
|
||||
} else {
|
||||
sprintf(pBueffel, "ERROR: parse error at %s, %s",
|
||||
pToken, "need parameter to drive");
|
||||
SCWrite(pCon, pBueffel, eError);
|
||||
return 0;
|
||||
}
|
||||
/* enter the parameter to drive into the appropriate mask */
|
||||
if(motorPointer >= 0)
|
||||
{
|
||||
motorMask[motorPointer] = 1;
|
||||
newPositions[motorPointer] = atof(pToken);
|
||||
}
|
||||
else if(varPointer >= 0 )
|
||||
{
|
||||
tasMask[varPointer] = 1;
|
||||
oldEnergy[varPointer] = self->tasPar[EMIN+varPointer]->fVal;
|
||||
self->tasPar[EMIN + varPointer]->fVal = atof(pToken);
|
||||
self->tasPar[ETARGET + varPointer]->fVal = atof(pToken);
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf(pBueffel,"ERROR: parse error at %s, %s",
|
||||
pToken,"need parameter to drive");
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
lastToken = NUM;
|
||||
}
|
||||
else /* text tokens */
|
||||
{
|
||||
lastToken = TXT;
|
||||
if( (status = isTASEnergy(pToken)) > -1) /* Ei, KI, EF, KF, Q.... */
|
||||
{
|
||||
iTAS = 1;
|
||||
motorPointer = -1;
|
||||
varPointer = status;
|
||||
if (EMIN + varPointer == QH) {
|
||||
qhPars = 4;
|
||||
}
|
||||
}
|
||||
else if( (status = isTASMotor(pToken)) > -1)
|
||||
{
|
||||
motorPointer = status;
|
||||
varPointer = -1;
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf(pBueffel,"ERROR: cannot drive %s", pToken);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
rStatus = 0;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}/* end of parse loop */
|
||||
}
|
||||
/* enter the parameter to drive into the appropriate mask */
|
||||
if (motorPointer >= 0) {
|
||||
motorMask[motorPointer] = 1;
|
||||
newPositions[motorPointer] = atof(pToken);
|
||||
} else if (varPointer >= 0) {
|
||||
tasMask[varPointer] = 1;
|
||||
oldEnergy[varPointer] = self->tasPar[EMIN + varPointer]->fVal;
|
||||
self->tasPar[EMIN + varPointer]->fVal = atof(pToken);
|
||||
self->tasPar[ETARGET + varPointer]->fVal = atof(pToken);
|
||||
} else {
|
||||
sprintf(pBueffel, "ERROR: parse error at %s, %s",
|
||||
pToken, "need parameter to drive");
|
||||
SCWrite(pCon, pBueffel, eError);
|
||||
return 0;
|
||||
}
|
||||
lastToken = NUM;
|
||||
} else { /* text tokens */
|
||||
|
||||
if(rStatus != 1)
|
||||
return rStatus;
|
||||
lastToken = TXT;
|
||||
if ((status = isTASEnergy(pToken)) > -1) { /* Ei, KI, EF, KF, Q.... */
|
||||
iTAS = 1;
|
||||
motorPointer = -1;
|
||||
varPointer = status;
|
||||
if (EMIN + varPointer == QH) {
|
||||
qhPars = 4;
|
||||
}
|
||||
} else if ((status = isTASMotor(pToken)) > -1) {
|
||||
motorPointer = status;
|
||||
varPointer = -1;
|
||||
} else {
|
||||
sprintf(pBueffel, "ERROR: cannot drive %s", pToken);
|
||||
SCWrite(pCon, pBueffel, eError);
|
||||
rStatus = 0;
|
||||
break;
|
||||
}
|
||||
}
|
||||
} /* end of parse loop */
|
||||
|
||||
if (rStatus != 1)
|
||||
return rStatus;
|
||||
|
||||
/*
|
||||
store SRO motor value
|
||||
*/
|
||||
getSRO(pCon,&self->oldSRO);
|
||||
store SRO motor value
|
||||
*/
|
||||
getSRO(pCon, &self->oldSRO);
|
||||
|
||||
|
||||
/* having done this, we can start the motors */
|
||||
for(i = 0; i < MAXMOT; i++)
|
||||
{
|
||||
if(motorMask[i] > 0)
|
||||
{
|
||||
pMot = FindMotor(pSics, tasMotorOrder[i]);
|
||||
if(pMot)
|
||||
{
|
||||
MotorGetSoftPosition(pMot, pCon, &oldPos);
|
||||
sprintf(pBueffel,"Driving %s from %f to %f",
|
||||
tasMotorOrder[i],oldPos,newPositions[i]);
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf(pBueffel,"Driving %s to %f",
|
||||
tasMotorOrder[i],newPositions[i]);
|
||||
}
|
||||
SCWrite(pCon,pBueffel,eWarning);
|
||||
status = StartMotor(pServ->pExecutor,pSics,pCon,
|
||||
tasMotorOrder[i],newPositions[i]);
|
||||
if(status == 0)
|
||||
{
|
||||
/* error should already have been reported by StartMotor*/
|
||||
rStatus = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
/* having done this, we can start the motors */
|
||||
for (i = 0; i < MAXMOT; i++) {
|
||||
if (motorMask[i] > 0) {
|
||||
pMot = FindMotor(pSics, tasMotorOrder[i]);
|
||||
if (pMot) {
|
||||
MotorGetSoftPosition(pMot, pCon, &oldPos);
|
||||
sprintf(pBueffel, "Driving %s from %f to %f",
|
||||
tasMotorOrder[i], oldPos, newPositions[i]);
|
||||
} else {
|
||||
sprintf(pBueffel, "Driving %s to %f",
|
||||
tasMotorOrder[i], newPositions[i]);
|
||||
}
|
||||
SCWrite(pCon, pBueffel, eWarning);
|
||||
status = StartMotor(pServ->pExecutor, pSics, pCon,
|
||||
tasMotorOrder[i], newPositions[i]);
|
||||
if (status == 0) {
|
||||
/* error should already have been reported by StartMotor */
|
||||
rStatus = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
/*
|
||||
if in TAS mode do the TAS calculation and start the appropriate
|
||||
motors.
|
||||
*/
|
||||
if(iTAS > 0)
|
||||
{
|
||||
if (qhPars == 3) {
|
||||
SCWrite(pCon, "ERROR: QH cannot have 2 values", eError);
|
||||
return 0;
|
||||
}
|
||||
if (iTAS > 0) {
|
||||
if (qhPars == 3) {
|
||||
SCWrite(pCon, "ERROR: QH cannot have 2 values", eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
status = TASCalc(self,pCon,tasMask,tasTargets, tasTargetMask);
|
||||
if(status)
|
||||
{
|
||||
/*
|
||||
do output, first Q-E variables
|
||||
status = TASCalc(self, pCon, tasMask, tasTargets, tasTargetMask);
|
||||
if (status) {
|
||||
/*
|
||||
do output, first Q-E variables
|
||||
*/
|
||||
for(i = 0; i < 12; i++)
|
||||
{
|
||||
if(tasMask[i])
|
||||
{
|
||||
sprintf(pBueffel,"Driving %s from %f to %f", tasVariableOrder[EI+i],
|
||||
oldEnergy[i],
|
||||
self->tasPar[EI+i]->fVal);
|
||||
SCWrite(pCon,pBueffel,eWarning);
|
||||
}
|
||||
}
|
||||
/*
|
||||
more output: the motor positions
|
||||
for (i = 0; i < 12; i++) {
|
||||
if (tasMask[i]) {
|
||||
sprintf(pBueffel, "Driving %s from %f to %f",
|
||||
tasVariableOrder[EI + i], oldEnergy[i],
|
||||
self->tasPar[EI + i]->fVal);
|
||||
SCWrite(pCon, pBueffel, eWarning);
|
||||
}
|
||||
}
|
||||
/*
|
||||
more output: the motor positions
|
||||
*/
|
||||
for(i = 0; i < 9; i++)
|
||||
{
|
||||
if(tasTargetMask[i])
|
||||
{
|
||||
pMot = FindMotor(pSics, tasMotorOrder[i]);
|
||||
if(pMot)
|
||||
{
|
||||
MotorGetSoftPosition(pMot, pCon, &oldPos);
|
||||
}
|
||||
else
|
||||
{
|
||||
oldPos = -9999.;
|
||||
}
|
||||
sprintf(pBueffel,"Driving %s from %f to %f",tasMotorOrder[i],
|
||||
oldPos, tasTargets[i]);
|
||||
SCWrite(pCon,pBueffel,eWarning);
|
||||
}
|
||||
}
|
||||
/*
|
||||
for (i = 0; i < 9; i++) {
|
||||
if (tasTargetMask[i]) {
|
||||
pMot = FindMotor(pSics, tasMotorOrder[i]);
|
||||
if (pMot) {
|
||||
MotorGetSoftPosition(pMot, pCon, &oldPos);
|
||||
} else {
|
||||
oldPos = -9999.;
|
||||
}
|
||||
sprintf(pBueffel, "Driving %s from %f to %f", tasMotorOrder[i],
|
||||
oldPos, tasTargets[i]);
|
||||
SCWrite(pCon, pBueffel, eWarning);
|
||||
}
|
||||
}
|
||||
/*
|
||||
output for magnet currents
|
||||
*/
|
||||
for(i = 0; i < 8; i++)
|
||||
{
|
||||
if(tasTargetMask[9+i])
|
||||
{
|
||||
oldPos = readDrivable(tasMotorOrder[CURMOT+i], pCon);
|
||||
sprintf(pBueffel,"Driving %s from %f to %f",tasMotorOrder[CURMOT+i],
|
||||
oldPos, tasTargets[9+i]);
|
||||
SCWrite(pCon,pBueffel,eWarning);
|
||||
}
|
||||
}
|
||||
|
||||
status = TASStart(self,pCon,pSics,tasTargets,tasTargetMask);
|
||||
if(status == 0)
|
||||
{
|
||||
rStatus = 0;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
for (i = 0; i < 8; i++) {
|
||||
if (tasTargetMask[9 + i]) {
|
||||
oldPos = readDrivable(tasMotorOrder[CURMOT + i], pCon);
|
||||
sprintf(pBueffel, "Driving %s from %f to %f",
|
||||
tasMotorOrder[CURMOT + i], oldPos, tasTargets[9 + i]);
|
||||
SCWrite(pCon, pBueffel, eWarning);
|
||||
}
|
||||
}
|
||||
|
||||
status = TASStart(self, pCon, pSics, tasTargets, tasTargetMask);
|
||||
if (status == 0) {
|
||||
rStatus = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
} else {
|
||||
rStatus = 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
/*
|
||||
wait till we are finished
|
||||
*/
|
||||
status = Wait4Success(GetExecutor());
|
||||
TASUpdate(self,pCon);
|
||||
status = Wait4Success(GetExecutor());
|
||||
TASUpdate(self, pCon);
|
||||
|
||||
/*
|
||||
/*
|
||||
handle interrupts
|
||||
*/
|
||||
if(status == DEVINT)
|
||||
{
|
||||
if(SCGetInterrupt(pCon) == eAbortOperation)
|
||||
{
|
||||
SCSetInterrupt(pCon,eContinue);
|
||||
sprintf(pBueffel,"Driving aborted");
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
}
|
||||
return 0;
|
||||
if (status == DEVINT) {
|
||||
if (SCGetInterrupt(pCon) == eAbortOperation) {
|
||||
SCSetInterrupt(pCon, eContinue);
|
||||
sprintf(pBueffel, "Driving aborted");
|
||||
SCWrite(pCon, pBueffel, eError);
|
||||
}
|
||||
else if(status == DEVDONE)
|
||||
{
|
||||
sprintf(pBueffel,"Driving done");
|
||||
SCWrite(pCon,pBueffel,eValue);
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf(pBueffel,
|
||||
"Driving finished");
|
||||
SCWrite(pCon,pBueffel,eValue);
|
||||
}
|
||||
return rStatus;
|
||||
return 0;
|
||||
} else if (status == DEVDONE) {
|
||||
sprintf(pBueffel, "Driving done");
|
||||
SCWrite(pCon, pBueffel, eValue);
|
||||
} else {
|
||||
sprintf(pBueffel, "Driving finished");
|
||||
SCWrite(pCon, pBueffel, eValue);
|
||||
}
|
||||
return rStatus;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user