314 lines
9.1 KiB
C
314 lines
9.1 KiB
C
/*--------------------------------------------------------------------------
|
|
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
|
|
|
|
This version has been modified for use with the new high performance
|
|
motor drivers.
|
|
|
|
Mark Koennecke, August 2003
|
|
|
|
Modified to do all the switching and reference running in Tcl
|
|
|
|
Mark Koennecke, August 2004
|
|
--------------------------------------------------------------------------*/
|
|
#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 <modriv.h>
|
|
#include <rs232controller.h>
|
|
#include "swmotor.h"
|
|
#include "swmotor.i"
|
|
|
|
typedef struct __MoDriv {
|
|
/* general motor driver interface
|
|
fields. REQUIRED!
|
|
*/
|
|
float fUpper; /* upper limit */
|
|
float fLower; /* lower limit */
|
|
char *name;
|
|
int (*GetPosition)(void *self,float *fPos);
|
|
int (*RunTo)(void *self, float fNewVal);
|
|
int (*GetStatus)(void *self);
|
|
void (*GetError)(void *self, int *iCode,
|
|
char *buffer, int iBufLen);
|
|
int (*TryAndFixIt)(void *self,int iError,
|
|
float fNew);
|
|
int (*Halt)(void *self);
|
|
int (*GetDriverPar)(void *self, char *name,
|
|
float *value);
|
|
int (*SetDriverPar)(void *self,SConnection *pCon,
|
|
char *name, float newValue);
|
|
void (*ListDriverPar)(void *self, char *motorName,
|
|
SConnection *pCon);
|
|
void (*KillPrivate)(void *self);
|
|
|
|
|
|
/* EL-734 specific fields */
|
|
prs232 controller;
|
|
int iMotor;
|
|
float lastValue;
|
|
int errorCode;
|
|
int oredMsr;
|
|
} EL734Driv, *pEL734Driv;
|
|
/*========================================================================
|
|
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 active, 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;
|
|
|
|
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:
|
|
MakeSWHPMotor master switchFunc slave1 slave2 slave3
|
|
-----------------------------------------------------------------------*/
|
|
int MakeSWHPMotor(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 of 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;
|
|
}
|
|
|
|
|