- Added tabledrive: table driven path for MARS
- Initial MARS development - Upgraded Manager Manual
This commit is contained in:
2
make_gen
2
make_gen
@ -17,7 +17,7 @@ OBJ=psi.o buffer.o ruli.o dmc.o nxsans.o nextrics.o sps.o pimotor.o \
|
||||
t_rlp.o t_conv.o el737hpdriv.o dornier2.o el734hp.o \
|
||||
el737hpv2driv.o swmotor2.o tricssupport.o \
|
||||
oicom.o fsm.o remob.o eve.o logger.o dgrambroadcast.o \
|
||||
ipsdriv.o itcdriv.o ilmdriv.o lcdriv.o sinq.o
|
||||
ipsdriv.o itcdriv.o ilmdriv.o lcdriv.o sinq.o tabledrive.o
|
||||
|
||||
libpsi.a: $(OBJ)
|
||||
rm -f libpsi.a
|
||||
|
3
psi.c
3
psi.c
@ -52,6 +52,7 @@
|
||||
#include "remob.h"
|
||||
#include "tricssupport.h"
|
||||
#include "sinq.h"
|
||||
#include "tabledrive.h"
|
||||
|
||||
static pSite sitePSI = NULL;
|
||||
|
||||
@ -83,6 +84,7 @@ static void AddPsiCommands(SicsInterp *pInter){
|
||||
AddCommand(pInter,"Remob",RemobCreate,NULL,NULL);
|
||||
AddCommand(pInter,"Graph",LoggerGraph,NULL,NULL);
|
||||
AddCommand(pInter,"MakeSinq",SinqFactory,NULL,NULL);
|
||||
AddCommand(pInter,"MakeTableDrive",TableDriveFactory,NULL,NULL);
|
||||
/*
|
||||
AddCommand(pInter,"MakeDifrac",MakeDifrac,NULL,NULL);
|
||||
*/
|
||||
@ -112,6 +114,7 @@ static void RemovePsiCommands(SicsInterp *pSics){
|
||||
RemoveCommand(pSics,"MakePSDFrame");
|
||||
RemoveCommand(pSics,"SerialInit");
|
||||
RemoveCommand(pSics,"MakeSinq");
|
||||
RemoveCommand(pSics,"MakeTableDrive");
|
||||
}
|
||||
/*---------------------------------------------------------------------*/
|
||||
MotorDriver *CreateEL734(SConnection *pCon, int argc, char *argv[]);
|
||||
|
634
tabledrive.c
Normal file
634
tabledrive.c
Normal file
@ -0,0 +1,634 @@
|
||||
/*---------------------------------------------------------------------------
|
||||
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 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 < 0. || 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;
|
||||
|
||||
status = LLDnodePtr2First(self->motorTable);
|
||||
while(status != 0){
|
||||
LLDnodeDataTo(self->motorTable,moti);
|
||||
if(strcmp(moti->motorName,self->orientMotor) == 0){
|
||||
return 1;
|
||||
}
|
||||
status = LLDnodePtr2Next(self->motorTable);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
/*-------------------------------------------------------------------------*/
|
||||
static float interpolateLocate(tdEntry lower, tdEntry upper,
|
||||
float value, int count){
|
||||
float diff, result = count -1;
|
||||
|
||||
result = count;
|
||||
diff = upper.position - lower.position;
|
||||
value -= lower.position;
|
||||
if(ABS(diff) > .00001){
|
||||
result += value/diff;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
/*-------------------------------------------------------------------------*/
|
||||
static float locatePosition(tdMotor moti, SConnection *pCon){
|
||||
int status, count = 0;
|
||||
float value;
|
||||
tdEntry entry, back;
|
||||
|
||||
status = MotorGetSoftPosition(moti.pMot,pCon,&value);
|
||||
if(!status) {
|
||||
return -9999.99;
|
||||
}
|
||||
status = LLDnodePtr2First(moti.table);
|
||||
while(status != 0){
|
||||
LLDnodeDataTo(moti.table,&entry);
|
||||
if(entry.position > value){
|
||||
if(count == 0){
|
||||
return 1.;
|
||||
}
|
||||
LLDnodePtr2Prev(moti.table);
|
||||
LLDnodeDataTo(moti.table,&back);
|
||||
return interpolateLocate(back,entry,value, count);
|
||||
}
|
||||
count++;
|
||||
status = LLDnodePtr2Next(moti.table);
|
||||
}
|
||||
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);
|
||||
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];
|
||||
|
||||
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;
|
||||
}
|
||||
/*------------------------------------------------------------------------
|
||||
* 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);
|
||||
moti.pMot->ParArray[SLOW].fVal = lower;
|
||||
moti.pMot->ParArray[SUPP].fVal = upper;
|
||||
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;
|
||||
moti.pMot->ParArray[SLOW].fVal = lower;
|
||||
moti.pMot->ParArray[SUPP].fVal = upper;
|
||||
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);
|
||||
}
|
||||
}
|
||||
/*-------------------------------------------------------------------------*/
|
||||
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);
|
||||
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);
|
||||
return status;
|
||||
}
|
||||
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;
|
||||
|
||||
if(!loadTable(pNew,argv[2],pCon)){
|
||||
killTableDrive(pNew);
|
||||
return 0;
|
||||
}
|
||||
|
||||
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;
|
||||
|
||||
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],"orient") == 0){
|
||||
if(argc > 2){
|
||||
if(!SCMatchRights(pCon,usMugger)){
|
||||
return 0;
|
||||
}
|
||||
strncpy(self->orientMotor,argv[2],80);
|
||||
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;
|
||||
}
|
||||
|
53
tabledrive.h
Normal file
53
tabledrive.h
Normal file
@ -0,0 +1,53 @@
|
||||
|
||||
/*---------------------------------------------------------------------------
|
||||
SICS object for driving a couple of motors along a tabulated given path.
|
||||
|
||||
copyright: see file COPYRIGHT
|
||||
|
||||
Mark Koennecke, July 2005
|
||||
---------------------------------------------------------------------------*/
|
||||
#ifndef SICSTABLEDRIVE
|
||||
#define SICSTABLEDRIVE
|
||||
#include <sics.h>
|
||||
#include "../motor.h"
|
||||
/*-------------------------------------------------------------------------*/
|
||||
|
||||
typedef struct{
|
||||
double lower, position, upper;
|
||||
int tablePos;
|
||||
}tdEntry, *ptdEntry;
|
||||
|
||||
/*-------------------------------------------------------------------------*/
|
||||
|
||||
typedef struct {
|
||||
char motorName[132];
|
||||
int table;
|
||||
pMotor pMot;
|
||||
}tdMotor, *ptdMotor;
|
||||
|
||||
/*-------------------------------------------------------------------------*/
|
||||
|
||||
typedef struct{
|
||||
pObjectDescriptor pDes;
|
||||
pIDrivable pDriv;
|
||||
int motorTable;
|
||||
int tableLength;
|
||||
float targetPosition;
|
||||
float currentPosition;
|
||||
int state;
|
||||
char orientMotor[80];
|
||||
int debug;
|
||||
}TableDrive, *pTableDrive;
|
||||
|
||||
/*-------------------------------------------------------------------------*/
|
||||
|
||||
int TableDriveFactory(SConnection *pCon, SicsInterp *pSics, void *pData,
|
||||
int argc, char *argv[]);
|
||||
int TableDriveAction(SConnection *pCon, SicsInterp *pSics, void *pData,
|
||||
int argc, char *argv[]);
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
|
117
tabledrive.w
Normal file
117
tabledrive.w
Normal file
@ -0,0 +1,117 @@
|
||||
\subsection{Tabled Driving}
|
||||
This object implements driving several motors along a predefined path. The definition
|
||||
of the path happens through a table. Positions between tabulated positions are
|
||||
interpolated by linear interpolation. Additionally, each motor may be driven a
|
||||
bit from the tabulated positions for fine adjustments. Of course the limits are
|
||||
variable from position to position. Thus this object also sets the software limits of the
|
||||
motors accordingly. This object assumes that motors can be driven between positions
|
||||
without watching for collisions. The original use of this module is to coordinate the
|
||||
movements of the MARS triffids or girafs.
|
||||
|
||||
The table lives in a separate file. The format of the file is very simple:
|
||||
Each block starts with a line containing:
|
||||
\begin{verbatim}
|
||||
# motorname
|
||||
\end{verbatim}
|
||||
This is a hash and the name of the motor.
|
||||
These lines are followed by n lines of:
|
||||
\begin{verbatim}
|
||||
lower position upper
|
||||
\end{verbatim}
|
||||
These are three numbers giving the lower and upper limit for this position in the table
|
||||
and, as the middle value, the target position for this entry.
|
||||
|
||||
|
||||
In order to achieve all this, we need a data structure per table entry:
|
||||
@d tdentry @{
|
||||
typedef struct{
|
||||
double lower, position, upper;
|
||||
int tablePos;
|
||||
}tdEntry, *ptdEntry;
|
||||
@}
|
||||
The fields are the lower and upper limits, the position for this table entry and the
|
||||
number of the entry.
|
||||
|
||||
|
||||
For each motor we need another data structure:
|
||||
@d tdmotor @{
|
||||
typedef struct {
|
||||
char motorName[132];
|
||||
int table;
|
||||
pMotor pMot;
|
||||
}tdMotor, *ptdMotor;
|
||||
@}
|
||||
The fields:
|
||||
\begin{description}
|
||||
\item[motorName] The name of the motor
|
||||
\item[table] A list of tabulated positions in the form of tdEntry
|
||||
\item[pMot] A pointer to the motor data structure.
|
||||
\end{description}
|
||||
|
||||
|
||||
The tabledrive object itself needs a data structure too:
|
||||
@d tdobj @{
|
||||
typedef struct{
|
||||
pObjectDescriptor pDes;
|
||||
pIDrivable pDriv;
|
||||
int motorTable;
|
||||
int tableLength;
|
||||
float targetPosition;
|
||||
float currentPosition;
|
||||
int state;
|
||||
char orientMotor[80];
|
||||
int debug;
|
||||
}TableDrive, *pTableDrive;
|
||||
@}
|
||||
The fields:
|
||||
\begin{description}
|
||||
\item[pDes] The standard SICS object descriptor
|
||||
\item[pDriv] The drivable interface which encapsulates most of the magic of this module.
|
||||
\item[motorTable] A list of tdMotor entries.
|
||||
\item[tableLength] The length of the path of positions.
|
||||
\item[targetPosition] The target position we have to drive to.
|
||||
\item[currentPosition] where we are now.
|
||||
\item[state] A state variable used during driving the path.
|
||||
\item[orientMotor] is the name of the orienting motor, i.e. the one used to determine
|
||||
the position.
|
||||
\end{description}
|
||||
|
||||
|
||||
In terms of an interface, this object implements the drivable interface which has to
|
||||
deal with most of the work. There is just an interpreter interface which allows to
|
||||
configure and query the object.
|
||||
|
||||
@d tdint @{
|
||||
int TableDriveFactory(SConnection *pCon, SicsInterp *pSics, void *pData,
|
||||
int argc, char *argv[]);
|
||||
int TableDriveAction(SConnection *pCon, SicsInterp *pSics, void *pData,
|
||||
int argc, char *argv[]);
|
||||
|
||||
@}
|
||||
|
||||
|
||||
@o tabledrive.h @{
|
||||
/*---------------------------------------------------------------------------
|
||||
SICS object for driving a couple of motors along a tabulated given path.
|
||||
|
||||
copyright: see file COPYRIGHT
|
||||
|
||||
Mark Koennecke, July 2005
|
||||
---------------------------------------------------------------------------*/
|
||||
#ifndef SICSTABLEDRIVE
|
||||
#define SICSTABLEDRIVE
|
||||
#include <sics.h>
|
||||
#include "../motor.h"
|
||||
/*-------------------------------------------------------------------------*/
|
||||
@<tdentry@>
|
||||
/*-------------------------------------------------------------------------*/
|
||||
@<tdmotor@>
|
||||
/*-------------------------------------------------------------------------*/
|
||||
@<tdobj@>
|
||||
/*-------------------------------------------------------------------------*/
|
||||
@<tdint@>
|
||||
|
||||
#endif
|
||||
|
||||
@}
|
||||
|
@ -1235,7 +1235,11 @@ static int RS__MAX_ASYNCH = 20; /* Asynch "ports" 0 - 19 will be allowed */
|
||||
while (Cl_info[i].pending >= 0) {
|
||||
j = Cl_info[i].pending;
|
||||
Cl_info[i].pending = Cl_info[j].pending;
|
||||
if(i != j){
|
||||
i = j;
|
||||
}else {
|
||||
break;
|
||||
}
|
||||
}
|
||||
/*
|
||||
** ... and start up the pending request.
|
||||
|
Reference in New Issue
Block a user