- Adapted indenation to new agreed upon system
- Added support for second generation scriptcontext based counter
This commit is contained in:
koennecke
819
drive.c
819
drive.c
@ -42,7 +42,7 @@
|
||||
#include "fortify.h"
|
||||
#include "sics.h"
|
||||
#include "drive.h"
|
||||
#include "nserver.h" /* for SicsWait */
|
||||
#include "nserver.h" /* for SicsWait */
|
||||
#include "drive.h"
|
||||
#include "splitter.h"
|
||||
#include "status.h"
|
||||
@ -50,452 +50,405 @@
|
||||
#include "motor.h"
|
||||
|
||||
/*---------------------------------------------------------------------------*/
|
||||
int Drive(SConnection *pCon, SicsInterp *pInter, char *name, float fNew)
|
||||
{
|
||||
CommandList *pObject = NULL;
|
||||
pObjectDescriptor pDes = NULL;
|
||||
pIDrivable pInt = NULL;
|
||||
Dummy *pDum;
|
||||
char pBueffel[512];
|
||||
int iRet;
|
||||
float fDelta, fPos;
|
||||
long lTime;
|
||||
|
||||
assert(pCon);
|
||||
assert(pInter);
|
||||
|
||||
/* check if user is allowed to drive */
|
||||
if(!SCMatchRights(pCon,usUser))
|
||||
{
|
||||
sprintf(pBueffel,"Insuficient Privilege to drive %s",
|
||||
name);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* first try to find the thing to drive */
|
||||
pObject = FindCommand(pInter,name);
|
||||
if(!pObject)
|
||||
{
|
||||
sprintf(pBueffel,"Cannot find %s to drive ", name);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* get the Descriptor, this RELIES on the descriptor being the first
|
||||
thing in the struct
|
||||
*/
|
||||
pDum = (Dummy *)pObject->pData;
|
||||
pDes = pDum->pDescriptor;
|
||||
if(!pDes)
|
||||
{
|
||||
sprintf(pBueffel,"%s is NOT drivable!",pDes->name);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* both scanable and drivable objects can be driven, check this
|
||||
and act accordingly
|
||||
*/
|
||||
int Drive(SConnection * pCon, SicsInterp * pInter, char *name, float fNew)
|
||||
{
|
||||
CommandList *pObject = NULL;
|
||||
pObjectDescriptor pDes = NULL;
|
||||
pIDrivable pInt = NULL;
|
||||
Dummy *pDum;
|
||||
char pBueffel[512];
|
||||
int iRet;
|
||||
float fDelta, fPos;
|
||||
long lTime;
|
||||
|
||||
pInt = pDes->GetInterface(pDum,DRIVEID);
|
||||
if(!pInt)
|
||||
{
|
||||
sprintf(pBueffel,"%s is NOT drivable!",pDes->name);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
if(pInt)
|
||||
{
|
||||
iRet = pInt->CheckLimits(pDum,fNew,pBueffel,511);
|
||||
if(!iRet)
|
||||
{
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
SCSetInterrupt(pCon,eAbortOperation);
|
||||
return 0;
|
||||
}
|
||||
iRet = StartDevice(GetExecutor(),name,pDes,pDum,pCon,fNew);
|
||||
if(!iRet)
|
||||
{
|
||||
sprintf(pBueffel,"ERROR: cannot start device %s",name);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
assert(pCon);
|
||||
assert(pInter);
|
||||
|
||||
/* wait for finish */
|
||||
iRet = Wait4Success(GetExecutor());
|
||||
fPos = pInt->GetValue(pDum,pCon);
|
||||
if(iRet == DEVINT)
|
||||
{
|
||||
if(SCGetInterrupt(pCon) == eAbortOperation)
|
||||
{
|
||||
SCSetInterrupt(pCon,eContinue);
|
||||
sprintf(pBueffel,"Driving %s aborted at %9.3f",name, fPos);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
else if(iRet == DEVDONE)
|
||||
{
|
||||
sprintf(pBueffel,"Driving %s to %9.3f done", name, fPos);
|
||||
SCWrite(pCon,pBueffel,eValue);
|
||||
return 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf(pBueffel,
|
||||
"Driving %s finished with problems, position: %9.3f",name,fPos);
|
||||
SCWrite(pCon,pBueffel,eValue);
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
sprintf(pBueffel,"%s is NOT drivable",pDes->name);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
/* check if user is allowed to drive */
|
||||
if (!SCMatchRights(pCon, usUser)) {
|
||||
sprintf(pBueffel, "Insuficient Privilege to drive %s", name);
|
||||
SCWrite(pCon, pBueffel, eError);
|
||||
return 0;
|
||||
}
|
||||
/*---------------------------------------------------------------------------*/
|
||||
int Start2Run(SConnection *pCon, SicsInterp *pInter, char *name, float fNew)
|
||||
{
|
||||
CommandList *pObject = NULL;
|
||||
pObjectDescriptor pDes = NULL;
|
||||
pIDrivable pInt = NULL;
|
||||
Dummy *pDum;
|
||||
char pBueffel[512];
|
||||
int iRet;
|
||||
float fDelta;
|
||||
long lTime;
|
||||
|
||||
assert(pCon);
|
||||
assert(pInter);
|
||||
|
||||
/* check if user is allowed to drive */
|
||||
if(!SCMatchRights(pCon,usUser))
|
||||
{
|
||||
sprintf(pBueffel,"Insuficient Privilege to drive %s",
|
||||
name);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* first try to find the thing to drive */
|
||||
pObject = FindCommand(pInter,name);
|
||||
if(!pObject)
|
||||
{
|
||||
sprintf(pBueffel,"Cannot find %s to drive ", name);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* get the Descriptor, this RELIES on the descriptor being the first
|
||||
thing in the struct
|
||||
*/
|
||||
pDum = (Dummy *)pObject->pData;
|
||||
pDes = pDum->pDescriptor;
|
||||
if(!pDes)
|
||||
{
|
||||
sprintf(pBueffel,"%s is NOT drivable!",pDes->name);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* Only drivable objects can be driven, check this
|
||||
and act accordingly
|
||||
*/
|
||||
pInt = pDes->GetInterface(pDum,DRIVEID);
|
||||
if(!pInt)
|
||||
{
|
||||
sprintf(pBueffel,"%s is NOT drivable!",pDes->name);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
if(pInt)
|
||||
{
|
||||
iRet = pInt->CheckLimits(pDum,fNew,pBueffel,511);
|
||||
if(!iRet)
|
||||
{
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
SCSetInterrupt(pCon,eAbortOperation);
|
||||
return 0;
|
||||
}
|
||||
iRet = StartDevice(GetExecutor(),name,pDes,pDum,pCon,fNew);
|
||||
if(!iRet)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf(pBueffel,"%s is NOT drivable",pDes->name);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
}
|
||||
|
||||
/* first try to find the thing to drive */
|
||||
pObject = FindCommand(pInter, name);
|
||||
if (!pObject) {
|
||||
sprintf(pBueffel, "Cannot find %s to drive ", name);
|
||||
SCWrite(pCon, pBueffel, eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* get the Descriptor, this RELIES on the descriptor being the first
|
||||
thing in the struct
|
||||
*/
|
||||
pDum = (Dummy *) pObject->pData;
|
||||
pDes = pDum->pDescriptor;
|
||||
if (!pDes) {
|
||||
sprintf(pBueffel, "%s is NOT drivable!", pDes->name);
|
||||
SCWrite(pCon, pBueffel, eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* both scanable and drivable objects can be driven, check this
|
||||
and act accordingly
|
||||
*/
|
||||
|
||||
pInt = pDes->GetInterface(pDum, DRIVEID);
|
||||
if (!pInt) {
|
||||
sprintf(pBueffel, "%s is NOT drivable!", pDes->name);
|
||||
SCWrite(pCon, pBueffel, eError);
|
||||
return 0;
|
||||
}
|
||||
if (pInt) {
|
||||
iRet = pInt->CheckLimits(pDum, fNew, pBueffel, 511);
|
||||
if (!iRet) {
|
||||
SCWrite(pCon, pBueffel, eError);
|
||||
SCSetInterrupt(pCon, eAbortOperation);
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
iRet = StartDevice(GetExecutor(), name, pDes, pDum, pCon, fNew);
|
||||
if (!iRet) {
|
||||
sprintf(pBueffel, "ERROR: cannot start device %s", name);
|
||||
SCWrite(pCon, pBueffel, eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* wait for finish */
|
||||
iRet = Wait4Success(GetExecutor());
|
||||
fPos = pInt->GetValue(pDum, pCon);
|
||||
if (iRet == DEVINT) {
|
||||
if (SCGetInterrupt(pCon) == eAbortOperation) {
|
||||
SCSetInterrupt(pCon, eContinue);
|
||||
sprintf(pBueffel, "Driving %s aborted at %9.3f", name, fPos);
|
||||
SCWrite(pCon, pBueffel, eError);
|
||||
}
|
||||
return 0;
|
||||
} else if (iRet == DEVDONE) {
|
||||
sprintf(pBueffel, "Driving %s to %9.3f done", name, fPos);
|
||||
SCWrite(pCon, pBueffel, eValue);
|
||||
return 1;
|
||||
} else {
|
||||
sprintf(pBueffel,
|
||||
"Driving %s finished with problems, position: %9.3f", name,
|
||||
fPos);
|
||||
SCWrite(pCon, pBueffel, eValue);
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
sprintf(pBueffel, "%s is NOT drivable", pDes->name);
|
||||
SCWrite(pCon, pBueffel, eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------------------*/
|
||||
int Start2Run(SConnection * pCon, SicsInterp * pInter, char *name,
|
||||
float fNew)
|
||||
{
|
||||
CommandList *pObject = NULL;
|
||||
pObjectDescriptor pDes = NULL;
|
||||
pIDrivable pInt = NULL;
|
||||
Dummy *pDum;
|
||||
char pBueffel[512];
|
||||
int iRet;
|
||||
float fDelta;
|
||||
long lTime;
|
||||
|
||||
assert(pCon);
|
||||
assert(pInter);
|
||||
|
||||
/* check if user is allowed to drive */
|
||||
if (!SCMatchRights(pCon, usUser)) {
|
||||
sprintf(pBueffel, "Insuficient Privilege to drive %s", name);
|
||||
SCWrite(pCon, pBueffel, eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* first try to find the thing to drive */
|
||||
pObject = FindCommand(pInter, name);
|
||||
if (!pObject) {
|
||||
sprintf(pBueffel, "Cannot find %s to drive ", name);
|
||||
SCWrite(pCon, pBueffel, eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* get the Descriptor, this RELIES on the descriptor being the first
|
||||
thing in the struct
|
||||
*/
|
||||
pDum = (Dummy *) pObject->pData;
|
||||
pDes = pDum->pDescriptor;
|
||||
if (!pDes) {
|
||||
sprintf(pBueffel, "%s is NOT drivable!", pDes->name);
|
||||
SCWrite(pCon, pBueffel, eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* Only drivable objects can be driven, check this
|
||||
and act accordingly
|
||||
*/
|
||||
pInt = pDes->GetInterface(pDum, DRIVEID);
|
||||
if (!pInt) {
|
||||
sprintf(pBueffel, "%s is NOT drivable!", pDes->name);
|
||||
SCWrite(pCon, pBueffel, eError);
|
||||
return 0;
|
||||
}
|
||||
if (pInt) {
|
||||
iRet = pInt->CheckLimits(pDum, fNew, pBueffel, 511);
|
||||
if (!iRet) {
|
||||
SCWrite(pCon, pBueffel, eError);
|
||||
SCSetInterrupt(pCon, eAbortOperation);
|
||||
return 0;
|
||||
}
|
||||
iRet = StartDevice(GetExecutor(), name, pDes, pDum, pCon, fNew);
|
||||
if (!iRet) {
|
||||
return 0;
|
||||
} else {
|
||||
return 1;
|
||||
}
|
||||
} else {
|
||||
sprintf(pBueffel, "%s is NOT drivable", pDes->name);
|
||||
SCWrite(pCon, pBueffel, eError);
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
/*-------------------------------------------------------------------------
|
||||
This is meant to be called specially from DriveWrapper at a stage when
|
||||
we already know, that name is a drivable motor. Thus no error checking
|
||||
is performed. Do not use this in any other context!!!!
|
||||
*/
|
||||
static float findPosition(SicsInterp *pSics, SConnection *pCon, char *name)
|
||||
{
|
||||
CommandList *pObject = NULL;
|
||||
pObjectDescriptor pDes = NULL;
|
||||
pIDrivable pInt = NULL;
|
||||
Dummy *pDum = NULL;
|
||||
pMotor pMot = NULL;
|
||||
float fPos;
|
||||
int iRet;
|
||||
char pBueffel[132];
|
||||
static float findPosition(SicsInterp * pSics, SConnection * pCon,
|
||||
char *name)
|
||||
{
|
||||
CommandList *pObject = NULL;
|
||||
pObjectDescriptor pDes = NULL;
|
||||
pIDrivable pInt = NULL;
|
||||
Dummy *pDum = NULL;
|
||||
pMotor pMot = NULL;
|
||||
float fPos;
|
||||
int iRet;
|
||||
char pBueffel[132];
|
||||
|
||||
/*
|
||||
treat motors separately in order to correct for zero points
|
||||
Sighh.........
|
||||
*/
|
||||
pMot = FindMotor(pSics,name);
|
||||
if(pMot != NULL)
|
||||
{
|
||||
iRet = MotorGetSoftPosition(pMot,pCon,&fPos);
|
||||
if(iRet)
|
||||
{
|
||||
return fPos;
|
||||
}
|
||||
else
|
||||
{
|
||||
return -999.;
|
||||
}
|
||||
/*
|
||||
treat motors separately in order to correct for zero points
|
||||
Sighh.........
|
||||
*/
|
||||
pMot = FindMotor(pSics, name);
|
||||
if (pMot != NULL) {
|
||||
iRet = MotorGetSoftPosition(pMot, pCon, &fPos);
|
||||
if (iRet) {
|
||||
return fPos;
|
||||
} else {
|
||||
return -999.;
|
||||
}
|
||||
|
||||
pObject = FindCommand(pSics,name);
|
||||
if(pObject)
|
||||
{
|
||||
pDum = (Dummy *)pObject->pData;
|
||||
if(pDum)
|
||||
{
|
||||
pDes = pDum->pDescriptor;
|
||||
if(pDes)
|
||||
{
|
||||
pInt = pDes->GetInterface(pDum,DRIVEID);
|
||||
if(!pInt)
|
||||
{
|
||||
sprintf(pBueffel,
|
||||
"ERROR: internal error in findPosition for %s",
|
||||
name);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return -999.99;
|
||||
}
|
||||
else
|
||||
{
|
||||
return pInt->GetValue(pDum,pCon);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return -999.99;
|
||||
}
|
||||
/*--------------------------------------------------------------------------*/
|
||||
int DriveWrapper(SConnection *pCon, SicsInterp *pSics, void *pData,
|
||||
int argc, char *argv[])
|
||||
{
|
||||
Tcl_Interp *tcl_interp;
|
||||
int iRet,i;
|
||||
double dTarget;
|
||||
char pBueffel[512];
|
||||
Status eOld;
|
||||
|
||||
assert(pCon);
|
||||
assert(pSics);
|
||||
tcl_interp = InterpGetTcl(pSics);
|
||||
|
||||
/* check Status */
|
||||
eOld = GetStatus();
|
||||
|
||||
|
||||
argtolower(argc,argv);
|
||||
/* check no of args */
|
||||
if(argc < 3)
|
||||
{
|
||||
sprintf(pBueffel,"Insufficient number of args. Usage %s name val",
|
||||
argv[0]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* check authorisation */
|
||||
if(!SCMatchRights(pCon,usUser))
|
||||
{
|
||||
SCWrite(pCon,
|
||||
"ERROR: You are not authorized to use the drive command",eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* interprete arguments as pairs name value and try to start */
|
||||
SetStatus(eDriving);
|
||||
for(i = 1; i < argc; i+=2)
|
||||
{
|
||||
if(argv[i+1] == NULL)
|
||||
{
|
||||
sprintf(pBueffel,"ERROR: no value found for driving %s",
|
||||
argv[i]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
SetStatus(eOld);
|
||||
return 0;
|
||||
pObject = FindCommand(pSics, name);
|
||||
if (pObject) {
|
||||
pDum = (Dummy *) pObject->pData;
|
||||
if (pDum) {
|
||||
pDes = pDum->pDescriptor;
|
||||
if (pDes) {
|
||||
pInt = pDes->GetInterface(pDum, DRIVEID);
|
||||
if (!pInt) {
|
||||
sprintf(pBueffel,
|
||||
"ERROR: internal error in findPosition for %s", name);
|
||||
SCWrite(pCon, pBueffel, eError);
|
||||
return -999.99;
|
||||
} else {
|
||||
return pInt->GetValue(pDum, pCon);
|
||||
}
|
||||
iRet = Tcl_GetDouble(tcl_interp, argv[i+1], &dTarget);
|
||||
if (iRet == TCL_ERROR) {
|
||||
SCWrite(pCon, Tcl_GetStringResult(tcl_interp), eError);
|
||||
StopExe(GetExecutor(),"ALL");
|
||||
SetStatus(eOld);
|
||||
return 0;
|
||||
}
|
||||
iRet = Start2Run(pCon,pSics,argv[i],dTarget);
|
||||
if(!iRet)
|
||||
{
|
||||
sprintf(pBueffel,"ERROR: cannot run %s to %s",argv[i],argv[i+1]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
StopExe(GetExecutor(),"ALL");
|
||||
SetStatus(eOld);
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
/* wait for completion */
|
||||
iRet = Wait4Success(GetExecutor());
|
||||
|
||||
/* print positions */
|
||||
for(i = 1; i < argc; i += 2)
|
||||
{
|
||||
sprintf(pBueffel,"New %s position: %9.3f",argv[i],
|
||||
findPosition(pSics,pCon,argv[i]));
|
||||
SCWrite(pCon,pBueffel,eValue);
|
||||
}
|
||||
|
||||
/* check the completion status */
|
||||
if(!(eOld == eScanning || eOld == eBatch))
|
||||
{
|
||||
eOld = eEager;
|
||||
}
|
||||
if(iRet == DEVERROR)
|
||||
{
|
||||
sprintf(pBueffel,"Driving finished with problem");
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
ClearExecutor(GetExecutor());
|
||||
SetStatus(eOld);
|
||||
return 0;
|
||||
}
|
||||
else if(iRet == DEVINT)
|
||||
{
|
||||
sprintf(pBueffel,"ERROR: Driving Interrupted!");
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
ClearExecutor(GetExecutor());
|
||||
SetStatus(eOld);
|
||||
return 0;
|
||||
}
|
||||
SCWrite(pCon,"Driving finished sucessfully",eValue);
|
||||
SetStatus(eOld);
|
||||
return 1;
|
||||
}
|
||||
/*---------------------------------------------------------------------------*/
|
||||
int RunWrapper(SConnection *pCon, SicsInterp *pSics, void *pData,
|
||||
int argc, char *argv[])
|
||||
{
|
||||
Tcl_Interp *tcl_interp;
|
||||
int iRet, i;
|
||||
double dTarget;
|
||||
char pBueffel[512];
|
||||
Status eOld;
|
||||
|
||||
assert(pCon);
|
||||
assert(pSics);
|
||||
tcl_interp = InterpGetTcl(pSics);
|
||||
/* check Status */
|
||||
eOld = GetStatus();
|
||||
|
||||
|
||||
/* check no of args */
|
||||
if(argc < 3)
|
||||
{
|
||||
sprintf(pBueffel,"Insufficient number of args. Usage %s name val",
|
||||
argv[0]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* check authorisation */
|
||||
if(!SCMatchRights(pCon, usUser))
|
||||
{
|
||||
SCWrite(pCon,
|
||||
"ERROR: You are not authorized to use the drive command",eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* interprete arguments as pairs name value and try to start */
|
||||
SetStatus(eDriving);
|
||||
for(i = 1; i < argc; i+=2)
|
||||
{
|
||||
if(argv[i+1] == NULL)
|
||||
{
|
||||
sprintf(pBueffel,"ERROR: no value found for driving %s",
|
||||
argv[i]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
SetStatus(eOld);
|
||||
return 0;
|
||||
}
|
||||
iRet = Tcl_GetDouble(tcl_interp, argv[i+1], &dTarget);
|
||||
if (iRet == TCL_ERROR) {
|
||||
SCWrite(pCon, Tcl_GetStringResult(tcl_interp), eError);
|
||||
StopExe(GetExecutor(),"ALL");
|
||||
SetStatus(eOld);
|
||||
return 0;
|
||||
}
|
||||
iRet = Start2Run(pCon,pSics,argv[i],dTarget);
|
||||
if(!iRet)
|
||||
{
|
||||
sprintf(pBueffel,"ERROR: cannot run %s to %s",argv[i],argv[i+1]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
StopExe(GetExecutor(),"ALL");
|
||||
SetStatus(eOld);
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
/*---------------------------------------------------------------------------*/
|
||||
int MakeDrive(SConnection *pCon, SicsInterp *pSics, void *pData,
|
||||
int argc, char *argv[])
|
||||
{
|
||||
int iRet;
|
||||
char pBueffel[512];
|
||||
|
||||
assert(pCon);
|
||||
assert(pSics);
|
||||
|
||||
if(argc > 1)
|
||||
{
|
||||
iRet = AddCommand(pSics,argv[1], DriveWrapper,NULL,NULL);
|
||||
} else {
|
||||
iRet = AddCommand(pSics,"drive", DriveWrapper,NULL,NULL);
|
||||
}
|
||||
if(!iRet)
|
||||
{
|
||||
sprintf(pBueffel,"ERROR: duplicate command drive not created");
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
if(argc > 2)
|
||||
{
|
||||
iRet = AddCommand(pSics,argv[2], RunWrapper,NULL,NULL);
|
||||
} else {
|
||||
iRet = AddCommand(pSics,"run", RunWrapper,NULL,NULL);
|
||||
}
|
||||
if(!iRet)
|
||||
{
|
||||
sprintf(pBueffel,"ERROR: duplicate command run not created");
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
return -999.99;
|
||||
}
|
||||
|
||||
/*--------------------------------------------------------------------------*/
|
||||
int DriveWrapper(SConnection * pCon, SicsInterp * pSics, void *pData,
|
||||
int argc, char *argv[])
|
||||
{
|
||||
Tcl_Interp *tcl_interp;
|
||||
int iRet, i;
|
||||
double dTarget;
|
||||
char pBueffel[512];
|
||||
Status eOld;
|
||||
|
||||
assert(pCon);
|
||||
assert(pSics);
|
||||
tcl_interp = InterpGetTcl(pSics);
|
||||
|
||||
/* check Status */
|
||||
eOld = GetStatus();
|
||||
|
||||
|
||||
argtolower(argc, argv);
|
||||
/* check no of args */
|
||||
if (argc < 3) {
|
||||
sprintf(pBueffel, "Insufficient number of args. Usage %s name val",
|
||||
argv[0]);
|
||||
SCWrite(pCon, pBueffel, eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* check authorisation */
|
||||
if (!SCMatchRights(pCon, usUser)) {
|
||||
SCWrite(pCon,
|
||||
"ERROR: You are not authorized to use the drive command",
|
||||
eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* interprete arguments as pairs name value and try to start */
|
||||
SetStatus(eDriving);
|
||||
for (i = 1; i < argc; i += 2) {
|
||||
if (argv[i + 1] == NULL) {
|
||||
sprintf(pBueffel, "ERROR: no value found for driving %s", argv[i]);
|
||||
SCWrite(pCon, pBueffel, eError);
|
||||
SetStatus(eOld);
|
||||
return 0;
|
||||
}
|
||||
iRet = Tcl_GetDouble(tcl_interp, argv[i + 1], &dTarget);
|
||||
if (iRet == TCL_ERROR) {
|
||||
SCWrite(pCon, Tcl_GetStringResult(tcl_interp), eError);
|
||||
StopExe(GetExecutor(), "ALL");
|
||||
SetStatus(eOld);
|
||||
return 0;
|
||||
}
|
||||
iRet = Start2Run(pCon, pSics, argv[i], dTarget);
|
||||
if (!iRet) {
|
||||
sprintf(pBueffel, "ERROR: cannot run %s to %s", argv[i],
|
||||
argv[i + 1]);
|
||||
SCWrite(pCon, pBueffel, eError);
|
||||
StopExe(GetExecutor(), "ALL");
|
||||
SetStatus(eOld);
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
/* wait for completion */
|
||||
iRet = Wait4Success(GetExecutor());
|
||||
|
||||
/* print positions */
|
||||
for (i = 1; i < argc; i += 2) {
|
||||
sprintf(pBueffel, "New %s position: %9.3f", argv[i],
|
||||
findPosition(pSics, pCon, argv[i]));
|
||||
SCWrite(pCon, pBueffel, eValue);
|
||||
}
|
||||
|
||||
/* check the completion status */
|
||||
if (!(eOld == eScanning || eOld == eBatch)) {
|
||||
eOld = eEager;
|
||||
}
|
||||
if (iRet == DEVERROR) {
|
||||
sprintf(pBueffel, "Driving finished with problem");
|
||||
SCWrite(pCon, pBueffel, eError);
|
||||
ClearExecutor(GetExecutor());
|
||||
SetStatus(eOld);
|
||||
return 0;
|
||||
} else if (iRet == DEVINT) {
|
||||
sprintf(pBueffel, "ERROR: Driving Interrupted!");
|
||||
SCWrite(pCon, pBueffel, eError);
|
||||
ClearExecutor(GetExecutor());
|
||||
SetStatus(eOld);
|
||||
return 0;
|
||||
}
|
||||
SCWrite(pCon, "Driving finished sucessfully", eValue);
|
||||
SetStatus(eOld);
|
||||
return 1;
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------------------*/
|
||||
int RunWrapper(SConnection * pCon, SicsInterp * pSics, void *pData,
|
||||
int argc, char *argv[])
|
||||
{
|
||||
Tcl_Interp *tcl_interp;
|
||||
int iRet, i;
|
||||
double dTarget;
|
||||
char pBueffel[512];
|
||||
Status eOld;
|
||||
|
||||
assert(pCon);
|
||||
assert(pSics);
|
||||
tcl_interp = InterpGetTcl(pSics);
|
||||
/* check Status */
|
||||
eOld = GetStatus();
|
||||
|
||||
|
||||
/* check no of args */
|
||||
if (argc < 3) {
|
||||
sprintf(pBueffel, "Insufficient number of args. Usage %s name val",
|
||||
argv[0]);
|
||||
SCWrite(pCon, pBueffel, eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* check authorisation */
|
||||
if (!SCMatchRights(pCon, usUser)) {
|
||||
SCWrite(pCon,
|
||||
"ERROR: You are not authorized to use the drive command",
|
||||
eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* interprete arguments as pairs name value and try to start */
|
||||
SetStatus(eDriving);
|
||||
for (i = 1; i < argc; i += 2) {
|
||||
if (argv[i + 1] == NULL) {
|
||||
sprintf(pBueffel, "ERROR: no value found for driving %s", argv[i]);
|
||||
SCWrite(pCon, pBueffel, eError);
|
||||
SetStatus(eOld);
|
||||
return 0;
|
||||
}
|
||||
iRet = Tcl_GetDouble(tcl_interp, argv[i + 1], &dTarget);
|
||||
if (iRet == TCL_ERROR) {
|
||||
SCWrite(pCon, Tcl_GetStringResult(tcl_interp), eError);
|
||||
StopExe(GetExecutor(), "ALL");
|
||||
SetStatus(eOld);
|
||||
return 0;
|
||||
}
|
||||
iRet = Start2Run(pCon, pSics, argv[i], dTarget);
|
||||
if (!iRet) {
|
||||
sprintf(pBueffel, "ERROR: cannot run %s to %s", argv[i],
|
||||
argv[i + 1]);
|
||||
SCWrite(pCon, pBueffel, eError);
|
||||
StopExe(GetExecutor(), "ALL");
|
||||
SetStatus(eOld);
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------------------*/
|
||||
int MakeDrive(SConnection * pCon, SicsInterp * pSics, void *pData,
|
||||
int argc, char *argv[])
|
||||
{
|
||||
int iRet;
|
||||
char pBueffel[512];
|
||||
|
||||
assert(pCon);
|
||||
assert(pSics);
|
||||
|
||||
if (argc > 1) {
|
||||
iRet = AddCommand(pSics, argv[1], DriveWrapper, NULL, NULL);
|
||||
} else {
|
||||
iRet = AddCommand(pSics, "drive", DriveWrapper, NULL, NULL);
|
||||
}
|
||||
if (!iRet) {
|
||||
sprintf(pBueffel, "ERROR: duplicate command drive not created");
|
||||
SCWrite(pCon, pBueffel, eError);
|
||||
return 0;
|
||||
}
|
||||
if (argc > 2) {
|
||||
iRet = AddCommand(pSics, argv[2], RunWrapper, NULL, NULL);
|
||||
} else {
|
||||
iRet = AddCommand(pSics, "run", RunWrapper, NULL, NULL);
|
||||
}
|
||||
if (!iRet) {
|
||||
sprintf(pBueffel, "ERROR: duplicate command run not created");
|
||||
SCWrite(pCon, pBueffel, eError);
|
||||
return 0;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user