diff --git a/site_ansto/beamstopaction.c b/site_ansto/beamstopaction.c new file mode 100644 index 00000000..9901a19b --- /dev/null +++ b/site_ansto/beamstopaction.c @@ -0,0 +1,362 @@ +/* +@file beamstopaction.c +@brief Proof of concept action object for Quokka beamstops. +*/ +#include +#include +#include +#include +#include +#include "beamstopaction.h" + +#define NOACTION -1 +#define DEFAULTACTION 0 +#define LASTACTION 1 +#define AO_CMDLEN 256 +#define SEQLEN 16 +#define ACTIONS 32 + +typedef enum { + ActID, + Args, + DevStatus, + DEBUG +} submcd; + +char *subcmdName[] = {"act", "args", "status", "debug"}; +char *validActions[LASTACTION+1] = {"up", "down"}; +/* Each action takes two arguments, an axis label and the jogspeed */ +char *dfltActionSequence[ACTIONS][SEQLEN] = { {"SH%c","JG%c=%d", "BG%c", NULL}, {"SH%c","JG%c=(-1.0)*(%d)", "BG%c", NULL} }; +int dfltactArgs[ACTIONS][SEQLEN] = { {1,2,1}, {1,2,1} }; +char *dfltStatusCheck[ACTIONS][SEQLEN] = {{"TS%c", NULL}, {"TS%c", NULL}}; +char *dfltDevStatusMsg[] = {"up", "down", "inbetween", "FAULT: Are the limit switches connected?"}; +int numactions = 2; +#define NUM_DEVSTATES 4 +// Mask TS reply to get limswitch status +#define LIMSWI_MASK 12 +int dfltDevStatMask[NUM_DEVSTATES]; +enum devicestatus {DEVUP, DEVDOWN, DEVBETWEEN, DEVFLT}; + +typedef struct __Action { + pObjectDescriptor pDes; + pIDrivable pDrivInt; + pAsyncUnit asyncUnit; + char objname[64]; + int actionID; + char *(*actionSequence)[ACTIONS][SEQLEN]; + int (*actionArgs)[ACTIONS][SEQLEN]; + char *(*statusCheck)[ACTIONS][SEQLEN]; + int devStatusID; + char **devStatusMsg; + char args[256]; + char cf_axis; + int cf_jogspeed; + int cf_mask[NUM_DEVSTATES]; + int status; + float fTarget; + int debug; +} Action, *pAction; + +void debugmsg(SConnection *pCon, pAction self, char *msg) { + if (self->debug) + SCWrite(pCon, msg, eValue); +} + +/** + * @brief Get the action ID corresponding to the given action name + * + * @par actnm action name + * @par actlist array of actions ordered by action ID + * @par numact number of available actions + * @return action ID or -1 (not found) + */ +int ActNm2ID(char *actnm, char *actlist[], int numact) { + int id; + for (id=0; id < numact; id++) { + if (strcasecmp(actlist[id], actnm) == 0) { + return id; + } + } + return -1; +} + +static void *AO_GetInterface(void *pData, int iID) { + pAction self = NULL; + + self = (pAction)pData; + assert(self); + + if(iID == DRIVEID){ + return self->pDrivInt; + } + return NULL; +} + +int AO_Wrapper(SConnection *pCon, SicsInterp *pSics, void *pData, int argc, char *argv[]) { + char msg[128]="No message", output[256]; + int actID, status; + OutCode msgType; + pAction self = (pAction) pData; + + if (argc == 1) { + sprintf(msg, "%d", self->status); + msgType = eValue; + } else if (argc == 2) { /* GET */ + if (strcasecmp(argv[1], subcmdName[ActID]) == 0) { + sprintf(msg, "%s", validActions[self->actionID]); + msgType = eValue; + } else if (strcasecmp(argv[1], subcmdName[Args]) == 0) { + sprintf(msg, "%s", self->args); + msgType = eValue; + } else if (strcasecmp(argv[1], subcmdName[DevStatus]) == 0) { + if ((status = AO_CheckStatus(pData, pCon)) == HWFault) { + return 0; + } + sprintf(msg, "%s", self->devStatusMsg[self->devStatusID]); + msgType = eValue; + } else if (strcasecmp(argv[1], subcmdName[DEBUG]) == 0) { + sprintf(msg, "%d", self->debug); + } + } else if (argc == 3) { /* SET */ + if (strcasecmp(argv[1], subcmdName[ActID]) == 0) { + if ( (actID = ActNm2ID(argv[2], validActions, numactions)) != -1) { + sprintf(msg, "%s setting %s = %s", argv[0], argv[1], argv[2]); + msgType = -1; + self->actionID = actID; + } + } else if (strcasecmp(argv[1], subcmdName[Args]) == 0) { + // TODO Check that args is a comma separated list a1,a2,a3 and type-check + strcpy(self->args, argv[2]); + sprintf(msg, "%s setting %s = %s", argv[0], argv[1], argv[2]); + msgType = -1; + } else if (strcasecmp(argv[1], subcmdName[DEBUG]) == 0) { + self->debug = atoi(argv[2]); + } + } + /* TODO allow plain output */ + switch (msgType) { + case eValue: + sprintf(output, "%s = %s", argv[0], msg); + SCWrite(pCon, output, eValue); + break; + default: + strcpy(output, msg); + debugmsg(pCon, self, output); + } + return OKOK; + +} + + +static int AO_Halt(void *pData) { + pAction self = (pAction) pData; + char cmd[AO_CMDLEN], reply[AO_CMDLEN]; + int comStatus, cmd_len; + + sprintf(cmd, "ST%c", self->cf_axis); + cmd_len = AO_CMDLEN; + comStatus = AsyncUnitTransact(self->asyncUnit, cmd, strlen(cmd), reply, &cmd_len); + sprintf(cmd, "MO%c", self->cf_axis); + cmd_len = AO_CMDLEN; + comStatus = AsyncUnitTransact(self->asyncUnit, cmd, strlen(cmd), reply, &cmd_len); + return 1; +} + +static int AO_CheckLimits(void *pData, float fVal, char *pError, int iErrLen) { + return 1; +} +static long AO_StartAction(void *pData, SConnection *pCon, float fVal) { +/* +if fVal == start then + get actionsequence which matches the actionID + send sequence + set currentAction = actionID, used by status +*/ + char msg[128]; + char cmd[AO_CMDLEN], reply[AO_CMDLEN]; + int i, cmd_len, comStatus; + + pAction self = (pAction) pData; + if (self->actionID < NOACTION || self->actionID > LASTACTION) { + sprintf(msg, "ERROR: Programmer error, invalid action ID %d for %s. Alert a SICS programmer", self->actionID, self->objname); + SCWrite(pCon, msg, eError); + return 0; + } + if (self->actionID == NOACTION) { + sprintf(msg, "ERROR: You must select an action before driving %s", self->objname); + SCWrite(pCon, msg, eError); + return 0; + } + for (i = 0; (*self->actionSequence)[self->actionID][i]; i++) { + switch ( (*self->actionArgs)[self->actionID][i]) { + case 1: + sprintf(cmd, (*self->actionSequence)[self->actionID][i], self->cf_axis); + break; + case 2: + sprintf(cmd, (*self->actionSequence)[self->actionID][i], self->cf_axis, self->cf_jogspeed); + break; + } + debugmsg(pCon, self, cmd); + cmd_len = AO_CMDLEN; + comStatus = AsyncUnitTransact(self->asyncUnit, cmd, strlen(cmd), reply, &cmd_len); + } + debugmsg(pCon, self, "Start action"); + self->fTarget = fVal; + return OKOK; +} +static int AO_CheckStatus(void *pData, SConnection *pCon) { + pAction self =(pAction) pData; + char cmd[AO_CMDLEN], reply[AO_CMDLEN]= " 0000000113 0007831721 0000000047 0000000000 0000000001\n:"; + int devStatus, sicsStatus, comStatus, limswi, cmd_len; + int iSteps, iCounts, iFlags, iBG, iStopCode; + char msg[128]; + + sprintf(cmd, "MG {F10.0} _TD%c,_TP%c,_TS%c,_BG%c,_SC%c", self->cf_axis,self->cf_axis,self->cf_axis,self->cf_axis,self->cf_axis); + debugmsg(pCon, self, cmd); +// comStatus = 1; +cmd_len = AO_CMDLEN; + comStatus = AsyncUnitTransact(self->asyncUnit, cmd, strlen(cmd), reply, &cmd_len); + if (comStatus != 1) { + SCWrite(pCon, "Command failed", eError); + sicsStatus = HWFault; + } else { + debugmsg(pCon, self, reply); + if ((reply[0] != ' ') & (reply[0] != '-')) { + SCWrite(pCon, "ERROR: Invalid status reply from device", eError); + sicsStatus = HWFault; + } else { + if ( 5 != sscanf(reply, "%d %d %d %d %d", &iSteps, &iCounts, &iFlags, &iBG, &iStopCode) ) { + SCWrite(pCon, "Failed to scan reply", eError); + sicsStatus = HWFault; + } else { + limswi = LIMSWI_MASK & iFlags; + for (devStatus = 0; devStatus < NUM_DEVSTATES; devStatus++) { + if (self->cf_mask[devStatus] == limswi) { + self->devStatusID = devStatus; + break; + } + } + if (iBG == 1) { + debugmsg(pCon, self, "BUSY"); + sicsStatus = HWBusy; + } else { + debugmsg(pCon, self, "IDLE"); + if (self->status == HWBusy) { + AO_Halt(pData); + } + sicsStatus = HWIdle; + } + } + } + } + self->status = sicsStatus; + return sicsStatus; +} + +/* @brief Check the device status and return the SICS status code +*/ +static float AO_GetValue(void *pData, SConnection *pCon) { + pAction self = (pAction) pData; + int status; + status = AO_CheckStatus(pData, pCon); + return status; +} + +static void AO_Notify(void *context, int event) { + return; +} +pAction AO_Create(SConnection *pCon, const char *asyncName) { + pAction self = NULL; + char msg[128]; + + self = (pAction) malloc(sizeof(Action)); + if (self == NULL) + return NULL; + + if (!AsyncUnitCreate(asyncName, &self->asyncUnit)) { + sprintf(msg, "ERROR: Failed to find asyncqueue %s", asyncName); + SCWrite(pCon, msg, eError); + free(self); + return NULL; + } + AsyncUnitSetNotify(self->asyncUnit, self, AO_Notify); + + self->pDrivInt = CreateDrivableInterface(); + if(!self->pDrivInt) { + SCWrite(pCon, "ERROR: Failed to create drivable interface", eError); + free(self); + return NULL; + } + self->pDrivInt->Halt = AO_Halt; + self->pDrivInt->CheckLimits = AO_CheckLimits; + self->pDrivInt->SetValue = AO_StartAction; + self->pDrivInt->CheckStatus = AO_CheckStatus; + self->pDrivInt->GetValue = AO_GetValue; + self->pDes = CreateDescriptor("ActionObject"); + self->pDes->GetInterface = AO_GetInterface; + return self; +} + +void AO_Kill(void *pData) { + pAction self = (pAction) pData; + if (self) { + if (self->pDes) { + free(self->pDes); + } + if (self->pDrivInt) { + free(self->pDrivInt); + } + free(self); + } +} + +/* MakeActionObject objname asyncName jogspeed upmask downmask axis */ +int ActionObjectFactory(SConnection *pCon, SicsInterp *pSics, void *pData, int argc, char *argv[]) +{ + char msg[128], pError[128]; + int i, iRet; + pAction pNew; + + if (argc < 7) { + SCWrite(pCon, "You must provide the following four arguments, objname asyncName jogspeed upmask axis", eError); + return 0; + } + printf("Create the %s action object on asyncqueue %s\n", argv[1], argv[2]); + + + pNew = AO_Create(pCon, argv[2]); + if (pNew == NULL) { + sprintf(pError, "ERROR: Failed to create %s", argv[1]); + SCWrite(pCon,pError,eError); + return 0; + } + strcpy(pNew->objname,argv[1]); + pNew->actionID = DEFAULTACTION; + pNew->debug = 0; + (pNew->args)[0] = '\0'; + pNew->actionSequence = dfltActionSequence; + pNew->actionArgs = dfltactArgs; + pNew->statusCheck = dfltStatusCheck; + pNew->devStatusMsg = dfltDevStatusMsg; + pNew->cf_jogspeed = atoi(argv[3]); + pNew->cf_mask[DEVUP] = atoi(argv[4]); + pNew->cf_mask[DEVDOWN] = atoi(argv[5]); + pNew->cf_mask[DEVBETWEEN] = 12; // REVSWI | FWDSWI + pNew->cf_mask[DEVFLT] = 0; // REV & FWD low + pNew->cf_axis = argv[6][0]; + printf("axis = %c\n", pNew->cf_axis); + printf("jogspeed = %d\n", pNew->cf_jogspeed); + for (i=0; i< NUM_DEVSTATES; i++) + printf("mask[%d] = %d\n",i, pNew->cf_mask[i]); + iRet = AddCommand(pSics, argv[1], AO_Wrapper, AO_Kill, pNew); + if(!iRet) { + sprintf(pError,"ERROR: duplicate command %s not created [%d]", argv[1], iRet); + SCWrite(pCon,pError,eError); + AO_Kill(pNew); + return 0; + } + SCSendOK(pCon); + printf("Created action object %s\n", pNew->objname); + AO_CheckStatus(pNew, pCon); + return 1; +} diff --git a/site_ansto/beamstopaction.h b/site_ansto/beamstopaction.h new file mode 100644 index 00000000..c74bffa4 --- /dev/null +++ b/site_ansto/beamstopaction.h @@ -0,0 +1,5 @@ +#ifndef BEAMSTOPACTION +#define BEAMSTOPACTION +int ActionObjectFactory(SConnection *pCon, SicsInterp *pSics, void *pData, int argc, char *argv[]); +static int AO_CheckStatus(void *pData, SConnection *pCon); +#endif