- Rearranged directory structure for forking out ANSTO
- Refactored site specific stuff into a site module - PSI specific stuff is now in the PSI directory. - The old version has been tagged with pre-ansto
This commit is contained in:
cvs
547
swmotor.c
Normal file
547
swmotor.c
Normal file
@ -0,0 +1,547 @@
|
||||
/*--------------------------------------------------------------------------
|
||||
TOPSI switched motors implementation module. Three motors share a common
|
||||
EL734 motor. The actual motor is selected through a SPS. More information in
|
||||
file switchedmotor.tex or with Jochen Stahn.
|
||||
|
||||
copyright: see file copyright.h
|
||||
|
||||
Mark Koennecke, May 2001
|
||||
--------------------------------------------------------------------------*/
|
||||
#include <stdlib.h>
|
||||
#include <assert.h>
|
||||
#include <stdio.h>
|
||||
#include <tcl.h>
|
||||
#include "fortify.h"
|
||||
#include "sics.h"
|
||||
#include "SCinter.h"
|
||||
#include "splitter.h"
|
||||
#include "hardsup/sinq_prototypes.h"
|
||||
#include "hardsup/rs232c_def.h"
|
||||
#include "hardsup/el734_def.h"
|
||||
#include "hardsup/el734fix.h"
|
||||
#include "modriv.h"
|
||||
#include "swmotor.h"
|
||||
#include "swmotor.i"
|
||||
|
||||
/*========================================================================
|
||||
We start of by implementing the interface functions for the various
|
||||
interfaces this module has to implement.
|
||||
==========================================================================*/
|
||||
static int SWHalt(void *pData)
|
||||
{
|
||||
pSWmot self = (pSWmot)pData;
|
||||
assert(self);
|
||||
return self->pMaster->pDrivInt->Halt(self->pMaster);
|
||||
}
|
||||
/*--------------------------------------------------------------------*/
|
||||
static int SWCheckLimits(void *pData, float fVal, char *error, int iErrLen)
|
||||
{
|
||||
pSWmot self = (pSWmot)pData;
|
||||
assert(self);
|
||||
return self->pMaster->pDrivInt->CheckLimits(self->pMaster,
|
||||
fVal,error,iErrLen);
|
||||
}
|
||||
/*--------------------------------------------------------------------*/
|
||||
static int SWCheckStatus(void *pData, SConnection *pCon)
|
||||
{
|
||||
pSWmot self = (pSWmot)pData;
|
||||
assert(self);
|
||||
return self->pMaster->pDrivInt->CheckStatus(self->pMaster,
|
||||
pCon);
|
||||
}
|
||||
/*---------------------------------------------------------------------*/
|
||||
static int SWSaveStatus(void *pData, char *name, FILE *fd)
|
||||
{
|
||||
pSWmot self = (pSWmot)pData;
|
||||
assert(self);
|
||||
|
||||
fprintf(fd,"%s savedValue = %f\n", name,
|
||||
self->positions[self->myNumber]);
|
||||
return 1;
|
||||
}
|
||||
/*-----------------------------------------------------------------------*/
|
||||
static void *SWGetInterface(void *pData, int ID)
|
||||
{
|
||||
pSWmot self = (pSWmot)pData;
|
||||
assert(self);
|
||||
|
||||
if(ID == DRIVEID)
|
||||
{
|
||||
return self->pDriv;
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
/*----------------------------------------------------------------------*/
|
||||
static float SWGetValue(void *pData, SConnection *pCon)
|
||||
{
|
||||
pSWmot self = (pSWmot)pData;
|
||||
float fVal;
|
||||
assert(self);
|
||||
|
||||
/*
|
||||
we are not selected: return stored data:
|
||||
*/
|
||||
if(self->myNumber != self->selectedMotor[0])
|
||||
{
|
||||
SCWrite(pCon,"WARNING: motor not activem returning stored value",
|
||||
eWarning);
|
||||
return self->positions[self->myNumber];
|
||||
}
|
||||
else
|
||||
{
|
||||
fVal = self->pMaster->pDrivInt->GetValue(self->pMaster, pCon);
|
||||
self->positions[self->myNumber] = fVal;
|
||||
return fVal;
|
||||
}
|
||||
}
|
||||
/*-----------------------------------------------------------------------*/
|
||||
static long SWSetValue(void *pData, SConnection *pCon, float fVal)
|
||||
{
|
||||
pSWmot self = (pSWmot)pData;
|
||||
char pCommand[256], pError[132];
|
||||
int status;
|
||||
EL734Driv *pElli;
|
||||
Tcl_Interp *pTcl;
|
||||
|
||||
assert(self);
|
||||
self->errCode = 1;
|
||||
|
||||
/*
|
||||
first case: we are already selected
|
||||
*/
|
||||
if(self->myNumber == self->selectedMotor[0])
|
||||
{
|
||||
return self->pMaster->pDrivInt->SetValue(self->pMaster,pCon,fVal);
|
||||
}
|
||||
else
|
||||
{
|
||||
/*
|
||||
second case: switch to the requested motor, do a reference run
|
||||
and then execute Set command
|
||||
*/
|
||||
sprintf(pCommand,"Switching to motor number %d", self->myNumber);
|
||||
SCWrite(pCon,pCommand,eWarning);
|
||||
sprintf(pCommand,"%s %d",self->switchFunc, self->myNumber);
|
||||
pTcl = (Tcl_Interp *)pServ->pSics->pTcl;
|
||||
status = Tcl_Eval(pTcl,pCommand);
|
||||
strncpy(pError,pTcl->result, 131);
|
||||
if(status != TCL_OK || strstr(pError,"OK") == NULL)
|
||||
{
|
||||
sprintf(pCommand,"ERROR: %s while switching motor", pError);
|
||||
SCWrite(pCon,pCommand, eError);
|
||||
self->errCode = -1001;
|
||||
return HWFault;
|
||||
}
|
||||
self->selectedMotor[0] = self->myNumber;
|
||||
|
||||
/*
|
||||
switch done! Start a reference run
|
||||
*/
|
||||
SicsWait(10);
|
||||
SCWrite(pCon,"Standby, starting reference run... ", eWarning);
|
||||
pElli = (EL734Driv *)self->pMaster->pDriver;
|
||||
sprintf(pCommand,"R %d\r",pElli->iMotor);
|
||||
status = EL734_SendCmnd(&(pElli->EL734struct),
|
||||
pCommand,
|
||||
pError, 131);
|
||||
if(status != 1)
|
||||
{
|
||||
sprintf(pCommand,"ERROR: %s while trying to start reference run",
|
||||
pError);
|
||||
SCWrite(pCon,pCommand,eError);
|
||||
self->errCode = -1002;
|
||||
return HWFault;
|
||||
}
|
||||
/*
|
||||
now loop forever until reference run is done. This is either when the
|
||||
motors stops being busy or when the user interrupts.
|
||||
*/
|
||||
sprintf(pCommand,"SS %d\r",pElli->iMotor);
|
||||
for( ; ;)
|
||||
{
|
||||
status = EL734_SendCmnd(&(pElli->EL734struct),
|
||||
pCommand,
|
||||
pError, 131);
|
||||
if(status != 1)
|
||||
{
|
||||
sprintf(pCommand,"ERROR: %s during reference run",
|
||||
pError);
|
||||
SCWrite(pCon,pCommand,eError);
|
||||
self->errCode = -1003;
|
||||
return HWFault;
|
||||
}
|
||||
if(strstr(pError,"?BSY") == NULL)
|
||||
break;
|
||||
if(SCGetInterrupt(pCon) != eContinue)
|
||||
{
|
||||
self->errCode = -1004;
|
||||
SCWrite(pCon,"ERROR: user interrupted reference run",eError);
|
||||
return HWFault;
|
||||
}
|
||||
SicsWait(2);
|
||||
}
|
||||
|
||||
/*
|
||||
now this is finished. We can really start driving the motor
|
||||
*/
|
||||
SCWrite(pCon,"Reference run completed, starting to drive motor..",
|
||||
eWarning);
|
||||
return self->pMaster->pDrivInt->SetValue(self->pMaster,pCon,fVal);
|
||||
}
|
||||
}
|
||||
/*----------------------------------------------------------------------*/
|
||||
static void KillSWFull(void *pData)
|
||||
{
|
||||
pSWmot self = (pSWmot)pData;
|
||||
if(self == NULL)
|
||||
return;
|
||||
|
||||
if(self->pDriv)
|
||||
free(self->pDriv);
|
||||
if(self->pDes)
|
||||
DeleteDescriptor(self->pDes);
|
||||
if(self->selectedMotor)
|
||||
free(self->selectedMotor);
|
||||
if(self->switchFunc)
|
||||
free(self->switchFunc);
|
||||
free(self);
|
||||
}
|
||||
/*---------------------------------------------------------------------*/
|
||||
static void KillSWHalf(void *pData)
|
||||
{
|
||||
pSWmot self = (pSWmot)pData;
|
||||
if(self)
|
||||
free(self);
|
||||
}
|
||||
/*----------------------------------------------------------------------
|
||||
Alright, now the interpreter functions follow
|
||||
Usage:
|
||||
MakeSWMotor master switchFunc slave1 slave2 slave3
|
||||
-----------------------------------------------------------------------*/
|
||||
int MakeSWMotor(SConnection *pCon, SicsInterp *pSics, void *pData,
|
||||
int argc, char *argv[])
|
||||
{
|
||||
pSWmot sw1, sw2, sw3;
|
||||
char pBueffel[256];
|
||||
int i, status;
|
||||
|
||||
/*
|
||||
check that we have enough arguments
|
||||
*/
|
||||
if(argc < 6)
|
||||
{
|
||||
SCWrite(pCon,"ERROR: insufficient number of arguments to MakeSWMotor",
|
||||
eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*
|
||||
allocate a new data structure
|
||||
*/
|
||||
sw1 = (pSWmot)malloc(sizeof(SWmot));
|
||||
if(sw1 == NULL)
|
||||
{
|
||||
SCWrite(pCon,"ERROR: out of memory in MakeSWMotor",eError);
|
||||
return 0;
|
||||
}
|
||||
memset(sw1,0,sizeof(SWmot));
|
||||
|
||||
/*
|
||||
fill it up with stuff
|
||||
*/
|
||||
sw1->pDes = CreateDescriptor("Sparbroetchen");
|
||||
sw1->pDriv = CreateDrivableInterface();
|
||||
sw1->selectedMotor = (int *)malloc(sizeof(int));
|
||||
if(!sw1->pDes || ! sw1->pDriv || !sw1->selectedMotor)
|
||||
{
|
||||
SCWrite(pCon,"ERROR: out of memory in MakeSWMotor",eError);
|
||||
return 0;
|
||||
}
|
||||
sw1->selectedMotor[0] = -1;
|
||||
sw1->pMaster = FindMotor(pSics,argv[1]);
|
||||
if(!sw1->pMaster)
|
||||
{
|
||||
sprintf(pBueffel,"ERROR: cannot find master motor %s", argv[1]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
sw1->switchFunc = strdup(argv[2]);
|
||||
for(i = 0; i < 3; i++)
|
||||
{
|
||||
strcpy(sw1->slaves[i],argv[3+i]);
|
||||
}
|
||||
|
||||
/*
|
||||
initialize function pointers
|
||||
*/
|
||||
sw1->pDes->SaveStatus = SWSaveStatus;
|
||||
sw1->pDes->GetInterface = SWGetInterface;
|
||||
sw1->pDriv->GetValue = SWGetValue;
|
||||
sw1->pDriv->SetValue = SWSetValue;
|
||||
sw1->pDriv->Halt = SWHalt;
|
||||
sw1->pDriv->CheckStatus = SWCheckStatus;
|
||||
sw1->pDriv->CheckLimits = SWCheckLimits;
|
||||
|
||||
/*
|
||||
create clones of the new data structure ofr the other slaves
|
||||
*/
|
||||
sw2 = (pSWmot)malloc(sizeof(SWmot));
|
||||
sw3 = (pSWmot)malloc(sizeof(SWmot));
|
||||
if(!sw2 || !sw2)
|
||||
{
|
||||
SCWrite(pCon,"ERROR: out of memory in MakeSWMotor",eError);
|
||||
return 0;
|
||||
}
|
||||
memcpy(sw2,sw1,sizeof(SWmot));
|
||||
sw2->myNumber = 1;
|
||||
memcpy(sw3,sw1,sizeof(SWmot));
|
||||
sw3->myNumber = 2;
|
||||
|
||||
/*
|
||||
install commands
|
||||
*/
|
||||
status = AddCommand(pSics, argv[3], SWMotorAction, KillSWFull, sw1);
|
||||
if(!status)
|
||||
{
|
||||
sprintf(pBueffel,"ERROR: command %s already exists!",argv[3]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
status = AddCommand(pSics, argv[4], SWMotorAction, KillSWHalf, sw2);
|
||||
if(!status)
|
||||
{
|
||||
sprintf(pBueffel,"ERROR: command %s already exists!",argv[4]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
status = AddCommand(pSics, argv[5], SWMotorAction, KillSWHalf, sw3);
|
||||
if(!status)
|
||||
{
|
||||
sprintf(pBueffel,"ERROR: command %s already exists!",argv[3]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
return 1;
|
||||
}
|
||||
/*-------------------------------------------------------------------
|
||||
some prototypes from functions implemented in motor.c
|
||||
--------------------------------------------------------------------*/
|
||||
void MotorListLL(pMotor self, SConnection *pCon);
|
||||
void MotorReset(pMotor pM);
|
||||
/*---------------------------------------------------------------------*/
|
||||
int SWMotorAction(SConnection *pCon, SicsInterp *pSics, void *pData,
|
||||
int argc, char *argv[])
|
||||
{
|
||||
char pBueffel[512];
|
||||
TokenList *pList = NULL;
|
||||
TokenList *pCurrent;
|
||||
TokenList *pName;
|
||||
int iRet;
|
||||
pSWmot self = NULL;
|
||||
float fValue;
|
||||
long lID;
|
||||
|
||||
assert(pCon);
|
||||
assert(pSics);
|
||||
assert(pData);
|
||||
|
||||
self = (pSWmot)pData;
|
||||
|
||||
/* create Tokenlist */
|
||||
argtolower(argc,argv);
|
||||
pList = SplitArguments(argc,argv);
|
||||
if(!pList)
|
||||
{
|
||||
SCWrite(pCon,"Error parsing argument list in SWMotorAction",eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* first argument can be one of list, reset or parameter name */
|
||||
pCurrent = pList->pNext;
|
||||
if(!pCurrent) /* no argument, print value */
|
||||
{
|
||||
fValue = self->pDriv->GetValue(self,pCon);
|
||||
if(fValue < -990.)
|
||||
{
|
||||
sprintf(pBueffel,"Error obtaining position for %s",argv[0]);
|
||||
SCWrite(pCon,pBueffel,eValue);
|
||||
DeleteTokenList(pList);
|
||||
return 0;
|
||||
}
|
||||
sprintf(pBueffel,"%s = %f",argv[0],fValue);
|
||||
SCWrite(pCon,pBueffel,eValue);
|
||||
DeleteTokenList(pList);
|
||||
return 1;
|
||||
}
|
||||
|
||||
/* check for list */
|
||||
if(strcmp(pCurrent->text,"list") == 0)
|
||||
{
|
||||
MotorListLL(self->pMaster,pCon);
|
||||
DeleteTokenList(pList);
|
||||
return 1;
|
||||
} /* check for reset */
|
||||
else if(strcmp(pCurrent->text,"reset") == 0)
|
||||
{
|
||||
if(!SCMatchRights(pCon,usUser))
|
||||
{
|
||||
sprintf(pBueffel,"Insufficient privilege to reset %s",
|
||||
argv[0]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
DeleteTokenList(pList);
|
||||
return 0;
|
||||
}
|
||||
MotorReset(self->pMaster);
|
||||
DeleteTokenList(pList);
|
||||
SCSendOK(pCon);
|
||||
return 1;
|
||||
}
|
||||
else if(strcmp(pCurrent->text,"savedValue") == 0)
|
||||
{
|
||||
pCurrent = pCurrent->pNext;
|
||||
if(!pCurrent)
|
||||
{
|
||||
/*
|
||||
print Value
|
||||
*/
|
||||
sprintf(pBueffel,"%s.savedValue = %f", argv[0],
|
||||
self->positions[self->myNumber]);
|
||||
SCWrite(pCon,pBueffel,eValue);
|
||||
DeleteTokenList(pList);
|
||||
return 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
/*
|
||||
set saveValue, must be a numeric parameter
|
||||
*/
|
||||
if(pCurrent->Type == eInt)
|
||||
{
|
||||
fValue = (float)pCurrent->iVal;
|
||||
}
|
||||
else if(pCurrent->Type == eFloat)
|
||||
{
|
||||
fValue = pCurrent->fVal;
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf(pBueffel,"Wrong argument type for %s %s ",
|
||||
argv[0],"saveValue");
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
DeleteTokenList(pList);
|
||||
return 0;
|
||||
}
|
||||
self->positions[self->myNumber] = fValue;
|
||||
SCSendOK(pCon);
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
else if(strcmp(pCurrent->text,"selected") == 0)
|
||||
{
|
||||
pCurrent = pCurrent->pNext;
|
||||
if(!pCurrent)
|
||||
{
|
||||
/*
|
||||
print Value
|
||||
*/
|
||||
sprintf(pBueffel,"%s.selected = %d", argv[0],
|
||||
self->selectedMotor[0]);
|
||||
SCWrite(pCon,pBueffel,eValue);
|
||||
DeleteTokenList(pList);
|
||||
return 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
/*
|
||||
set selected, must be a numeric parameter
|
||||
*/
|
||||
if(pCurrent->Type == eInt)
|
||||
{
|
||||
fValue = (float)pCurrent->iVal;
|
||||
}
|
||||
else if(pCurrent->Type == eFloat)
|
||||
{
|
||||
fValue = pCurrent->fVal;
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf(pBueffel,"Wrong argument type for %s %s ",
|
||||
argv[0],"selected");
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
DeleteTokenList(pList);
|
||||
return 0;
|
||||
}
|
||||
self->selectedMotor[0] = (int)fValue;
|
||||
SCSendOK(pCon);
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
else /* one of the parameter commands or error left now */
|
||||
{
|
||||
pName = pCurrent;
|
||||
pCurrent = pCurrent->pNext;
|
||||
if(!pCurrent) /* no third par: print val */
|
||||
{
|
||||
/* deal with position first */
|
||||
if(strcmp(pName->text,"position") == 0)
|
||||
{
|
||||
fValue = self->pDriv->GetValue(self,pCon);
|
||||
if(fValue < 999.)
|
||||
{
|
||||
sprintf(pBueffel,"Error obtaining position for %s",argv[0]);
|
||||
SCWrite(pCon,pBueffel,eValue);
|
||||
DeleteTokenList(pList);
|
||||
return 0;
|
||||
}
|
||||
sprintf(pBueffel,"%s.SoftPosition = %f",argv[0],fValue);
|
||||
SCWrite(pCon,pBueffel,eValue);
|
||||
DeleteTokenList(pList);
|
||||
return 1;
|
||||
}
|
||||
iRet = MotorGetPar(self->pMaster,pName->text,&fValue);
|
||||
if(!iRet)
|
||||
{
|
||||
sprintf(pBueffel,"Parameter %s not found ",pName->text);
|
||||
SCWrite(pCon,pBueffel,eValue);
|
||||
DeleteTokenList(pList);
|
||||
return 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf(pBueffel, " %s.%s = %f",argv[0],pName->text,fValue);
|
||||
SCWrite(pCon,pBueffel,eValue);
|
||||
DeleteTokenList(pList);
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
else /* set */
|
||||
{ /* set command */
|
||||
/* parameter must be numerical value */
|
||||
if(pCurrent->Type == eInt)
|
||||
{
|
||||
fValue = (float)pCurrent->iVal;
|
||||
}
|
||||
else if(pCurrent->Type == eFloat)
|
||||
{
|
||||
fValue = pCurrent->fVal;
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf(pBueffel,"Wrong argument type for %s %s set",
|
||||
argv[0],pName->text);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
DeleteTokenList(pList);
|
||||
return 0;
|
||||
}
|
||||
iRet = MotorSetPar(self->pMaster,pCon,pName->text,fValue);
|
||||
DeleteTokenList(pList);
|
||||
if(iRet)
|
||||
SCSendOK(pCon);
|
||||
return iRet;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user