8fbfe687aa
- Added error resetting and a coupel of bug fixes for SLS magnets - Implemented new table driving mode for MARS
905 lines
26 KiB
C
905 lines
26 KiB
C
/*---------------------------------------------------------------------------
|
|
SICS object for driving a couple of motors along a tabulated given path.
|
|
|
|
copyright: see file COPYRIGHT
|
|
|
|
Mark Koennecke, July 2005
|
|
---------------------------------------------------------------------------*/
|
|
#include <stdio.h>
|
|
#include <math.h>
|
|
#include "../lld.h"
|
|
#include "tabledrive.h"
|
|
/*--------------------------------------------------------------------------*/
|
|
#define OUTOFSYNC 100
|
|
#define STARTING 101
|
|
#define STEPPING 102
|
|
#define WAITING 103
|
|
#define WAITFINISH 104
|
|
#define IDLE 105
|
|
|
|
#define ABS(x) (x < 0 ? -(x) : (x))
|
|
#define SIGN(x) (x < .0 ? (-1) : (1))
|
|
extern char *trim(char *txt);
|
|
/*
|
|
* copied from motor.c!!
|
|
*/
|
|
#define SLOW 0
|
|
#define SUPP 1
|
|
|
|
/*=================== the drivable interface functions =====================*/
|
|
static int TableDriveHalt(void *pData){
|
|
pTableDrive self = (pTableDrive)pData;
|
|
int status;
|
|
tdMotor moti;
|
|
|
|
if(self == NULL || self->motorTable < 0){
|
|
return 0;
|
|
}
|
|
|
|
status = LLDnodePtr2First(self->motorTable);
|
|
while(status != 0){
|
|
LLDnodeDataTo(self->motorTable,&moti);
|
|
if(moti.pMot != NULL){
|
|
moti.pMot->pDrivInt->Halt(moti.pMot);
|
|
}
|
|
status = LLDnodePtr2Next(self->motorTable);
|
|
}
|
|
self->state = OUTOFSYNC;
|
|
return 1;
|
|
}
|
|
/*-------------------------------------------------------------------------*/
|
|
static int TableDriveCheckLimits(void *pData, float fVal, char *error,
|
|
int iErrLen){
|
|
pTableDrive self = (pTableDrive)pData;
|
|
tdMotor moti;
|
|
|
|
if(self == NULL || self->motorTable < 0){
|
|
strncpy(error,"Path Table Not Defined!",25);
|
|
return 0;
|
|
}
|
|
if(fVal < 1. || fVal >= self->tableLength){
|
|
strncpy(error,"Out of Range",25);
|
|
return 0;
|
|
}
|
|
return 1;
|
|
}
|
|
/*--------------------------------------------------------------------------*/
|
|
static long TableDriveSetValue(void *pData, SConnection *pCon, float fVal){
|
|
|
|
pTableDrive self = (pTableDrive)pData;
|
|
char pError[132];
|
|
|
|
if(self == NULL || self->motorTable < 0){
|
|
SCWrite(pCon,"ERROR: no path defined",eError);
|
|
return HWFault;
|
|
}
|
|
strcpy(pError,"ERROR:");
|
|
if(!TableDriveCheckLimits(self,fVal,&pError[6],120)){
|
|
SCWrite(pCon,pError,eError);
|
|
return HWFault;
|
|
}
|
|
self->state = STARTING;
|
|
self->targetPosition = fVal;
|
|
return OKOK;
|
|
}
|
|
/*-------------------------------------------------------------------------*/
|
|
static int findOrientingMotor(pTableDrive self, ptdMotor moti){
|
|
int status;
|
|
|
|
if(!self->oriInvalid){
|
|
*moti = self->oriMotor;
|
|
return 1;
|
|
}
|
|
status = LLDnodePtr2First(self->motorTable);
|
|
while(status != 0){
|
|
LLDnodeDataTo(self->motorTable,moti);
|
|
if(strcmp(moti->motorName,self->orientMotor) == 0){
|
|
self->oriMotor = *moti;
|
|
self->oriInvalid = 0;
|
|
return 1;
|
|
}
|
|
status = LLDnodePtr2Next(self->motorTable);
|
|
}
|
|
return 0;
|
|
}
|
|
/*-------------------------------------------------------------------------*/
|
|
static float locatePosition(tdMotor moti, SConnection *pCon){
|
|
int status, begin = 1;
|
|
float value;
|
|
double diff;
|
|
tdEntry lower, upper;
|
|
|
|
status = MotorGetSoftPosition(moti.pMot,pCon,&value);
|
|
if(!status) {
|
|
return -9999.99;
|
|
}
|
|
status = LLDnodePtr2First(moti.table);
|
|
while(status != 0){
|
|
LLDnodeDataTo(moti.table,&lower);
|
|
status = LLDnodePtr2Next(moti.table);
|
|
if(status != 0) {
|
|
LLDnodeDataTo(moti.table,&upper);
|
|
} else {
|
|
/*
|
|
* end of table
|
|
*/
|
|
return lower.tablePos;
|
|
}
|
|
/**
|
|
* we have to deal with ascending and descending tables. This is
|
|
* why we have to have some funny logic here
|
|
*/
|
|
if(upper.position > lower.position){
|
|
/*
|
|
* ascending table
|
|
*/
|
|
if(value >= lower.position && value < upper.position){
|
|
diff = upper.position - lower.position;
|
|
return lower.tablePos + (value - lower.position)/diff;
|
|
}
|
|
if(begin == 1 && value < lower.position) {
|
|
return lower.tablePos;
|
|
}
|
|
} else {
|
|
/*
|
|
* descending table
|
|
*/
|
|
if(value <= lower.position && value > upper.position){
|
|
diff = lower.position - upper.position;
|
|
return lower.tablePos + ABS(value - lower.position)/diff;
|
|
}
|
|
if(begin == 1 && value > lower.position){
|
|
return lower.tablePos;
|
|
}
|
|
}
|
|
begin = 0;
|
|
}
|
|
return -999.99;
|
|
}
|
|
/*-------------------------------------------------------------------------*/
|
|
static void findBracket(tdMotor moti, float value, ptdEntry upper,
|
|
ptdEntry lower){
|
|
int status, targetCount;
|
|
float diff;
|
|
|
|
targetCount = (int)floor(value);
|
|
status = LLDnodePtr2First(moti.table);
|
|
while(status != 0){
|
|
LLDnodeDataTo(moti.table,lower);
|
|
if(lower->tablePos == targetCount){
|
|
LLDnodeDataTo(moti.table,upper);
|
|
if(LLDnodePtr2Next(moti.table)){
|
|
LLDnodeDataTo(moti.table,upper);
|
|
}
|
|
return;
|
|
}
|
|
status = LLDnodePtr2Next(moti.table);
|
|
}
|
|
}
|
|
/*-------------------------------------------------------------------------*/
|
|
static float findTarget(tdMotor moti, float value){
|
|
int targetCount;
|
|
tdEntry lower, upper;
|
|
float diff;
|
|
|
|
targetCount = (int)floor(value);
|
|
findBracket(moti,value,&upper,&lower);
|
|
if(ABS(targetCount - value) < .00001){
|
|
return lower.position;
|
|
} else {
|
|
diff = upper.position - lower.position;
|
|
return (float)lower.position + (value - targetCount)*diff;
|
|
}
|
|
}
|
|
/*-------------------------------------------------------------------------*/
|
|
static void checkSync(pTableDrive self, SConnection *pCon, float value){
|
|
int status, test;
|
|
float motorPosition, targetPosition, tolerance, diff;
|
|
tdMotor moti;
|
|
char pBueffel[256];
|
|
|
|
if(self->state != IDLE){
|
|
return;
|
|
}
|
|
|
|
status = LLDnodePtr2First(self->motorTable);
|
|
while(status != 0){
|
|
LLDnodeDataTo(self->motorTable,&moti);
|
|
test = MotorGetSoftPosition(moti.pMot,pCon,&motorPosition);
|
|
if(!test){
|
|
snprintf(pBueffel,255,"ERROR: failed to read motor %s",
|
|
moti.motorName);
|
|
SCWrite(pCon,pBueffel,eError);
|
|
} else {
|
|
MotorGetPar(moti.pMot,"precision",&tolerance);
|
|
targetPosition = findTarget(moti,value);
|
|
if(ABS(targetPosition - motorPosition) > tolerance){
|
|
snprintf(pBueffel,256,
|
|
"WARNING: motor %s out of sync by %f",
|
|
moti.motorName, ABS(targetPosition - motorPosition));
|
|
SCWrite(pCon,pBueffel,eWarning);
|
|
}
|
|
}
|
|
status = LLDnodePtr2Next(self->motorTable);
|
|
}
|
|
}
|
|
/*--------------------------------------------------------------------------*/
|
|
static float TableDriveGetValue(void *pData, SConnection *pCon){
|
|
pTableDrive self = (pTableDrive)pData;
|
|
tdMotor orient;
|
|
float value;
|
|
|
|
if(self == NULL || self->motorTable < 0){
|
|
SCWrite(pCon,"ERROR: no path defined",eError);
|
|
return -9999.99;
|
|
}
|
|
if(!findOrientingMotor(self,&orient)){
|
|
SCWrite(pCon,"ERROR: table corrupted or orienting motor not found",
|
|
eError);
|
|
return -9999.99;
|
|
}
|
|
value = locatePosition(orient,pCon);
|
|
checkSync(self,pCon, value);
|
|
|
|
self->currentPosition = value;
|
|
return value;
|
|
}
|
|
/*-------------------------------------------------------------------------*/
|
|
static void tableInfo(pTableDrive self, SConnection *pCon){
|
|
int status, test;
|
|
float motorPosition, targetPosition, tolerance, diff, value;
|
|
tdMotor moti;
|
|
char pBueffel[256];
|
|
|
|
value = TableDriveGetValue(self,pCon);
|
|
snprintf(pBueffel,255," Triffid Position: %8.2f", value);
|
|
SCWrite(pCon,pBueffel,eValue);
|
|
|
|
status = LLDnodePtr2First(self->motorTable);
|
|
while(status != 0){
|
|
LLDnodeDataTo(self->motorTable,&moti);
|
|
test = MotorGetSoftPosition(moti.pMot,pCon,&motorPosition);
|
|
if(!test){
|
|
snprintf(pBueffel,255,"ERROR: failed to read motor %s",
|
|
moti.motorName);
|
|
SCWrite(pCon,pBueffel,eError);
|
|
} else {
|
|
MotorGetPar(moti.pMot,"precision",&tolerance);
|
|
targetPosition = findTarget(moti,value);
|
|
snprintf(pBueffel,256,"Motor %10s, should: %8.2f, is %8.2f, diff = %8.2f",
|
|
moti.motorName, motorPosition, targetPosition,
|
|
ABS(targetPosition - motorPosition));
|
|
SCWrite(pCon,pBueffel,eValue);
|
|
}
|
|
status = LLDnodePtr2Next(self->motorTable);
|
|
}
|
|
}
|
|
/*------------------------------------------------------------------------*/
|
|
static void tableSetPar(pMotor pMot, char *name, float value){
|
|
ObPar *ob = NULL;
|
|
|
|
ob = ObParFind(pMot->ParArray,name);
|
|
assert(ob != NULL);
|
|
ob->fVal = value;
|
|
}
|
|
/*------------------------------------------------------------------------
|
|
* Before I can drive the motors, I have to release the software limits..
|
|
-------------------------------------------------------------------------*/
|
|
static void liberateMotors(pTableDrive self, SConnection *pCon){
|
|
int status;
|
|
tdMotor moti;
|
|
float upper, lower;
|
|
|
|
status = LLDnodePtr2First(self->motorTable);
|
|
while(status != 0){
|
|
LLDnodeDataTo(self->motorTable,&moti);
|
|
MotorGetPar(moti.pMot,"hardupperlim",&upper);
|
|
MotorGetPar(moti.pMot,"hardlowerlim",&lower);
|
|
|
|
tableSetPar(moti.pMot,"softupperlim",upper);
|
|
tableSetPar(moti.pMot,"softlowerlim",lower);
|
|
status = LLDnodePtr2Next(self->motorTable);
|
|
}
|
|
}
|
|
/*------------------------------------------------------------------------
|
|
* after driving the motors, we have to set the software limits in order
|
|
* to prevent the user from manually crashing components
|
|
------------------------------------------------------------------------ */
|
|
static void closeMotors(pTableDrive self, SConnection *pCon){
|
|
int status;
|
|
tdMotor moti;
|
|
tdEntry tdLower, tdUpper;
|
|
float upper, lower, diff, targetCount;
|
|
|
|
targetCount = floor(self->currentPosition);
|
|
status = LLDnodePtr2First(self->motorTable);
|
|
while(status != 0){
|
|
LLDnodeDataTo(self->motorTable,&moti);
|
|
findBracket(moti,self->currentPosition,&tdUpper,&tdLower);
|
|
diff = tdUpper.upper - tdLower.upper;
|
|
upper = tdLower.upper + (self->currentPosition - targetCount)*diff;
|
|
diff = tdUpper.lower - tdLower.lower;
|
|
lower = tdLower.lower + (self->currentPosition - targetCount)*diff;
|
|
tableSetPar(moti.pMot,"softupperlim",upper);
|
|
tableSetPar(moti.pMot,"softlowerlim",lower);
|
|
status = LLDnodePtr2Next(self->motorTable);
|
|
}
|
|
}
|
|
/*------------------------------------------------------------------------*/
|
|
static float calculateNextStep(pTableDrive self){
|
|
float step;
|
|
|
|
step = self->targetPosition - self->currentPosition;
|
|
if(ABS(step) > 1.){
|
|
step = SIGN(step)*1.0;
|
|
}
|
|
return step;
|
|
}
|
|
/*------------------------------------------------------------------------*/
|
|
static int runToNextStep(pTableDrive self, SConnection *pCon,
|
|
float value){
|
|
int status, test;
|
|
tdMotor moti;
|
|
float target;
|
|
|
|
status = LLDnodePtr2First(self->motorTable);
|
|
while(status != 0){
|
|
LLDnodeDataTo(self->motorTable,&moti);
|
|
target = findTarget(moti,value);
|
|
test = MotorRun(moti.pMot,pCon,target);
|
|
if(test != OKOK){
|
|
return test;
|
|
}
|
|
status = LLDnodePtr2Next(self->motorTable);
|
|
}
|
|
return 1;
|
|
}
|
|
/*------------------------------------------------------------------------*/
|
|
static int checkRunning(pTableDrive self, SConnection *pCon){
|
|
int status;
|
|
int test;
|
|
tdMotor moti;
|
|
|
|
status = LLDnodePtr2First(self->motorTable);
|
|
while(status != 0){
|
|
LLDnodeDataTo(self->motorTable,&moti);
|
|
test = moti.pMot->pDrivInt->CheckStatus(moti.pMot,pCon);
|
|
if(test != HWIdle && test != OKOK){
|
|
return test;
|
|
}
|
|
status = LLDnodePtr2Next(self->motorTable);
|
|
}
|
|
return HWIdle;
|
|
}
|
|
/*------------------------------------------------------------------------*/
|
|
static void showPositions(pTableDrive self, SConnection *pCon,
|
|
char *pBueffel, int buffLen){
|
|
float value;
|
|
int status, test, len;
|
|
tdMotor moti;
|
|
|
|
value = TableDriveGetValue(self,pCon);
|
|
snprintf(pBueffel,buffLen," %8.2f : ", value);
|
|
|
|
status = LLDnodePtr2First(self->motorTable);
|
|
while(status != 0){
|
|
LLDnodeDataTo(self->motorTable,&moti);
|
|
test = MotorGetSoftPosition(moti.pMot,pCon,&value);
|
|
len = strlen(pBueffel);
|
|
snprintf(&pBueffel[len],buffLen - len,
|
|
"%s = %8.2f ", moti.motorName, value);
|
|
status = LLDnodePtr2Next(self->motorTable);
|
|
}
|
|
}
|
|
/*-------------------------------------------------------------------------
|
|
* This is the old scheme: drive table positions tablestep by tablestep.
|
|
* I leave this code here: may be it is useful in another scenario then
|
|
* MARS. Then some means must be found to select the table drive
|
|
* algorithm.
|
|
* -----------------------------------------------------------------------*/
|
|
static int TableDriveCheckStatusOld(void *pData, SConnection *pCon){
|
|
pTableDrive self = (pTableDrive)pData;
|
|
int status;
|
|
float step;
|
|
char pBueffel[1024];
|
|
|
|
switch(self->state){
|
|
case STARTING:
|
|
/*
|
|
* make sure that our current position is up-to-date
|
|
*/
|
|
TableDriveGetValue(self,pCon);
|
|
liberateMotors(self,pCon);
|
|
self->state = STEPPING;
|
|
return HWBusy;
|
|
break;
|
|
case STEPPING:
|
|
step = calculateNextStep(self);
|
|
status = runToNextStep(self,pCon, self->currentPosition + step);
|
|
if(status == OKOK){
|
|
if(ABS(step) < 1.){
|
|
self->state = WAITFINISH;
|
|
} else {
|
|
self->state = WAITING;
|
|
}
|
|
return HWBusy;
|
|
} else {
|
|
TableDriveHalt(self);
|
|
return HWFault;
|
|
}
|
|
break;
|
|
case WAITING:
|
|
status = checkRunning(self,pCon);
|
|
switch(status){
|
|
case HWIdle:
|
|
case OKOK:
|
|
self->state = STEPPING;
|
|
if(self->debug > 0){
|
|
showPositions(self,pCon,pBueffel,1023);
|
|
SCWrite(pCon,pBueffel,eValue);
|
|
} else {
|
|
self->currentPosition += calculateNextStep(self);
|
|
}
|
|
return HWBusy;
|
|
break;
|
|
case HWBusy:
|
|
return HWBusy;
|
|
break;
|
|
default:
|
|
TableDriveHalt(self);
|
|
self->state = WAITFINISH;
|
|
return HWBusy;
|
|
break;
|
|
}
|
|
case WAITFINISH:
|
|
status = checkRunning(self,pCon);
|
|
if(status != HWBusy){
|
|
TableDriveGetValue(self,pCon);
|
|
closeMotors(self,pCon);
|
|
self->state = IDLE;
|
|
return status;
|
|
} else {
|
|
return HWBusy;
|
|
}
|
|
break;
|
|
case OUTOFSYNC:
|
|
SCWrite(pCon,"WARNING: tabledrive out of sync",eWarning);
|
|
self->state = IDLE;
|
|
return HWFault;
|
|
break;
|
|
default:
|
|
SCWrite(pCon,
|
|
"ERROR: programming error in tabledrive, invalid state",
|
|
eError);
|
|
return HWFault;
|
|
break;
|
|
}
|
|
return HWFault;
|
|
}
|
|
/*===========================================================================
|
|
* This is code for the new scheme of driving MARS tables:
|
|
* 1) drive ds and as to 0
|
|
* 2) drive all other motors to targets directly
|
|
* 3) drive ds and as to desired positions.
|
|
* =========================================================================*/
|
|
#define WAITDSAS 110
|
|
#define WAITOTHER 111
|
|
#define ASDSTO0 200
|
|
#define ASDSTOTARGET 201
|
|
/*-------------------------------------------------------------------------*/
|
|
static int startASDS(pTableDrive self, SConnection *pCon, int target){
|
|
tdMotor moti;
|
|
int status;
|
|
char name[132];
|
|
float targetValue;
|
|
|
|
if(self == NULL || self->motorTable < 0){
|
|
return 0;
|
|
}
|
|
|
|
status = LLDnodePtr2First(self->motorTable);
|
|
while(status != 0){
|
|
LLDnodeDataTo(self->motorTable,&moti);
|
|
strncpy(name,moti.motorName,131);
|
|
strtolower(name);
|
|
if(strstr(name,"ds") != NULL || strstr(name,"as") != NULL){
|
|
if(target == ASDSTO0){
|
|
targetValue = 0.0;
|
|
} else {
|
|
targetValue = findTarget(moti, self->targetPosition);
|
|
}
|
|
status = MotorRun(moti.pMot,pCon,targetValue);
|
|
if(status != OKOK){
|
|
return status;
|
|
}
|
|
}
|
|
status = LLDnodePtr2Next(self->motorTable);
|
|
}
|
|
return OKOK;
|
|
}
|
|
/*-------------------------------------------------------------------------*/
|
|
static int startOthers(pTableDrive self, SConnection *pCon){
|
|
tdMotor moti;
|
|
int status;
|
|
char name[132];
|
|
float targetValue;
|
|
|
|
if(self == NULL || self->motorTable < 0){
|
|
return 0;
|
|
}
|
|
|
|
status = LLDnodePtr2First(self->motorTable);
|
|
while(status != 0){
|
|
LLDnodeDataTo(self->motorTable,&moti);
|
|
strncpy(name,moti.motorName,131);
|
|
strtolower(name);
|
|
if(strstr(name,"ds") == NULL && strstr(name,"as") == NULL){
|
|
targetValue = findTarget(moti, self->targetPosition);
|
|
status = MotorRun(moti.pMot,pCon,targetValue);
|
|
if(status != OKOK){
|
|
return status;
|
|
}
|
|
}
|
|
status = LLDnodePtr2Next(self->motorTable);
|
|
}
|
|
return OKOK;
|
|
}
|
|
/*-------------------------------------------------------------------------*/
|
|
static int TableDriveCheckStatus(void *pData, SConnection *pCon){
|
|
pTableDrive self = (pTableDrive)pData;
|
|
int status;
|
|
float step;
|
|
char pBueffel[1024];
|
|
|
|
switch(self->state){
|
|
case STARTING:
|
|
/*
|
|
* make sure that our current position is up-to-date
|
|
*/
|
|
TableDriveGetValue(self,pCon);
|
|
liberateMotors(self,pCon);
|
|
status = startASDS(self,pCon,ASDSTO0);
|
|
if(status != OKOK){
|
|
TableDriveHalt(self);
|
|
return HWFault;
|
|
}
|
|
self->state = WAITDSAS;
|
|
return HWBusy;
|
|
break;
|
|
case WAITDSAS:
|
|
status = checkRunning(self,pCon);
|
|
switch(status){
|
|
case HWIdle:
|
|
case OKOK:
|
|
if(self->debug > 0){
|
|
showPositions(self,pCon,pBueffel,1023);
|
|
SCWrite(pCon,pBueffel,eValue);
|
|
}
|
|
status = startOthers(self,pCon);
|
|
if(status != OKOK){
|
|
TableDriveHalt(self);
|
|
return HWFault;
|
|
}
|
|
self->state = WAITOTHER;
|
|
return HWBusy;
|
|
break;
|
|
case HWBusy:
|
|
return HWBusy;
|
|
break;
|
|
default:
|
|
TableDriveHalt(self);
|
|
self->state = WAITFINISH;
|
|
return HWBusy;
|
|
break;
|
|
}
|
|
break;
|
|
case WAITOTHER:
|
|
status = checkRunning(self,pCon);
|
|
switch(status){
|
|
case HWIdle:
|
|
case OKOK:
|
|
if(self->debug > 0){
|
|
showPositions(self,pCon,pBueffel,1023);
|
|
SCWrite(pCon,pBueffel,eValue);
|
|
}
|
|
status = startASDS(self,pCon,ASDSTOTARGET);
|
|
if(status != OKOK){
|
|
TableDriveHalt(self);
|
|
return HWFault;
|
|
}
|
|
self->state = WAITFINISH;
|
|
return HWBusy;
|
|
break;
|
|
case HWBusy:
|
|
return HWBusy;
|
|
break;
|
|
default:
|
|
TableDriveHalt(self);
|
|
self->state = WAITFINISH;
|
|
return HWBusy;
|
|
break;
|
|
}
|
|
break;
|
|
case WAITFINISH:
|
|
status = checkRunning(self,pCon);
|
|
if(status != HWBusy){
|
|
if(self->debug > 0){
|
|
showPositions(self,pCon,pBueffel,1023);
|
|
SCWrite(pCon,pBueffel,eValue);
|
|
}
|
|
TableDriveGetValue(self,pCon);
|
|
closeMotors(self,pCon);
|
|
self->state = IDLE;
|
|
return status;
|
|
} else {
|
|
return HWBusy;
|
|
}
|
|
break;
|
|
case OUTOFSYNC:
|
|
SCWrite(pCon,"WARNING: tabledrive out of sync",eWarning);
|
|
self->state = IDLE;
|
|
return HWFault;
|
|
break;
|
|
default:
|
|
SCWrite(pCon,
|
|
"ERROR: programming error in tabledrive, invalid state",
|
|
eError);
|
|
return HWFault;
|
|
break;
|
|
}
|
|
return HWFault;
|
|
}
|
|
/*================== live and death ========================================*/
|
|
static void *TableDriveInterface(void *pData, int ID){
|
|
pTableDrive self = (pTableDrive)pData;
|
|
if(self == NULL){
|
|
return NULL;
|
|
}
|
|
|
|
if(ID == DRIVEID){
|
|
return self->pDriv;
|
|
} else {
|
|
return NULL;
|
|
}
|
|
}
|
|
/*--------------------------------------------------------------------------*/
|
|
static void clearTable(int table){
|
|
int status;
|
|
tdMotor moti;
|
|
|
|
status = LLDnodePtr2First(table);
|
|
while(status != 0){
|
|
LLDnodeDataTo(table,&moti);
|
|
LLDdelete(moti.table);
|
|
status = LLDnodePtr2Next(table);
|
|
}
|
|
}
|
|
/*------------------------------------------------------------------------*/
|
|
static int loadTable(pTableDrive self, char *filename, SConnection *pCon){
|
|
FILE *fd = NULL;
|
|
tdMotor moti;
|
|
tdEntry entry;
|
|
char pBueffel[512];
|
|
int lineCount, tableCount = -1, read;
|
|
|
|
if(self->motorTable >= 0){
|
|
clearTable(self->motorTable);
|
|
}
|
|
fd = fopen(filename,"r");
|
|
if(fd == NULL){
|
|
snprintf(pBueffel,511,"ERROR: failed to open %s for reading",
|
|
filename);
|
|
SCWrite(pCon,pBueffel,eError);
|
|
return 0;
|
|
}
|
|
self->motorTable = LLDcreate(sizeof(tdMotor));
|
|
if(self->motorTable < 0){
|
|
SCWrite(pCon,"ERROR: failed to allocate motor table",eError);
|
|
fclose(fd);
|
|
return 0;
|
|
}
|
|
|
|
while(fgets(pBueffel,511,fd) != NULL){
|
|
if(pBueffel[0] == '#'){
|
|
strcpy(moti.motorName,trim(&pBueffel[1]));
|
|
moti.pMot = FindCommandData(pServ->pSics,
|
|
moti.motorName,
|
|
"Motor");
|
|
if(moti.pMot == NULL){
|
|
snprintf(pBueffel,511,"ERROR: motor %s NOT found!",
|
|
moti.motorName);
|
|
SCWrite(pCon,pBueffel,eError);
|
|
fclose(fd);
|
|
return 0;
|
|
}
|
|
moti.table = LLDcreate(sizeof(tdEntry));
|
|
if(moti.table < 0){
|
|
SCWrite(pCon,"ERROR: failed to allocate table data",eError);
|
|
fclose(fd);
|
|
return 0;
|
|
}
|
|
lineCount = 1;
|
|
} else {
|
|
read = sscanf(pBueffel,"%lf %lf %lf", &entry.lower,&entry.position,
|
|
&entry.upper);
|
|
if(read >= 3){
|
|
entry.tablePos = lineCount;
|
|
LLDnodeAppendFrom(moti.table,&entry);
|
|
lineCount++;
|
|
} else {
|
|
/*
|
|
* we are on a break line
|
|
*/
|
|
if(tableCount < 0){
|
|
tableCount = lineCount;
|
|
} else {
|
|
if(lineCount != tableCount){
|
|
SCWrite(pCon,
|
|
"ERROR: bad table file, table length mismatch",
|
|
eError);
|
|
fclose(fd);
|
|
return 0;
|
|
}
|
|
}
|
|
LLDnodeAppendFrom(self->motorTable,&moti);
|
|
}
|
|
}
|
|
}
|
|
self->tableLength = tableCount-1;
|
|
fclose(fd);
|
|
return 1;
|
|
}
|
|
/*-------------------------------------------------------------------------*/
|
|
static void killTableDrive(void *pData){
|
|
pTableDrive self = (pTableDrive)pData;
|
|
if(self == NULL){
|
|
return;
|
|
}
|
|
if(self->pDes != NULL){
|
|
DeleteDescriptor(self->pDes);
|
|
}
|
|
if(self->motorTable >= 0){
|
|
clearTable(self->motorTable);
|
|
LLDdelete(self->motorTable);
|
|
self->motorTable = -1;
|
|
}
|
|
if(self->pDriv != NULL){
|
|
free(self->pDriv);
|
|
}
|
|
free(self);
|
|
}
|
|
/*-------------------------------------------------------------------------*/
|
|
int TableDriveFactory(SConnection *pCon, SicsInterp *pSics, void *pData,
|
|
int argc, char *argv[]){
|
|
pTableDrive pNew = NULL;
|
|
char pBueffel[256];
|
|
int status;
|
|
|
|
if(argc < 3){
|
|
SCWrite(pCon,"ERROR: insufficient parameters to TableDriveFactory",
|
|
eError);
|
|
return 0;
|
|
}
|
|
|
|
pNew = (pTableDrive)malloc(sizeof(TableDrive));
|
|
if(pNew == NULL){
|
|
SCWrite(pCon,"ERROT: out of memory creating table drive",eError);
|
|
return 0;
|
|
}
|
|
memset(pNew,0,sizeof(TableDrive));
|
|
|
|
pNew->pDes = CreateDescriptor("TableDrive");
|
|
pNew->pDriv = CreateDrivableInterface();
|
|
if(pNew->pDes == NULL || pNew->pDriv == NULL){
|
|
SCWrite(pCon,"ERROT: out of memory creating table drive",eError);
|
|
return 0;
|
|
}
|
|
pNew->pDes->GetInterface = TableDriveInterface;
|
|
pNew->pDriv->CheckLimits = TableDriveCheckLimits;
|
|
pNew->pDriv->CheckStatus = TableDriveCheckStatus;
|
|
pNew->pDriv->GetValue = TableDriveGetValue;
|
|
pNew->pDriv->Halt = TableDriveHalt;
|
|
pNew->pDriv->SetValue = TableDriveSetValue;
|
|
pNew->motorTable = -1;
|
|
pNew->state = OUTOFSYNC;
|
|
pNew->tableLength = 0;
|
|
pNew->targetPosition = 0;
|
|
pNew->oriInvalid = 1;
|
|
|
|
if(!loadTable(pNew,argv[2],pCon)){
|
|
killTableDrive(pNew);
|
|
return 0;
|
|
}
|
|
pNew->state = IDLE;
|
|
|
|
status = AddCommand(pSics,argv[1],
|
|
TableDriveAction,
|
|
killTableDrive,
|
|
pNew);
|
|
if(!status){
|
|
snprintf(pBueffel,256,"ERROR: duplicate command %s not created",
|
|
argv[1]);
|
|
SCWrite(pCon,pBueffel,eError);
|
|
killTableDrive(pNew);
|
|
return 0;
|
|
}
|
|
return 1;
|
|
}
|
|
/*====================== interpreter interface ============================*/
|
|
int TableDriveAction(SConnection *pCon, SicsInterp *pSics, void *pData,
|
|
int argc, char *argv[]){
|
|
pTableDrive self = (pTableDrive)pData;
|
|
int status;
|
|
char pBueffel[1024];
|
|
float value;
|
|
pDynString print = NULL;
|
|
|
|
if(argc < 2){
|
|
value = TableDriveGetValue(self,pCon);
|
|
snprintf(pBueffel,255,"%s = %f", argv[0], value);
|
|
SCWrite(pCon,pBueffel,eValue);
|
|
if(value > -999){
|
|
return 1;
|
|
} else {
|
|
return 0;
|
|
}
|
|
} else {
|
|
strtolower(argv[1]);
|
|
if(strcmp(argv[1],"load") == 0){
|
|
if(!SCMatchRights(pCon,usMugger)){
|
|
return 0;
|
|
}
|
|
if(argc < 3){
|
|
SCWrite(pCon,"ERROR: require filename to load",
|
|
eError);
|
|
return 0;
|
|
}
|
|
status = loadTable(self,argv[2],pCon);
|
|
if(status == 1){
|
|
SCSendOK(pCon);
|
|
}
|
|
return status;
|
|
}else if(strcmp(argv[1],"show") == 0){
|
|
showPositions(self,pCon,pBueffel,1023);
|
|
SCWrite(pCon,pBueffel,eValue);
|
|
return 1;
|
|
}else if(strcmp(argv[1],"info") == 0){
|
|
SCStartBuffering(pCon);
|
|
tableInfo(self,pCon);
|
|
print = SCEndBuffering(pCon);
|
|
if(print != NULL){
|
|
SCWrite(pCon,GetCharArray(print), eValue);
|
|
}
|
|
return 1;
|
|
}else if(strcmp(argv[1],"orient") == 0){
|
|
if(argc > 2){
|
|
if(!SCMatchRights(pCon,usMugger)){
|
|
return 0;
|
|
}
|
|
strncpy(self->orientMotor,argv[2],80);
|
|
self->oriInvalid = 1;
|
|
SCSendOK(pCon);
|
|
return 1;
|
|
} else {
|
|
snprintf(pBueffel,255,"%s.orient = %s",
|
|
argv[0],self->orientMotor);
|
|
SCWrite(pCon,pBueffel,eValue);
|
|
return 1;
|
|
}
|
|
}else if(strcmp(argv[1],"debug") == 0){
|
|
if(self->debug == 0){
|
|
self->debug = 1;
|
|
} else {
|
|
self->debug = 0;
|
|
}
|
|
SCSendOK(pCon);
|
|
return 1;
|
|
} else {
|
|
SCWrite(pCon,"ERROR: sub command not understood",eError);
|
|
return 0;
|
|
}
|
|
}
|
|
return 1;
|
|
}
|
|
|