linse/sicspsi
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

- Created a modified swithc motor which takes into account the new motor drivers

Browse Source 
- Added the missing linux_def file into the repository
- Fixed a bug relating to scattering sense in selector.c
This commit is contained in:
cvs
2003-08-20 14:32:52 +00:00
parent a9d5607fea
commit 3f17e76df1
4 changed files with 367 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

361
swmotor2.c Normal file
View File

@ -0,0 +1,361 @@
/*--------------------------------------------------------------------------
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
--------------------------------------------------------------------------*/
#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 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 = transactRS232(pElli->controller,pCommand,strlen(pCommand),
pError,131);
if(status != 1)
{
getRS232Error(status,pError,131);
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 = transactRS232(pElli->controller,pCommand,strlen(pCommand),
pError,131);
if(status != 1)
{
getRS232Error(status,pError,131);
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:
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 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;
}