linse/sics
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
main
sics/drive.c

533 lines
15 KiB
C
Raw Permalink Blame History

/*--------------------------------------------------------------------------
Implementation file for the drive command
This version as Test for Device Executor.
Mark Koennecke, December 1996
Labor fuer Neutronenstreuung
Paul Scherrer Institut
CH-5423 Villigen-PSI
The authors hereby grant permission to use, copy, modify, distribute,
and license this software and its documentation for any purpose, provided
that existing copyright notices are retained in all copies and that this
notice is included verbatim in any distributions. No written agreement,
license, or royalty fee is required for any of the authorized uses.
Modifications to this software may be copyrighted by their authors
and need not follow the licensing terms described here, provided that
the new terms are clearly indicated on the first page of each file where
they apply.
IN NO EVENT SHALL THE AUTHORS OR DISTRIBUTORS BE LIABLE TO ANY PARTY
FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
ARISING OUT OF THE USE OF THIS SOFTWARE, ITS DOCUMENTATION, OR ANY
DERIVATIVES THEREOF, EVEN IF THE AUTHORS HAVE BEEN ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE.
THE AUTHORS AND DISTRIBUTORS SPECIFICALLY DISCLAIM ANY WARRANTIES,
INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE, AND NON-INFRINGEMENT. THIS SOFTWARE
IS PROVIDED ON AN "AS IS" BASIS, AND THE AUTHORS AND DISTRIBUTORS HAVE
NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR
MODIFICATIONS.
------------------------------------------------------------------------------*/
#include <stdlib.h>
#include <assert.h>
#include <string.h>
#include <math.h>
#include "fortify.h"
#include "sics.h"
#include "drive.h"
#include "nserver.h" /* for SicsWait */
#include "drive.h"
#include "splitter.h"
#include "status.h"
#include "devexec.h"
#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)) {
snprintf(pBueffel, 511, "Insufficient 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) {
snprintf(pBueffel,511, "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) {
snprintf(pBueffel,511, "%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) {
snprintf(pBueffel,511, "%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, eLogError);
SCSetInterrupt(pCon, eAbortOperation);
return 0;
}
iRet = StartDevice(GetExecutor(), name, pDes, pDum, pCon, RUNDRIVE, fNew);
if (!iRet) {
snprintf(pBueffel,511, "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);
snprintf(pBueffel, 511, "Driving %s aborted at %.6g", name, fPos);
SCWrite(pCon, pBueffel, eError);
}
return 0;
} else if (iRet == DEVDONE) {
snprintf(pBueffel, 511, "Driving %s to %.6g done", name, fPos);
SCWrite(pCon, pBueffel, eValue);
return 1;
} else {
snprintf(pBueffel, 511,
"Driving %s finished with problems, position: %.6g", name,
fPos);
SCWrite(pCon, pBueffel, eValue);
return 1;
}
}
snprintf(pBueffel, 511, "%s is NOT drivable", pDes->name);
SCWrite(pCon, pBueffel, eError);
return 0;
}
/*---------------------------------------------------------------------------*/
int Start2Run(SConnection * pCon, SicsInterp * pInter, char *name,
int level, float fNew)
{
CommandList *pObject = NULL;
pObjectDescriptor pDes = NULL;
pIDrivable pInt = NULL;
Dummy *pDum;
char pBueffel[512];
int iRet;
float fDelta;
long lTime;
float target;
assert(pCon);
assert(pInter);
/* check if user is allowed to drive */
if (!SCMatchRights(pCon, usUser)) {
snprintf(pBueffel,511, "ERROR: 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) {
snprintf(pBueffel,511, "ERROR: 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) {
snprintf(pBueffel,511, "ERROR: %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) {
snprintf(pBueffel,511, "ERROR: %s is NOT drivable!", pDes->name);
SCWrite(pCon, pBueffel, eError);
return 0;
}
if (pInt) {
iRet = pInt->CheckLimits(pDum, fNew, pBueffel, 511);
if (!iRet) {
SCPrintf(pCon, eError, "ERROR: %s", pBueffel);
SCSetInterrupt(pCon, eAbortOperation);
return 0;
}
iRet = StartDevice(GetExecutor(), name, pDes, pDum, pCon,level, fNew);
if (!iRet) {
return 0;
} else {
return 1;
}
} else {
snprintf(pBueffel,511, "ERROR: %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];
/*
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) {
snprintf(pBueffel,131,
"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;
char *error = NULL;
assert(pCon);
assert(pSics);
tcl_interp = InterpGetTcl(pSics);
/* check Status */
eOld = GetStatus();
argtolower(argc, argv);
/* check no of args */
if (argc < 3) {
snprintf(pBueffel, 511,"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;
}
/* interpret arguments as pairs (name, value) and try to start */
for (i = 1; i < argc; i += 2) {
if (argv[i + 1] == NULL) {
snprintf(pBueffel, 511, "ERROR: no value found for driving %s", argv[i]);
SCWrite(pCon, pBueffel, eError);
return 0;
}
iRet = Tcl_GetDouble(tcl_interp, argv[i + 1], &dTarget);
if (iRet == TCL_ERROR) {
SCWrite(pCon, Tcl_GetStringResult(tcl_interp), eError);
return 0;
} else if (!isfinite(dTarget)) {
snprintf(pBueffel, 511,
"ERROR: target %s value for %s is not a finite number",
argv[i + 1], argv[i]);
SCWrite(pCon, pBueffel, eError);
StopExe(GetExecutor(), "ALL");
return 0;
}
iRet = Start2Run(pCon, pSics, argv[i], RUNDRIVE, dTarget);
if (!iRet) {
error = Tcl_GetStringResult(tcl_interp);
if(error != NULL) {
snprintf(pBueffel, 511,"%s", error);
} else {
snprintf(pBueffel, 511,"ERROR: cannot run %s to %s", argv[i],
argv[i + 1]);
}
SCWrite(pCon, pBueffel, eError);
StopExe(GetExecutor(), "ALL");
return 0;
}
}
/* wait for completion */
iRet = Wait4Success(GetExecutor());
/* print positions */
for (i = 1; i < argc; i += 2) {
snprintf(pBueffel,511, "New %s position: %.6g", 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());
return 0;
} else if (iRet == DEVINT) {
sprintf(pBueffel, "ERROR: Driving Interrupted!");
SCWrite(pCon, pBueffel, eError);
ClearExecutor(GetExecutor());
return 0;
}
SCWrite(pCon, "Driving finished successfully", eValue);
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) {
snprintf(pBueffel,511, "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;
}
/* interpret arguments as pairs name value and try to start */
for (i = 1; i < argc; i += 2) {
if (argv[i + 1] == NULL) {
snprintf(pBueffel,511, "ERROR: no value found for driving %s", argv[i]);
SCWrite(pCon, pBueffel, eError);
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");
return 0;
}
iRet = Start2Run(pCon, pSics, argv[i], RUNRUN, dTarget);
if (!iRet) {
snprintf(pBueffel, 511, "ERROR: cannot run %s to %s", argv[i],
argv[i + 1]);
SCWrite(pCon, pBueffel, eError);
StopExe(GetExecutor(), "ALL");
return 0;
}
}
return 1;
}
/*---------------------------------------------------------------------------*/
int MoveWrapper(SConnection * pCon, SicsInterp * pSics, void *pData,
int argc, char *argv[])
{
Tcl_Interp *tcl_interp;
int iRet, i;
double dTarget;
float curPos;
char pBueffel[512];
Status eOld;
long groupID, taskID;
void *obj;
assert(pCon);
assert(pSics);
tcl_interp = InterpGetTcl(pSics);
/* check Status */
eOld = GetStatus();
/* check no of args */
if (argc < 3) {
snprintf(pBueffel,511, "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 mv command",
eError);
return 0;
}
groupID = GetTaskGroupID(pServ->pTasker);
for (i = 1; i < argc; i += 2) {
if (argv[i + 1] == NULL) {
snprintf(pBueffel,511, "ERROR: no value found for driving %s", argv[i]);
SCWrite(pCon, pBueffel, eError);
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");
return 0;
}
obj = FindCommandData(pSics,argv[i],NULL);
if(obj == NULL || GetDrivableInterface(obj) == NULL){
SCPrintf(pCon,eError, "ERROR: %s not found, not started",argv[i]);
break;
}
GetDrivablePosition(obj,pCon,&curPos);
dTarget = curPos + dTarget;
taskID = StartDriveTask(obj, pCon, argv[i], (float)dTarget);
AddTaskToGroup(pServ->pTasker,taskID,groupID);
}
while(isTaskGroupRunning(pServ->pTasker,groupID)){
TaskYield(pServ->pTasker);
}
SCSendOK(pCon);
return 0;
}
/*---------------------------------------------------------------------------*/
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;
}
if (argc > 3) {
iRet = AddCommand(pSics, argv[3], MoveWrapper, NULL, NULL);
} else {
iRet = AddCommand(pSics, "mv", MoveWrapper, NULL, NULL);
}
if (!iRet) {
sprintf(pBueffel, "ERROR: duplicate command mv not created");
SCWrite(pCon, pBueffel, eError);
return 0;
}
return 1;
}
Reference in New Issue View Git Blame Copy Permalink