linse/sics
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
main
sics/motor.c
koennecke 0e2605b570 First commit towards replacing pCon-> with someFunc(pCon) 
This is accompanied with the removal of dead code in conman.c and else
2016-10-31 08:33:36 +01:00

1434 lines
42 KiB
C
Raw Permalink Blame History

/*------------------------------------------------------------------------
M O T O R S
This file implements the SICS motor handling. This is the logical
level, the nitty gritty hardware interface is in the driver.
Mark Koennecke, November 1996
revised: Mark Koennecke, June 1997
callback added: Mark Koennecke, August 1997
endscript facility added: Mark Koennecke, August 2002
Modified to support driver parameters, Mark Koennecke, January 2003
Reworked to allow for multiple implementations of motors on the same
interface in preparation for second generation motors.
Mark Koennecke, December 2008
Copyright:
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.
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 <tcl.h>
#include "fortify.h"
#include "sics.h"
#include "devexec.h"
#include "motor.h"
#include "splitter.h"
#include "status.h"
#include "tclmotdriv.h"
#include "site.h"
/*-------------------------------------------------------------------------
some lokal defines
*/
#define ZEROINACTIVE 0
#define INTCONT 0.
#define INTSCAN 1.
#define INTBATCH 2.
#define INTHALT 3.
#define SLOW 0
#define SUPP 1
#define SZERO 2
#define FIX 3
#define INT 4
#define PREC 5
#define USRIGHTS 6
#define SIGN 7
#define ECOUNT 8
#define POSCOUNT 9
#define IGNOREFAULT 10
#define MOVECOUNT 11
extern double DoubleTime(void);
/*-------------------------------------------------------------------------*/
static void SetMotorError(pMotor self, char *text)
{
if(self->error != NULL){
free(self->error);
}
self->error = strdup(text);
}
/*-------------------------------------------------------------------------*/
static void *MotorGetInterface(void *pData, int iID)
{
pMotor self = NULL;
self = (pMotor) pData;
assert(self);
if (iID == DRIVEID) {
return self->pDrivInt;
} else if (iID == CALLBACKINTERFACE) {
return self->pCall;
}
return NULL;
}
/*------------------------------------------------------------------------*/
static int MotorHalt(void *sulf)
{
pMotor self;
assert(sulf);
self = (pMotor) sulf;
/* reduce the error count by 1. This is because the driver is
expected to return an error when the motor had been stopped.
However, a stop is usually a consequence of a user intervention
or program logic. This prevents to a false motor alarm when the
motor was repeatedly stopped for other reasons.
*/
self->pDrivInt->iErrorCount--;
if (self->pDrivInt->iErrorCount < 0)
self->pDrivInt->iErrorCount = 0;
self->stopped = 1;
return self->pDriver->Halt((void *) self->pDriver);
}
/*---------------------------------------------------------------------------
MotorCheckBoundary checks for violation of boundary conditions and
transforms from SoftCoordinates to hard coordinates.
*/
static int MotorCheckBoundaryImpl(pMotor self, float fVal, float *fNew,
char *pError, int iErrLen)
{
float fHard;
float fZero;
float fLim;
assert(self);
/* check for fixed */
if (ObVal(self->ParArray, FIX) >= 0) {
snprintf(pError, iErrLen, "Motor %s is Fixed", self->name);
return 0; /* is this an error? */
}
/* check against software boundaries */
if (fVal > ObVal(self->ParArray, SUPP)) {
snprintf(pError, iErrLen, "%g violates upper software limit %g on %s",
fVal, ObVal(self->ParArray, SUPP), self->name);
return 0;
}
if (fVal < ObVal(self->ParArray, SLOW)) {
snprintf(pError, iErrLen, "%g violates lower software limit %g on %s",
fVal, ObVal(self->ParArray, SLOW), self->name);
return 0;
}
/* correct for zero point */
fZero = ObVal(self->ParArray, SZERO);
fZero = -fZero;
fHard = fVal - fZero;
/* apply sign */
fHard = fHard * ObVal(self->ParArray, SIGN);
/* check for hardware limits */
if (fHard > self->pDriver->fUpper) {
fLim = self->pDriver->fUpper * ObVal(self->ParArray,SIGN) + fZero;
snprintf(pError, iErrLen,
"%g violates upper hardware limit %g (%g) on %s",
fVal, fLim, self->pDriver->fUpper, self->name);
return 0;
}
if (fHard < self->pDriver->fLower) {
fLim = self->pDriver->fLower * ObVal(self->ParArray,SIGN) + fZero;
snprintf(pError, iErrLen,
"%g violates lower hardware limit %g (%g) on %s",
fVal, fLim, self->pDriver->fLower, self->name);
return 0;
}
*fNew = fHard;
return 1;
}
/*--------------------------------------------------------------------------*/
static int MotorLimits(void *sulf, float fVal, char *error, int iErrLen)
{
float fHard;
pMotor self;
assert(sulf);
self = (pMotor) sulf;
return MotorCheckBoundaryImpl(self, fVal, &fHard, error, iErrLen);
}
/* ------------------------------------------------------------------------ */
static int MotorGetSoftPositionImpl(pMotor self, SConnection * pCon,
float *fVal)
{
int iRet;
float fValue;
assert(self);
assert(pCon);
/* get the hard position */
iRet = MotorGetHardPosition(self, pCon, &fValue);
if (!iRet) {
*fVal = fValue;
return 0;
}
/* apply zeropoint */
if (ObVal(self->ParArray, SIGN) < 0.) {
fValue += ObVal(self->ParArray, SZERO);
} else {
fValue -= ObVal(self->ParArray, SZERO);
}
*fVal = fValue;
/* apply sign */
/* *fVal = MotorHardToSoftPosition(self,fValue); */
*fVal = fValue * ObVal(self->ParArray, SIGN);
return 1;
}
/*---------------------------------------------------------------------------*/
static float MotorGetValue(void *pData, SConnection * pCon)
{
int iRet;
float fVal = 0.;
assert(pData);
iRet = MotorGetSoftPositionImpl((pMotor) pData, pCon, &fVal);
if (iRet != OKOK) {
fVal = -9999999.99;
}
return fVal;
}
/*------------------------------------------------------------------------*/
static int MotorSaveStatus(void *pData, char *name, FILE * fd)
{
pMotor self = NULL;
char pBueffel[512];
assert(pData);
assert(fd);
self = (pMotor) pData;
fprintf(fd, "# Motor %s\n", name);
snprintf(pBueffel,sizeof(pBueffel)-1, "%s sign %f\n", name, ObVal(self->ParArray, SIGN));
fputs(pBueffel, fd);
snprintf(pBueffel,sizeof(pBueffel)-1, "%s SoftZero %f\n", name,
ObVal(self->ParArray, SZERO));
fputs(pBueffel, fd);
snprintf(pBueffel,sizeof(pBueffel)-1, "%s SoftLowerLim %f\n", name,
ObVal(self->ParArray, SLOW));
fputs(pBueffel, fd);
snprintf(pBueffel,sizeof(pBueffel)-1, "%s SoftUpperLim %f\n", name,
ObVal(self->ParArray, SUPP));
fputs(pBueffel, fd);
snprintf(pBueffel,sizeof(pBueffel)-1, "%s Fixed %f\n", name, ObVal(self->ParArray, FIX));
fputs(pBueffel, fd);
snprintf(pBueffel,sizeof(pBueffel)-1, "%s InterruptMode %f\n", name,
ObVal(self->ParArray, INT));
fputs(pBueffel, fd);
snprintf(pBueffel,sizeof(pBueffel)-1, "%s precision %f\n", name,
ObVal(self->ParArray, PREC));
fputs(pBueffel, fd);
snprintf(pBueffel,sizeof(pBueffel)-1, "%s ignorefault %f\n", name,
ObVal(self->ParArray, IGNOREFAULT));
fputs(pBueffel, fd);
snprintf(pBueffel,sizeof(pBueffel)-1, "%s AccessCode %f\n", name,
ObVal(self->ParArray, USRIGHTS));
fputs(pBueffel, fd);
snprintf(pBueffel,sizeof(pBueffel)-1, "%s failafter %f\n", name,
ObVal(self->ParArray, ECOUNT));
fputs(pBueffel, fd);
snprintf(pBueffel,sizeof(pBueffel)-1, "%s maxretry %f\n", name,
ObVal(self->ParArray, POSCOUNT));
fputs(pBueffel, fd);
snprintf(pBueffel,sizeof(pBueffel)-1, "%s movecount %f\n", name,
ObVal(self->ParArray, MOVECOUNT));
fputs(pBueffel, fd);
return 1;
}
/*-------------------------------------------------------------------------*/
static float absf(float f)
{
if (f < 0.) {
return -f;
} else {
return f;
}
}
/*-------------------------------------------------------------------------*/
static void MotorInterrupt(SConnection * pCon, int iVal)
{
if (SCGetInterrupt(pCon) < iVal) {
SCSetInterrupt(pCon, iVal);
}
}
/*---------------------------------------------------------------------*/
static int statusRunTo(pMotor self, SConnection * pCon)
{
char pBueffel[256];
if (self->retryCount >= ObVal(self->ParArray, POSCOUNT)) {
snprintf(pBueffel, 255, "ERROR: aborting motor %s after %d retries",
self->name, self->retryCount);
SCWrite(pCon, pBueffel, eLogError);
return HWFault;
}
if (SCGetInterrupt(pCon) != eContinue) {
return HWFault;
}
self->retryCount++;
snprintf(pBueffel, 255, "WARNING: restarting %s, %d time",
self->name, self->retryCount);
SCWrite(pCon, pBueffel, eLog);
self->pDriver->RunTo(self->pDriver, self->fTarget);
return HWBusy;
}
/*--------------------------------------------------------------------*/
static int checkPosition(pMotor self, SConnection * pCon)
{
float fHard;
char pBueffel[132];
int status;
MotorGetHardPosition(self, pCon, &fHard);
self->fPosition = fHard;
if (absf(fHard - self->fTarget) > ObVal(self->ParArray, PREC)) {
if (SCGetInterrupt(pCon) != eContinue) {
return HWFault;
}
if (self->stopped) {
snprintf(pBueffel, 131, "WARNING: %s stopped", self->name);
SCWrite(pCon, pBueffel, eWarning);
return HWFault;
}
snprintf(pBueffel, 131, "WARNING: %s off position by %f%s",
self->name, absf(fHard - self->fTarget),
self->fTarget > fHard ? "-" : "");
SCWrite(pCon, pBueffel, eLog);
status = statusRunTo(self, pCon);
return status;
}
return HWIdle;
}
/*--------------------------------------------------------------------*/
void finishDriving(pMotor self, SConnection * pCon)
{
MotCallback sCall;
MotorGetSoftPosition(self, pCon, &sCall.fVal);
sCall.pName = self->name;
self->fPosition = sCall.fVal;
self->fPosition = sCall.fVal;
if (self->moving) {
InvokeCallBack(self->pCall, MOTDRIVE, &sCall); /* send also very last position */
InvokeCallBack(self->pCall, MOTEND, &sCall);
tracePar(self->name,"%f",sCall.fVal);
}
self->moving = 0;
self->running = 0;
}
/*--------------------------------------------------------------------*/
static int reportAndFixError(pMotor self, SConnection * pCon)
{
char pBueffel[256], pError[131];
int iCode, iRet, newStatus;
self->pDriver->GetError(self->pDriver, &iCode, pError, 131);
iRet = self->pDriver->TryAndFixIt(self->pDriver, iCode, self->fTarget);
switch (iRet) {
case MOTFAIL:
snprintf(pBueffel, 255, "ERROR: %s on %s", pError, self->name);
SCWrite(pCon, pBueffel, eLogError);
newStatus = HWFault;
SetMotorError(self,pError);
break;
case MOTREDO:
snprintf(pBueffel, 255, "WARNING: %s on %s", pError, self->name);
SCWrite(pCon, pBueffel, eLog);
newStatus = statusRunTo(self, pCon);
break;
case MOTOK:
snprintf(pBueffel, 255, "WARNING: %s on %s", pError, self->name);
SCWrite(pCon, pBueffel, eLog);
newStatus = HWIdle;
break;
default:
SCWrite(pCon, "WARNING: bad status code in motor.c:reportAndFixError",
eWarning);
SCWrite(pCon, "You may continue, but show this to a SICS programmer",
eWarning);
newStatus = HWIdle;
break;
}
return newStatus;
}
/*---------------------------------------------------------------------
New version, refactored October 2003
-----------------------------------------------------------------------*/
static int evaluateStatus(pMotor self, SConnection * pCon)
{
int iRet, iCode, newStatus;
MotCallback sCall;
char pBueffel[256], pError[132];
float fHard;
iRet = self->pDriver->GetStatus(self->pDriver);
newStatus = iRet;
switch (iRet) {
case OKOK:
case HWIdle:
newStatus = checkPosition(self, pCon);
if (newStatus != HWBusy) {
finishDriving(self, pCon);
}
break;
case HWFault:
newStatus = reportAndFixError(self, pCon);
break;
case HWPosFault:
newStatus = reportAndFixError(self, pCon);
if (newStatus == HWFault && ObVal(self->ParArray, IGNOREFAULT) < 1) {
newStatus = HWPosFault;
}
if (newStatus == HWIdle || newStatus == OKOK) {
newStatus = checkPosition(self, pCon);
if (newStatus != HWBusy) {
finishDriving(self, pCon);
}
}
break;
case HWBusy:
newStatus = HWBusy;
break;
case HWWarn:
self->pDriver->GetError(self->pDriver, &iCode, pError, 131);
snprintf(pBueffel, 255, "WARNING: motor reported: %s", pError);
SCWrite(pCon, pBueffel, eWarning);
newStatus = HWIdle;
break;
default:
SCWrite(pCon, "WARNING: Bad status in motor.c:evaluatStatus",
eWarning);
SCWrite(pCon, "You may continue, but show this to a SICS programmer",
eWarning);
break;
}
if (newStatus == HWFault) {
finishDriving(self, pCon);
MotorInterrupt(pCon, ObVal(self->ParArray, INT));
self->retryCount = 0;
}
return newStatus;
}
/*---------------------------------------------------------------------*/
static void handleMoveCallback(pMotor self, SConnection * pCon)
{
MotCallback sCall;
double current_time, skip_time;
current_time = DoubleTime();
skip_time = 0.001 * ObVal(self->ParArray,MOVECOUNT);
if(self->last_report_time + skip_time <= current_time) {
MotorGetSoftPosition(self, pCon, &sCall.fVal);
sCall.pName = self->name;
InvokeCallBack(self->pCall, MOTDRIVE, &sCall);
tracePar(self->name,"%f",sCall.fVal);
self->last_report_time = current_time;
}
}
/*-----------------------------------------------------------------------*/
static int MotorStatus(void *sulf, SConnection * pCon)
{
pMotor self = NULL;
int status;
assert(sulf);
self = (pMotor) sulf;
if(self->running != 1){
return HWIdle;
}
status = evaluateStatus(self, pCon);
if (self->pDrivInt->drivableStatus != status) {
((SConnection *) pCon)->conEventType = STATUS;
((SConnection *) pCon)->conStatus = status;
SCWrite(pCon, "", eEvent);
self->pDrivInt->drivableStatus = status;
}
if (status == HWBusy) {
handleMoveCallback(self, pCon);
}
return status;
}
/*---------------------------------------------------------------------------*/
int MotorGetPar(pMotor self, char *name, float *fVal)
{
return self->MotorGetPar(self, name, fVal);
}
/*---------------------------------------------------------------------------*/
int MotorSetPar(pMotor self, SConnection * pCon, char *name, float fVal)
{
return self->MotorSetPar(self, pCon, name, fVal);
}
/*---------------------------------------------------------------------------*/
int MotorGetHardPosition(pMotor self, SConnection * pCon, float *fVal)
{
return self->MotorGetHardPosition(self, pCon, fVal);
}
/*--------------------------------------------------------------------------*/
static int MotorGetParImpl(pMotor self, char *name, float *fVal)
{
ObPar *pPar = NULL;
assert(self);
if (strcmp(name, "hardupperlim") == 0) {
*fVal = self->pDriver->fUpper;
return 1;
}
if (strcmp(name, "hardlowerlim") == 0) {
*fVal = self->pDriver->fLower;
return 1;
}
pPar = ObParFind(self->ParArray, name);
if (pPar) {
*fVal = pPar->fVal;
return 1;
} else {
/* can still be position */
if (strcmp(name, "position") == 0) {
*fVal = self->fPosition;
return 1;
} else if (strcmp(name, "target") == 0) {
*fVal = self->fTarget;
return 1;
} else {
/*
check for a driver parameter
*/
if (self->pDriver->GetDriverPar != NULL) {
return self->pDriver->GetDriverPar(self->pDriver, name, fVal);
} else {
return 0;
}
}
}
}
/*---------------------------------------------------------------------------*/
static int MotorSetParImpl(pMotor self, SConnection * pCon, char *name,
float fVal)
{
ObPar *pPar = NULL;
char pBueffel[512];
int iRet;
float fLimit, fOld, fChange;
assert(self);
assert(pCon);
/*
try set driver parameters
*/
if (self->pDriver->SetDriverPar != NULL) {
iRet = self->pDriver->SetDriverPar(self->pDriver, pCon, name, fVal);
if (iRet == 1) {
SCparChange(pCon);
InvokeCallBack(self->pCall, HDBVAL, self);
tracePar(self->name,"%s:%f",name, fVal);
return iRet;
}
}
if (strcmp(name, "softzero") == 0) {
/* set it first, this also tests the necessary privileges */
fOld = ObVal(self->ParArray, SZERO);
iRet = ObParSet(self->ParArray, self->name, name, fVal, pCon);
if (!iRet) {
return iRet;
}
/* shift the limits by the difference between old and new */
fChange = fVal - fOld;
/* upper limit */
fLimit = ObVal(self->ParArray, SUPP);
fLimit -= fChange;
ObParSet(self->ParArray, self->name, "softupperlim", fLimit, pCon);
/* lower limit */
fLimit = ObVal(self->ParArray, SLOW);
fLimit -= fChange;
ObParSet(self->ParArray, self->name, "softlowerlim", fLimit, pCon);
SCparChange(pCon);
InvokeCallBack(self->pCall, HDBVAL, self);
tracePar(self->name,"%s:%f",name, fVal);
return 1;
}
iRet = ObParSet(self->ParArray, self->name, name, fVal, pCon);
if (strcmp(name, "sign") == 0) {
if ((absf(fVal) - 1.0) > 0.01) {
SCWrite(pCon, "ERROR: Invalid Sign Value", eError);
return 0;
}
if (fVal < 0.0) {
ObParInit(self->ParArray, SLOW, "softlowerlim",
self->pDriver->fUpper * fVal, usUser);
ObParInit(self->ParArray, SUPP, "softupperlim",
self->pDriver->fLower * fVal, usUser);
ObParInit(self->ParArray, SZERO, "softzero", ZEROINACTIVE, usUser);
}
}
InvokeCallBack(self->pCall, HDBVAL, self);
SCparChange(pCon);
tracePar(self->name,"%s:%f",name, fVal);
return iRet;
}
/*------------------------------------------------------------------------*/
static int MotorGetHardPositionImpl(pMotor self, SConnection * pCon,
float *fHard)
{
int iRet, iCode;
float fVal;
char pBueffel[512], pError[256];
assert(self);
assert(pCon);
iRet = self->pDriver->GetPosition(self->pDriver, &fVal);
if (iRet == OKOK) { /* all went well, the exception */
*fHard = fVal;
return 1;
} else { /* a problem, the usual case: try fix the problem */
/* no point in trying this three times */
self->pDriver->GetError(self->pDriver, &iCode, pError, 255);
iRet = self->pDriver->TryAndFixIt(self->pDriver, iCode, fVal);
snprintf(pBueffel,sizeof(pBueffel)-1, "WARNING: Trying to fix %s", pError);
SCWrite(pCon, pBueffel, eWarning);
switch (iRet) {
case MOTFAIL:
snprintf(pBueffel,sizeof(pBueffel)-1, "ERROR: cannot fix motor %s", self->name);
SCWrite(pCon, pBueffel, eError);
SCSetInterrupt(pCon, (int) ObVal(self->ParArray, INT));
*fHard = fVal;
return 0;
case MOTOK:
case MOTREDO:
iRet = self->pDriver->GetPosition(self->pDriver, &fVal);
if (iRet) {
*fHard = fVal * ObVal(self->ParArray, SIGN);
return 1;
} else {
snprintf(pBueffel,sizeof(pBueffel)-1, "ERROR: cannot fix motor %s", self->name);
SCSetInterrupt(pCon, (int) ObVal(self->ParArray, INT));
SCWrite(pCon, pBueffel, eError);
*fHard = fVal;
return 0;
}
}
}
*fHard = fVal * ObVal(self->ParArray, SIGN);
return 0;
}
/*---------------------------------------------------------------------------*/
static long MotorRunImpl(void *sulf, SConnection * pCon, float fNew)
{
float fHard;
int i, iRet, iCode;
char pBueffel[512];
char pError[132];
pMotor self;
long lTime;
float fDelta;
self = (pMotor) (sulf);
assert(self);
assert(pCon);
/* check if I'am allowed to move this motor */
if (!SCMatchRights(pCon, (int) ObVal(self->ParArray, USRIGHTS))) {
snprintf(pBueffel,sizeof(pBueffel)-1, "ERROR: You are not authorised to move motor %s",
self->name);
SCWrite(pCon, pBueffel, eError);
SCSetInterrupt(pCon, eAbortBatch);
return 0;
}
SetMotorError(self,"None");
/* check boundaries first */
iRet = MotorCheckBoundaryImpl(self, fNew, &fHard, pError, 131);
if (!iRet) {
snprintf(pBueffel, 511, "ERROR: %s", pError);
SCWrite(pCon, pBueffel, eError);
SCSetInterrupt(pCon, eAbortOperation);
SetMotorError(self,pError);
return 0;
}
/* check if the bad motor flag is set */
if ((int) ObVal(self->ParArray, IGNOREFAULT) > 0) {
snprintf(pBueffel, 511, "WARNING: motor %s is unreliable", self->name);
SCWrite(pCon, pBueffel, eWarning);
self->pDrivInt->iErrorCount = 0;
}
/* check our error count and interrupt if to much */
iCode = (int) ObVal(self->ParArray, ECOUNT);
if (self->pDrivInt->iErrorCount > iCode) {
/* big alarm */
ServerWriteGlobal("ERROR: !!! MOTOR ALARM !!! MOTOR ALARM !!!",
eError);
snprintf(pBueffel,sizeof(pBueffel)-1,
"ERROR: too many position errors counted at motor %s",
self->name);
ServerWriteGlobal(pBueffel, eError);
SCSetInterrupt(pCon, eAbortBatch);
self->pDrivInt->iErrorCount = 0;
SetMotorError(self,"Motor ALARM!!!!");
return 0;
}
/* Boundaries OK, send command */
self->posFaultCount = 0;
self->retryCount = 0;
self->stopped = 0;
self->moving = 1;
self->fTarget = fHard;
InvokeCallBack(self->pCall, HDBVAL, self);
self->last_report_time = 0.0;
self->posCount = 0;
iRet = self->pDriver->RunTo(self->pDriver, fHard);
if (iRet != OKOK) { /* try three times to fix it */
for (i = 0; (i < 3) && (iRet != OKOK); i++) {
self->pDriver->GetError(self->pDriver, &iCode, pError, 131);
snprintf(pBueffel,sizeof(pBueffel)-1, "WARNING: Trying to fix: %s", pError);
SCWrite(pCon, pBueffel, eWarning);
iRet = self->pDriver->TryAndFixIt(self->pDriver, iCode, fHard);
switch (iRet) {
case MOTFAIL:
SCWrite(pCon, pError, eError);
SCWrite(pCon, "\n", eError);
SCSetInterrupt(pCon, (int) ObVal(self->ParArray, INT));
SetMotorError(self,pError);
return HWFault;
case MOTREDO:
iRet = self->pDriver->RunTo(self->pDriver, fHard);
if (iRet == OKOK) {
self->running = 1;
return OKOK;
}
break;
case MOTOK:
self->running = 1;
return OKOK;
break;
}
}
/* tried three times, refuses to work */
SCWrite(pCon, pError, eError);
SCWrite(pCon, "\n", eError);
SCSetInterrupt(pCon, (int) ObVal(self->ParArray, INT));
return HWFault;
}
self->running = 1;
return OKOK;
}
/*---------------------------------------------------------------------------*/
pMotor MotorInit(char *drivername, char *name, MotorDriver * pDriv)
{
pMotor pM = NULL;
assert(drivername);
assert(pDriv);
assert(name);
/* get memory */
pM = (pMotor) malloc(sizeof(Motor));
if (!pM) {
return NULL;
}
memset(pM,0,sizeof(Motor));
/* create and initialize parameters */
pM->ParArray = ObParCreate(MOTOBPARLENGTH);
if (!pM->ParArray) {
free(pM);
return NULL;
}
ObParInit(pM->ParArray, SLOW, "softlowerlim", pDriv->fLower, usUser);
ObParInit(pM->ParArray, SUPP, "softupperlim", pDriv->fUpper, usUser);
ObParInit(pM->ParArray, SZERO, "softzero", ZEROINACTIVE, usUser);
ObParInit(pM->ParArray, FIX, "fixed", -1, usUser);
ObParInit(pM->ParArray, INT, "interruptmode", INTCONT, usMugger);
ObParInit(pM->ParArray, PREC, "precision", 0.01, usMugger);
ObParInit(pM->ParArray, USRIGHTS, "accesscode", (float) usUser,
usMugger);
ObParInit(pM->ParArray, SIGN, "sign", 1.0, usMugger);
ObParInit(pM->ParArray, ECOUNT, "failafter", 3.0, usMugger);
ObParInit(pM->ParArray, POSCOUNT, "maxretry", 3.0, usMugger);
ObParInit(pM->ParArray, IGNOREFAULT, "ignorefault", 0.0, usMugger);
ObParInit(pM->ParArray, MOVECOUNT, "movecount", 500.0, usMugger);
pDriv->GetPosition(pDriv, &(pM->fPosition));
pM->fTarget = pM->fPosition;
pM->endScriptID = 0;
/* copy arguments */
pM->pDriver = pDriv;
pM->drivername = strdup(drivername);
pM->name = strdup(name);
pM->error = strdup("None");
/* initialise object descriptor */
pM->pDescriptor = CreateDescriptor("Motor");
if (!pM->pDescriptor) {
ObParDelete(pM->ParArray);
free(pM);
return NULL;
}
pM->pDescriptor->GetInterface = MotorGetInterface;
pM->pDescriptor->SaveStatus = MotorSaveStatus;
/* initialise Drivable interface */
pM->pDrivInt = CreateDrivableInterface();
if (!pM->pDrivInt) {
DeleteDescriptor(pM->pDescriptor);
ObParDelete(pM->ParArray);
free(pM);
return NULL;
}
pM->pDrivInt->SetValue = MotorRunImpl;
pM->pDrivInt->CheckLimits = MotorLimits;
pM->pDrivInt->CheckStatus = MotorStatus;
pM->pDrivInt->GetValue = MotorGetValue;
pM->pDrivInt->Halt = MotorHalt;
/* initialize function pointers */
pM->MotorGetPar = MotorGetParImpl;
pM->MotorSetPar = MotorSetParImpl;
pM->MotorGetHardPosition = MotorGetHardPositionImpl;
/* initialise callback interface */
pM->pCall = CreateCallBackInterface();
if (!pM->pCall) {
MotorKill(pM);
return NULL;
}
/* done */
return pM;
}
/*--------------------------------------------------------------------------*/
long MotorRun(void *data, SConnection * pCon, float fNew)
{
pMotor self = (pMotor) data;
return self->pDrivInt->SetValue(data, pCon, fNew);
}
/*--------------------------------------------------------------------------*/
int MotorCheckBoundary(pMotor self, float fVal, float *fHard,
char *error, int iErrLen)
{
return self->pDrivInt->CheckLimits(self, fVal, error, iErrLen);
}
/*--------------------------------------------------------------------------*/
int MotorGetSoftPosition(pMotor self, SConnection * pCon, float *fVal)
{
float myVal;
myVal = self->pDrivInt->GetValue(self, pCon);
*fVal = myVal;
if (myVal <= -9999999.99) {
return 0;
} else {
return 1;
}
}
/*--------------------------------------------------------------------------*/
extern void KillPiPiezo(void *pData);
void MotorKill(void *self)
{
pMotor pM;
assert(self);
pM = (pMotor) self;
/* MotorHalt(pM); */
if (pM->name)
free(pM->name);
if (pM->pDrivInt) {
free(pM->pDrivInt);
}
if (pM->pCall) {
DeleteCallBackInterface(pM->pCall);
}
/* kill driver */
if (pM->drivername) {
if (pM->pDriver->KillPrivate != NULL) {
pM->pDriver->KillPrivate(pM->pDriver);
if (pM->pDriver->name != NULL) {
free(pM->pDriver->name);
}
free(pM->pDriver);
}
free(pM->drivername);
}
/* get rid of parameter space */
if (pM->ParArray) {
ObParDelete(pM->ParArray);
}
/* kill Descriptor */
DeleteDescriptor(pM->pDescriptor);
free(pM);
}
/*------------------------------------------------------------------------*/
float MotorHardToSoftPosition(pMotor self, float fValue)
{
/* apply zeropoint */
if (ObVal(self->ParArray, SIGN) < 0.) {
fValue += ObVal(self->ParArray, SZERO);
} else {
fValue -= ObVal(self->ParArray, SZERO);
}
/* apply sign */
return fValue * ObVal(self->ParArray, SIGN);
}
/*---------------------------------------------------------------------------*/
int MotorCheckPosition(void *sulf, SConnection * pCon)
{
float fHard;
int i, iRet, iCode;
char pBueffel[512];
pMotor self;
self = (pMotor) sulf;
assert(self);
assert(pCon);
/* try to find current position */
iRet = MotorGetHardPosition(self, pCon, &fHard);
if (iRet) {
if (absf(fHard - self->fTarget) < ObVal(self->ParArray, PREC)) {
self->fPosition = fHard;
return 1;
} else {
/* Oooopppsss error */
return 0;
}
} else { /* error getting hold of position, MotorGetHard already tried to
solve the problem and FAILED, the fucking bastard
even messaged the connection accordingly */
return -1;
}
}
/* --------------------------------------------------------------------------
The Factory function for creating a motor. Usage:
MotorCreate name DriverName.
Drivernames currently understood are:
EL734 SINQ EL734 stepper motor
EL734DC SINQ EL734 DC motor
PiPiezo Physik-Instrumente E-255 Piezo Controller
SIM simulated motor
*/
extern MotorDriver *MakePiPiezo(Tcl_Interp * pTcl, char *pArray);
extern MotorDriver *epicsMakeMotorDriver(char *baseName);
int MotorCreate(SConnection * pCon, SicsInterp * pSics, void *pData,
int argc, char *argv[])
{
pMotor pNew = NULL;
MotorDriver *pDriver = NULL;
char pBueffel[512];
int iD, iRet;
Tcl_Interp *pTcl = (Tcl_Interp *) pSics->pTcl;
pSite site = NULL;
assert(pCon);
assert(pSics);
/* a first check */
if (argc < 3) {
SCWrite(pCon, "Insufficient arguments for motor creation", eLogError);
return 0;
}
/* create the driver */
strtolower(argv[2]);
strtolower(argv[1]);
if (strcmp(argv[2], "sim") == 0) {
iD = argc - 3;
pDriver = CreateSIM(pCon, iD, &argv[3]);
if (!pDriver) {
return 0;
}
/* create the motor */
pNew = MotorInit("SIM", argv[1], pDriver);
if (!pNew) {
snprintf(pBueffel,sizeof(pBueffel)-1, "Failure to create motor %s", argv[1]);
SCWrite(pCon, pBueffel, eLogError);
return 0;
}
} else if (strcmp(argv[2], "tclmot") == 0) {
pDriver = CreateTclMotDriv(pCon, argc, argv);
if (!pDriver) {
return 0;
}
/* create the motor */
pNew = MotorInit("TCLMOT", argv[1], pDriver);
if (!pNew) {
snprintf(pBueffel,sizeof(pBueffel)-1, "Failure to create motor %s", argv[1]);
SCWrite(pCon, pBueffel, eLogError);
return 0;
}
} else if (strcmp(argv[2], "regress") == 0) {
pDriver = RGMakeMotorDriver();
if (!pDriver) {
return 0;
}
/* create the motor */
pNew = MotorInit("regress", argv[1], pDriver);
if (!pNew) {
snprintf(pBueffel,sizeof(pBueffel)-1, "Failure to create motor %s", argv[1]);
SCWrite(pCon, pBueffel, eLogError);
return 0;
}
} else {
site = getSite();
if (site != NULL) {
pNew = site->CreateMotor(pCon, argc - 1, &argv[1]);
}
if (pNew == NULL) {
snprintf(pBueffel,sizeof(pBueffel)-1, "Motor Type %s not recognized for motor %s",
argv[2], argv[1]);
SCWrite(pCon, pBueffel, eLogError);
return 0;
}
}
/* create the interpreter command */
iRet = AddCommand(pSics, argv[1], MotorAction, MotorKill, pNew);
if (!iRet) {
snprintf(pBueffel,sizeof(pBueffel)-1, "ERROR: duplicate command %s not created", argv[1]);
SCWrite(pCon, pBueffel, eError);
return 0;
}
return 1;
}
/* ----------------- some private functions used in MotorAction -------------*/
void MotorListLL(pMotor self, SConnection * pCon)
{
char pBueffel[512];
int i, iLen;
iLen = ObParLength(self->ParArray);
snprintf(pBueffel,sizeof(pBueffel)-1, "Parameter Listing for motor %s", self->name);
SCWrite(pCon, pBueffel, eValue);
snprintf(pBueffel,sizeof(pBueffel)-1, "%s.Position = %f\n", self->name, self->fPosition);
SCWrite(pCon, pBueffel, eValue);
snprintf(pBueffel,sizeof(pBueffel)-1, "%s.TargetPosition = %f\n", self->name, self->fTarget);
SCWrite(pCon, pBueffel, eValue);
snprintf(pBueffel, 511, "%s.hardlowerlim = %f", self->name,
self->pDriver->fLower);
SCWrite(pCon, pBueffel, eValue);
snprintf(pBueffel, 511, "%s.hardupperlim = %f", self->name,
self->pDriver->fUpper);
SCWrite(pCon, pBueffel, eValue);
for (i = 0; i < iLen; i++) {
snprintf(pBueffel,sizeof(pBueffel)-1, "%s.%s = %f\n", self->name,
self->ParArray[i].name, self->ParArray[i].fVal);
SCWrite(pCon, pBueffel, eValue);
}
/*
list driver parameters when appropriate
*/
if (self->pDriver->ListDriverPar != NULL) {
self->pDriver->ListDriverPar(self->pDriver, self->name, pCon);
}
}
/*--------------------------------------------------------------------------*/
void MotorReset(pMotor pM)
{
ObParInit(pM->ParArray, SLOW, "softlowerlim", pM->pDriver->fLower,
usUser);
ObParInit(pM->ParArray, SUPP, "softupperlim", pM->pDriver->fUpper,
usUser);
ObParInit(pM->ParArray, SZERO, "softzero", ZEROINACTIVE, usUser);
ObParInit(pM->ParArray, FIX, "fixed", -1, usUser);
pM->running = 0;
}
/*------------------------------------------------------------------------*/
typedef struct {
char *pName;
SConnection *pCon;
float lastValue;
} MotInfo, *pMotInfo;
/*-----------------------------------------------------------------------*/
static void KillInfo(void *pData)
{
pMotInfo self = NULL;
assert(pData);
self = (pMotInfo) pData;
if (self->pName) {
free(self->pName);
}
if (self->pCon != NULL) {
SCDeleteConnection(self->pCon);
}
free(self);
}
/*------------------- The CallBack function for interest ------------------*/
static int InterestCallback(int iEvent, void *pEvent, void *pUser)
{
pMotInfo pInfo = NULL;
char pBueffel[80];
MotCallback *psCall;
assert(pEvent);
assert(pUser);
psCall = (MotCallback *) pEvent;
pInfo = (MotInfo *) pUser;
if (!SCisConnected(pInfo->pCon)) {
return -1;
}
if (pInfo->lastValue != psCall->fVal) {
pInfo->lastValue = psCall->fVal;
(pInfo->pCon)->conEventType = POSITION;
snprintf(pBueffel,sizeof(pBueffel)-1, "%s.position = %f ", pInfo->pName, pInfo->lastValue);
SCWrite(pInfo->pCon, pBueffel, eEvent);
}
return 1;
}
/*------------------------------------------------------------------------*/
static void KillScript(void *pData)
{
if (pData != NULL) {
free(pData);
}
}
/*------------------------ The endscript callback function ----------------*/
static int EndScriptCallback(int iEvent, void *pEvent, void *pUser)
{
char *pScript = NULL;
MotCallback *psCall;
char pCommand[1024];
int iRet;
assert(pEvent);
assert(pUser);
psCall = (MotCallback *) pEvent;
pScript = (char *) pUser;
snprintf(pCommand,sizeof(pCommand)-1, "%s %f", pScript, psCall->fVal);
iRet = Tcl_Eval(pServ->pSics->pTcl, pCommand);
return iRet;
}
/*
* Context test function for callback removal
*/
int CheckMotiMatch(const void* context, const void* pUserData)
{
pMotInfo pMoti = (pMotInfo) pUserData;
SConnection *pCon = (SConnection*) context;
if (VerifyConnection(pCon) && VerifyConnection(pMoti->pCon)) {
if (SCGetIdent(pMoti->pCon) == SCGetIdent(pCon))
return 0;
}
return 1;
}
/*----------------------------------------------------------------------------
The wrapper function for a motor. Commands currently supported are:
motorname parametername newval : sets a parameter
motorname parametername : gets the value for a par
motorname list : lists all parameters
motorname reset : puts softlimits to default
motorname interest : starts sending automatic
notifications when driving
motorname endscript : script to call when motor
finishes driving
-----------------------------------------------------------------------------*/
int MotorAction(SConnection * pCon, SicsInterp * pSics, void *pData,
int argc, char *argv[])
{
char pBueffel[512];
TokenList *pList = NULL;
TokenList *pCurrent;
TokenList *pName;
int iRet;
pMotor self;
float fValue;
long lID;
pMotInfo pMoti = NULL;
assert(pCon);
assert(pSics);
assert(pData);
self = (pMotor) pData;
/* create Tokenlist */
argtolower(argc, argv);
pList = SplitArguments(argc, argv);
if (!pList) {
SCWrite(pCon, "Error parsing argument list in MotorAction", eError);
return 0;
}
/* first argument can be one of list, reset or parameter name */
pCurrent = pList->pNext;
if (!pCurrent) { /* no argument, print value */
iRet = MotorGetSoftPosition(self, pCon, &fValue);
if (!iRet) {
snprintf(pBueffel,sizeof(pBueffel)-1, "Error obtaining position for %s", argv[0]);
SCWrite(pCon, pBueffel, eError);
DeleteTokenList(pList);
return 0;
}
snprintf(pBueffel,sizeof(pBueffel)-1, "%s = %f", argv[0], fValue);
SCWrite(pCon, pBueffel, eValue);
DeleteTokenList(pList);
return 1;
}
/* check for list */
if (strcmp(pCurrent->text, "list") == 0) {
MotorListLL(self, pCon);
DeleteTokenList(pList);
return 1;
} /* check for reset */
else if (strcmp(pCurrent->text, "reset") == 0) {
if (!SCMatchRights(pCon, usUser)) {
snprintf(pBueffel,sizeof(pBueffel)-1, "Insufficient privilege to reset %s", argv[0]);
SCWrite(pCon, pBueffel, eError);
DeleteTokenList(pList);
return 0;
}
MotorReset(self);
DeleteTokenList(pList);
SCSendOK(pCon);
return 1;
} /* check for error */
else if (strcmp(pCurrent->text, "error") == 0) {
SCPrintf(pCon,eValue,"%s.error = %s", argv[0], self->error);
return 1;
} else if (strcmp(pCurrent->text, "interest") == 0) { /* interest */
pMoti = (pMotInfo) malloc(sizeof(MotInfo));
if (!pMoti) {
SCWrite(pCon, "ERROR: out of memory in motor.c", eError);
return 0;
}
pMoti->pName = strdup(argv[0]);
pMoti->pCon = SCCopyConnection(pCon);
iRet = MotorGetSoftPosition(self, pCon, &fValue);
if (!iRet) {
snprintf(pBueffel,sizeof(pBueffel)-1,
"Failed to register interest, Reason:Error obtaining current position for %s",
argv[0]);
SCWrite(pCon, pBueffel, eError);
DeleteTokenList(pList);
return 0;
}
pMoti->lastValue = fValue;
RemoveCallbackUsr(self->pCall, InterestCallback, CheckMotiMatch, pCon); /* only this one */
lID = RegisterCallback(self->pCall, MOTDRIVE, InterestCallback,
pMoti, KillInfo);
DeleteTokenList(pList);
SCSendOK(pCon);
return 1;
} else if (strcmp(pCurrent->text, "uninterest") == 0) {
RemoveCallbackUsr(self->pCall, InterestCallback, CheckMotiMatch, pCon); /* only this one */
SCSendOK(pCon);
DeleteTokenList(pList);
return 1;
} else if (strcmp(pCurrent->text, "endscript") == 0) { /* endscript */
if (!SCMatchRights(pCon, usMugger)) {
return 0;
}
if (self->endScriptID != 0) {
RemoveCallback(self->pCall, self->endScriptID);
self->endScriptID = 0;
}
pCurrent = pCurrent->pNext;
if (!pCurrent) {
SCWrite(pCon, "ERROR: scriptname argument missing", eError);
return 0;
}
self->endScriptID =
RegisterCallback(self->pCall, MOTEND, EndScriptCallback,
strdup(pCurrent->text), KillScript);
DeleteTokenList(pList);
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) {
iRet = MotorGetSoftPosition(self, pCon, &fValue);
if (!iRet) {
snprintf(pBueffel,sizeof(pBueffel)-1, "Error obtaining position for %s", argv[0]);
SCWrite(pCon, pBueffel, eValue);
DeleteTokenList(pList);
return 0;
}
snprintf(pBueffel,sizeof(pBueffel)-1, "%s.SoftPosition = %f", argv[0], fValue);
SCWrite(pCon, pBueffel, eValue);
DeleteTokenList(pList);
return 1;
} else if (strcmp(pName->text, "hardposition") == 0) {
iRet = MotorGetHardPosition(self, pCon, &fValue);
if (!iRet) {
snprintf(pBueffel,sizeof(pBueffel)-1, "Error obtaining position for %s", argv[0]);
SCWrite(pCon, pBueffel, eValue);
DeleteTokenList(pList);
return 0;
}
snprintf(pBueffel,sizeof(pBueffel)-1, "%s.HardPosition = %f", argv[0], fValue);
SCWrite(pCon, pBueffel, eValue);
DeleteTokenList(pList);
return 1;
} else if(strcmp(pName->text,"status") == 0){
if(self->running){
SCPrintf(pCon,eValue,"%s.status = run",argv[0]);
} else {
SCPrintf(pCon,eValue,"%s.status = idle",argv[0]);
}
return 1;
}
iRet = MotorGetPar(self, pName->text, &fValue);
if (!iRet) {
snprintf(pBueffel,sizeof(pBueffel)-1, "Parameter %s not found ", pName->text);
SCWrite(pCon, pBueffel, eValue);
DeleteTokenList(pList);
return 0;
} else {
snprintf(pBueffel,sizeof(pBueffel)-1, "%s.%s = %f", self->name, 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 {
snprintf(pBueffel,sizeof(pBueffel)-1, "Wrong argument type for %s %s set",
argv[0], pName->text);
SCWrite(pCon, pBueffel, eError);
DeleteTokenList(pList);
return 0;
}
iRet = MotorSetPar(self, pCon, pName->text, fValue);
DeleteTokenList(pList);
if (iRet)
SCSendOK(pCon);
return iRet;
}
}
return 0;
}
Reference in New Issue View Git Blame Copy Permalink