299 lines
8.4 KiB
C
299 lines
8.4 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);
|
|
strlcpy(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;
|
|
}
|