378 lines
11 KiB
C
378 lines
11 KiB
C
/*
|
|
@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
|
|
|
|
extern int DMC2280MotionControl;
|
|
|
|
typedef enum {
|
|
ActID,
|
|
Args,
|
|
DevStatus,
|
|
DEBUG
|
|
} submcd;
|
|
|
|
char *subcmdName[] = {"act", "args", "status", "debug", NULL};
|
|
char *validActions[LASTACTION+1] = {"up", "down"};
|
|
/* Each action takes two arguments, an axis label and the jogspeed */
|
|
static char *dfltActionSequence[ACTIONS][SEQLEN] = { {"SH%c","JG%c=%d", "BG%c", NULL}, {"SH%c","JG%c=(-1.0)*(%d)", "BG%c", NULL} };
|
|
static int dfltactArgs[ACTIONS][SEQLEN] = { {1,2,1}, {1,2,1} };
|
|
static char *dfltStatusCheck[ACTIONS][SEQLEN] = {{"TS%c", NULL}, {"TS%c", NULL}};
|
|
static 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_devState[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, i;
|
|
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], "list") == 0) {
|
|
for (i=0; subcmdName[i]; i++) {
|
|
sprintf(msg, "%s", subcmdName[i]);
|
|
SCWrite(pCon,msg,eValue);
|
|
}
|
|
return OKOK;
|
|
} else 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);
|
|
msgType = eValue;
|
|
}
|
|
} 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;
|
|
|
|
if (DMC2280MotionControl != 1) {
|
|
AO_Halt(pData);
|
|
if (DMC2280MotionControl == 0) {
|
|
SCWrite(pCon, "ERROR: Motion control is off", eError);
|
|
return 0;
|
|
} else {
|
|
SCWrite(pCon, "ERROR: Motion control state is unknown", eError);
|
|
return 0;
|
|
}
|
|
}
|
|
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_devState[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_devState[DEVUP] = atoi(argv[4]);
|
|
pNew->cf_devState[DEVDOWN] = atoi(argv[5]);
|
|
pNew->cf_devState[DEVBETWEEN] = 12; // REVSWI | FWDSWI
|
|
pNew->cf_devState[DEVFLT] = 0; // REV & FWD low
|
|
pNew->cf_axis = argv[6][0];
|
|
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;
|
|
}
|