beamstopaction.[ch]
NEW: Implements an action command to drive the quokka beamstops up and down. r2671 | ffr | 2008-08-07 13:19:12 +1000 (Thu, 07 Aug 2008) | 3 lines
This commit is contained in:
Ferdi Franceschini
committed by
Douglas Clowes
Douglas Clowes
parent
d7cea6914c
commit
3398c75eaf
362
site_ansto/beamstopaction.c
Normal file
362
site_ansto/beamstopaction.c
Normal file
@@ -0,0 +1,362 @@
|
||||
/*
|
||||
@file beamstopaction.c
|
||||
@brief Proof of concept action object for Quokka beamstops.
|
||||
*/
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <sics.h>
|
||||
#include <asyncqueue.h>
|
||||
#include <nwatch.h>
|
||||
#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;
|
||||
}
|
||||
Reference in New Issue
Block a user