From b8efd2eed74acea97b508cad4347912faae97cb0 Mon Sep 17 00:00:00 2001 From: koennecke Date: Fri, 23 Sep 2011 07:56:58 +0000 Subject: [PATCH] - Added EIGER A2 motor code --- eigera2.c | 508 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 508 insertions(+) create mode 100644 eigera2.c diff --git a/eigera2.c b/eigera2.c new file mode 100644 index 0000000..e59fbc5 --- /dev/null +++ b/eigera2.c @@ -0,0 +1,508 @@ +/** + * This module implements a drivable for running the eiger A2 motor. + * The problem with that one is that the slits have to follow it. + * In addition the slit width needs to be adjusted too. All this + * is done in this module. + * + * CAVEAT: This module requires for its working the existence + * of 3 motors in SICS: + * a2rot, d2r, d2l + * As this module is very special to EIGER no special checks have + * been implemented on this. SICS will assert and fail if this + * condition is not met. + * + * copyright: see file COPYRIGHT + * + * Mark Koennecke, July 2011 + */ +#include +#include +#include +#define ABS(x) (x < 0 ? -(x) : (x)) +/*------------------------------ states ------------------------*/ +#define IDLE 0 +#define RUNNING 1 +#define WAITSTOP 2 +#define WAITSLIT 3 +/* ---------------------------- motor names --------------------*/ +#define A2 "a2rot" +#define A2R "d2r" +#define A2L "d2l" +/*----------------------------- data structure -----------------*/ +typedef struct { + pObjectDescriptor pDes; + pIDrivable pDriv; + int motorList; + pIDrivable listDriv; + int state; + float a2Target; + float a2wTarget; + float leftSize; + float rightSize; +}EigerA2, *eigera2; +/*---------------------------------------------------------------*/ +static void calcSlitTargets(eigera2 self, float a2, float *d2r, float *d2l) +{ + *d2r = self->rightSize - a2 + self->a2wTarget/2; + *d2l = self->leftSize + a2 + self->a2wTarget/2; +} +/*---------------------------------------------------------------*/ +static float calcA2W(eigera2 self, float d2r, float d2l) +{ + double d2ro, d2lo, a2w; + d2ro = self->rightSize - self->a2Target; + d2lo = self->leftSize + self->a2Target; + a2w = (d2lo - d2l) + (d2ro - d2r); + return ABS(a2w); +} +/*---------------------------------------------------------------*/ +static void *eigera2GetInterface(void *data, int iD){ + eigera2 self = NULL; + + self = (eigera2)data; + if(self != NULL && iD == DRIVEID){ + return self->pDriv; + } else { + return NULL; + } + return NULL; +} +/*---------------------------------------------------------------*/ +static int eigera2SaveStatus(void *data, char *name, FILE *fd) +{ + eigera2 self = NULL; + + self = (eigera2)data; + fprintf(fd,"%s target %f\n", name, self->a2Target); + return 1; +} +/*---------------------------------------------------------------- + This routine can return either OKOK or HWFault when thing + go wrong. However, the return value of Halt is usually ignored! +------------------------------------------------------------------*/ +static int eigera2Halt(void *data) { + eigera2 self = NULL; + + self = (eigera2)data; + if(self->state == RUNNING){ + self->listDriv->Halt(&self->motorList); + self->state = WAITSTOP; + } + return OKOK; +} +/*---------------------------------------------------------------- + This routine can return either 1 or 0. 1 means the position can + be reached, 0 NOT + If 0, error shall contain up to errlen characters of information + about which limit was violated +------------------------------------------------------------------*/ +static int eigera2CheckLimits(void *data, float val, + char *error, int errlen){ + eigera2 self = NULL; + pIDrivable pDriv = NULL; + int status; + float d2r, d2l; + + self = (eigera2)data; + + pDriv = FindDrivable(pServ->pSics,A2); + assert(pDriv != NULL); + status = pDriv->CheckLimits(FindCommandData(pServ->pSics,A2,NULL), val, + error, errlen); + if(status == 0){ + return 0; + } + + calcSlitTargets(self,val,&d2r, &d2l); + + pDriv = FindDrivable(pServ->pSics,A2R); + assert(pDriv != NULL); + status = pDriv->CheckLimits(FindCommandData(pServ->pSics,A2R,NULL), + d2r, + error, errlen); + if(status == 0){ + return 0; + } + + pDriv = FindDrivable(pServ->pSics,A2L); + assert(pDriv != NULL); + status = pDriv->CheckLimits(FindCommandData(pServ->pSics,A2L,NULL), + d2l, + error, errlen); + if(status == 0){ + return 0; + } + + + return 1; +} +/*---------------------------------------------------------------- + This routine can return 0 when a limit problem occurred + OKOK when the motor was successfully started + HWFault when a problem occured starting the device + Possible errors shall be printed to pCon + For real motors, this is supposed to try at least three times + to start the motor in question + val is the value to drive the motor too +------------------------------------------------------------------*/ +static long eigera2SetValue(void *data, SConnection *pCon, float val){ + eigera2 self = NULL; + float d2r, d2l; + + self = (eigera2)data; + + if(self->state != IDLE){ + SCWrite(pCon,"ERROR: Eiger A2 still busy...", eError); + return HWFault; + } + self->a2Target = val; + setNewMotorTarget(self->motorList,A2,self->a2Target); + calcSlitTargets(self,val,&d2r, &d2l); + setNewMotorTarget(self->motorList,A2R,d2r); + setNewMotorTarget(self->motorList,A2L,d2l); + self->state = RUNNING; + return self->listDriv->SetValue(&self->motorList, pCon, 27.); +} +/*---------------------------------------------------------------- + GetValue is supposed to read a motor position + On errors, -99999999.99 is returned and messages printed to pCon +------------------------------------------------------------------*/ +static float eigera2GetValue(void *data, SConnection *pCon){ + eigera2 self = NULL; + pIDrivable pDriv = NULL; + + float val = -99999999.99; + + self = (eigera2)data; + pDriv = FindDrivable(pServ->pSics,A2); + assert(pDriv != NULL); + return pDriv->GetValue(FindCommandData(pServ->pSics,A2,NULL), + pCon); +} +/*---------------------------------------------------------------- + Checks the status of a running motor. Possible return values + HWBusy The motor is still running + OKOK or HWIdle when the motor finished driving + HWFault when a hardware problem ocurred + HWPosFault when the hardware cannot reach a position + Errors are duly to be printed to pCon + For real motors CheckStatus again shall try hard to fix any + issues with the motor +------------------------------------------------------------------*/ +static int eigera2CheckStatus(void *data, SConnection *pCon){ + eigera2 self = NULL; + int status; + pIDrivable pDriv = NULL; + float d2r, d2l; + + self = (eigera2)data; + + switch(self->state){ + case RUNNING: + case WAITSLIT: + status = self->listDriv->CheckStatus(&self->motorList,pCon); + if(status == HWFault || status == HWIdle){ + self->state = IDLE; + } + return status; + case WAITSTOP: + /* + * The point of this is to get the slits into the right + * position after a stop. + */ + status = self->listDriv->CheckStatus(&self->motorList,pCon); + if(status == HWFault || status == HWIdle){ + SCWrite(pCon,"WARNING: driving slits into position after stop", eWarning); + self->state = WAITSLIT; + self->a2Target = eigera2GetValue(data,pCon); + setNewMotorTarget(self->motorList,A2,self->a2Target); + calcSlitTargets(self,self->a2Target,&d2r, &d2l); + setNewMotorTarget(self->motorList,A2R,d2r); + setNewMotorTarget(self->motorList,A2L,d2l); + self->listDriv->SetValue(&self->motorList, pCon, 27.); + return HWBusy; + } + return status; + } + return 1; +} +/*---------------------------------------------------------------- + returns NULL on failure, a new datastructure else +------------------------------------------------------------------*/ +static eigera2 eigera2MakeObject(){ + eigera2 self = NULL; + pIDrivable pDriv = NULL; + float val1, val2; + + self = malloc(sizeof(EigerA2)); + if(self == NULL){ + return NULL; + } + memset(self,0,sizeof(EigerA2)); + self->pDes = CreateDescriptor("EigerA2"); + self->pDriv = CreateDrivableInterface(); + if(self->pDes == NULL || self->pDriv == NULL){ + return NULL; + } + self->motorList = LLDcreate(sizeof(MotControl)); + self->listDriv = makeMotListInterface(); + + pDriv = FindDrivable(pServ->pSics,A2); + assert(pDriv != NULL); + val1 = pDriv->GetValue(FindCommandData(pServ->pSics,A2,NULL), + pServ->dummyCon); + addMotorToList(self->motorList,A2,val1); + self->a2Target = val1; + + pDriv = FindDrivable(pServ->pSics,A2R); + assert(pDriv != NULL); + val1 = pDriv->GetValue(FindCommandData(pServ->pSics,A2R,NULL), + pServ->dummyCon); + + pDriv = FindDrivable(pServ->pSics,A2L); + assert(pDriv != NULL); + val2 = pDriv->GetValue(FindCommandData(pServ->pSics,A2L,NULL), + pServ->dummyCon); + addMotorToList(self->motorList,A2R,val1); + addMotorToList(self->motorList,A2L,val2); + self->leftSize = -101.5; + self->rightSize = -3.5; + self->a2wTarget = calcA2W(self,val1,val2); + + self->state = IDLE; + + self->pDes->GetInterface = eigera2GetInterface; + self->pDes->SaveStatus = eigera2SaveStatus; + self->pDriv->Halt = eigera2Halt; + self->pDriv->CheckLimits = eigera2CheckLimits; + self->pDriv->SetValue = eigera2SetValue; + self->pDriv->CheckStatus = eigera2CheckStatus; + self->pDriv->GetValue = eigera2GetValue; + + + return self; +} +/*====================================================== + * The A2W Drivable + ======================================================*/ +typedef struct { + pObjectDescriptor pDes; + pIDrivable pDriv; + eigera2 eiger; +}EigerA2W, *eigera2w; + +/*------------------------------------------------------*/ +static void *eigera2wGetInterface(void *data, int iD){ + eigera2w self = NULL; + + self = (eigera2w)data; + if(self != NULL && iD == DRIVEID){ + return self->pDriv; + } else { + return NULL; + } + return NULL; +} +/*---------------------------------------------------------------*/ +static int eigera2wSaveStatus(void *data, char *name, FILE *fd) +{ + eigera2w self = NULL; + + self = (eigera2w)data; + fprintf(fd,"%s target %f\n", name, self->eiger->a2wTarget); + return 1; +} +/*---------------------------------------------------------------- + This routine can return either OKOK or HWFault when thing + go wrong. However, the return value of Halt is usually ignored! +------------------------------------------------------------------*/ +static int eigera2wHalt(void *data) { + eigera2w self = NULL; + + self = (eigera2w)data; + return self->eiger->pDriv->Halt(self->eiger); +} +/*---------------------------------------------------------------- + This routine can return either 1 or 0. 1 means the position can + be reached, 0 NOT + If 0, error shall contain up to errlen characters of information + about which limit was violated +------------------------------------------------------------------*/ +static int eigera2wCheckLimits(void *data, float val, + char *error, int errlen){ + eigera2w self = NULL; + float vala2; + + self = (eigera2w)data; + self->eiger->a2wTarget = val; + vala2 = eigera2GetValue(self->eiger,pServ->dummyCon); + return self->eiger->pDriv->CheckLimits(self->eiger,vala2,error,errlen); +} +/*---------------------------------------------------------------- + This routine can return 0 when a limit problem occurred + OKOK when the motor was successfully started + HWFault when a problem occured starting the device + Possible errors shall be printed to pCon + For real motors, this is supposed to try at least three times + to start the motor in question + val is the value to drive the motor too +------------------------------------------------------------------*/ +static long eigera2wSetValue(void *data, SConnection *pCon, float val){ + eigera2w self = NULL; + float vala2; + + self = (eigera2w)data; + self->eiger->a2wTarget = val; + vala2 = eigera2GetValue(self->eiger,pCon); + return self->eiger->pDriv->SetValue(self->eiger,pCon,vala2); +} +/*---------------------------------------------------------------- + Checks the status of a running motor. Possible return values + HWBusy The motor is still running + OKOK or HWIdle when the motor finished driving + HWFault when a hardware problem ocurred + HWPosFault when the hardware cannot reach a position + Errors are duly to be printed to pCon + For real motors CheckStatus again shall try hard to fix any + issues with the motor +------------------------------------------------------------------*/ +static int eigera2wCheckStatus(void *data, SConnection *pCon){ + eigera2w self = NULL; + + self = (eigera2w)data; + return self->eiger->pDriv->CheckStatus(self->eiger,pCon); +} +/*---------------------------------------------------------------- + GetValue is supposed to read a motor position + On errors, -99999999.99 is returned and messages printed to pCon +------------------------------------------------------------------*/ +static float eigera2wGetValue(void *data, SConnection *pCon){ + eigera2w self = NULL; + float valr, vall; + pIDrivable pDriv = NULL; + + self = (eigera2w)data; + pDriv = FindDrivable(pServ->pSics,A2R); + assert(pDriv != NULL); + valr = pDriv->GetValue(FindCommandData(pServ->pSics,A2R,NULL), + pCon); + + pDriv = FindDrivable(pServ->pSics,A2L); + assert(pDriv != NULL); + vall = pDriv->GetValue(FindCommandData(pServ->pSics,A2L,NULL), + pCon); + + return calcA2W(self->eiger,valr, vall); +} +/*---------------------------------------------------------------- + returns NULL on failure, a new datastructure else +------------------------------------------------------------------*/ +static eigera2w eigera2wMakeObject(eigera2 eiger){ + eigera2w self = NULL; + + self = malloc(sizeof(EigerA2W)); + if(self == NULL){ + return NULL; + } + memset(self,0,sizeof(EigerA2W)); + self->pDes = CreateDescriptor("EigerA2W"); + self->pDriv = CreateDrivableInterface(); + if(self->pDes == NULL || self->pDriv == NULL){ + return NULL; + } + self->eiger = eiger; + + self->pDes->GetInterface = eigera2wGetInterface; + self->pDes->SaveStatus = eigera2wSaveStatus; + self->pDriv->Halt = eigera2wHalt; + self->pDriv->CheckLimits = eigera2wCheckLimits; + self->pDriv->SetValue = eigera2wSetValue; + self->pDriv->CheckStatus = eigera2wCheckStatus; + self->pDriv->GetValue = eigera2wGetValue; + + return self; +} +/*---------------------------------------------------------------*/ +int DrivableAction(SConnection * pCon, SicsInterp * pSics, void *pData, + int argc, char *argv[]) +{ + pIDrivable pDriv = NULL; + pDummy pDum; + float value; + char pBuffer[132]; + eigera2w self= NULL; + eigera2 selfe = NULL; + + + assert(pData != NULL); + + if(argc > 1){ + strtolower(argv[1]); + if(strcmp(argv[1],"target") == 0){ + if(strcmp(argv[0],"a2w") == 0 && argc > 2){ + self = (eigera2w)pData; + self->eiger->a2wTarget = atof(argv[2]); + SCSendOK(pCon); + return 1; + } else if (strcmp(argv[0],"a2") == 0 && argc > 2) { + selfe = (eigera2)pData; + selfe->a2Target = atof(argv[2]); + SCSendOK(pCon); + return 1; + } + } + } + pDum = (pDummy)pData; + pDriv = (pIDrivable) pDum->pDescriptor->GetInterface(pDum,DRIVEID); + value = pDriv->GetValue(pDum, pCon); + if (value < -9000.) { + snprintf(pBuffer, 131, "ERROR: failed to read %s", argv[0]); + SCWrite(pCon, pBuffer, eError); + return 0; + } + snprintf(pBuffer, 131, "%s = %f", argv[0], value); + SCWrite(pCon, pBuffer, eValue); + return 1; +} +/*---------------------------------------------------------------*/ +int InitEiger(SConnection *pCon, SicsInterp *pSics, + void *pData, int argc, char *argv[]) +{ + pIDrivable pDriv = NULL; + eigera2 a2 = NULL; + eigera2w a2w = NULL; + + pDriv = FindDrivable(pSics,A2); + if(pDriv == NULL){ + SCWrite(pCon,"ERROR: A2 motor not found", eError); + return 0; + } + pDriv = FindDrivable(pSics,A2R); + if(pDriv == NULL){ + SCWrite(pCon,"ERROR: A2R motor not found", eError); + return 0; + } + pDriv = FindDrivable(pSics,A2L); + if(pDriv == NULL){ + SCWrite(pCon,"ERROR: A2L motor not found", eError); + return 0; + } + + a2 = eigera2MakeObject(); + a2w = eigera2wMakeObject(a2); + if(a2 == NULL || a2w == NULL){ + SCWrite(pCon,"ERROR: out of memory creating a2, a2w", eError); + return 0; + } + if(argc > 1){ + a2->leftSize = atof(argv[1]); + } + if(argc > 2){ + a2->rightSize = atof(argv[2]); + } + + AddCommand(pSics, "a2", DrivableAction, NULL, a2); + AddCommand(pSics, "a2w", DrivableAction, NULL, a2w); + + return 1; + }