- Added EIGER A2 motor code
This commit is contained in:
koennecke
508
eigera2.c
Normal file
508
eigera2.c
Normal file
@ -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 <sics.h>
|
||||||
|
#include <motorlist.h>
|
||||||
|
#include <lld.h>
|
||||||
|
#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;
|
||||||
|
}
|
||||||
Reference in New Issue
Block a user