linse/sics
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
9388b5941d288e93ecd61cbf796b5130a6a1b45d
sics/tasdrive.c
Ferdi Franceschini 9b19ddf4e6 SICS-720 Add autofocussing on Taipan using the following energy relations, 
Avfocus = 115 + 2.13 * Ef
Mvfocus = 102.2 + 1.78 * Ei

Ahfocus = 45.68 - (-105.7) * (0.945) ^ Ef
Mhfocus = 184.42 - (60.1) * (0.951) ^ Ei

NOTE: Parameters are set via tasub mono/ana  VB1/VB2/HB1/HB2/HB3
2014-03-10 14:06:22 +11:00

908 lines
26 KiB
C
Raw Blame History

/*---------------------------------------------------------------------
SICS interface to the triple axis drivable motors.
copyright: see file COPYRIGHT
Mark Koennecke, May 2005
In the long run it might be useful to switch all this to the more
general motor list driving code.
Modified to rather use drivables then motors
Mark Koennecke, September 2011
--------------------------------------------------------------------*/
#include <assert.h>
#include "sics.h"
#include "tasdrive.h"
#define ABS(x) (x < 0 ? -(x) : (x))
#define MOTPREC .01
/*------------------- motor indexes in motor data structure ---------
WARNING: have to match tasub.c !!!!!
--------------------------------------------------------------------*/
#define A1 0
#define A2 1
#define MCV 2
#define MCH 3
#define A3 4
#define A4 5
#define SGU 6
#define SGL 7
#define A5 8
#define A6 9
#define ACV 10
#define ACH 11
/*============== Drivable Interface functions =====================*/
static long TASSetValue(void *pData, SConnection * pCon, float value)
{
ptasMot self = (ptasMot) pData;
int qcodes[] = {3,4,5,8}; /* qh,qk,ql, en */
int i;
assert(self);
if (self->code > 5 && self->math->tasMode == ELASTIC) {
SCWrite(pCon, "ERROR: cannot drive this motor in elastic mode",
eLogError);
return HWFault;
}
setTasPar(&self->math->target, self->math->tasMode, self->code, value);
self->math->mustDrive = 1;
for(i = 0; i < 4; i++){
if(self->code == qcodes[i]) {
self->math->mustDriveQ = 1;
}
}
return OKOK;
}
/*----------------------------------------------------------------*/
static int readTASMotAngles(ptasUB self, SConnection * pCon,
ptasAngles ang)
{
int status;
float val, theta;
/*
Monochromator
*/
status = GetDrivablePosition(self->motors[A1], pCon, &val);
if (status == 0) {
return status;
}
theta = val;
status = GetDrivablePosition(self->motors[A2], pCon, &val);
if (status == 0) {
return status;
}
ang->monochromator_two_theta = val;
if (ABS(val / 2. - theta) > .1) {
if(DevExecLevelRunning(pServ->pExecutor,RUNRUN) != 1) {
SCWrite(pCon, "WARNING: theta monochromator not half of two theta",
eWarning);
}
}
/*
Analyzer
*/
if (self->tasMode != ELASTIC) {
status = GetDrivablePosition(self->motors[A5], pCon, &val);
if (status == 0) {
return status;
}
theta = val;
status = GetDrivablePosition(self->motors[A6], pCon, &val);
if (status == 0) {
return status;
}
ang->analyzer_two_theta = val;
if (ABS(val / 2. - theta) > .1 && DevExecLevelRunning(pServ->pExecutor,RUNRUN) != 1) {
SCWrite(pCon, "WARNING: theta analyzer not half of two theta",
eWarning);
}
} else {
ang->analyzer_two_theta = ang->monochromator_two_theta;
}
/*
crystal
*/
status = GetDrivablePosition(self->motors[A3], pCon, &val);
if (status == 0) {
return status;
}
ang->a3 = val;
status = GetDrivablePosition(self->motors[A4], pCon, &val);
if (status == 0) {
return status;
}
ang->sample_two_theta = val;
status = GetDrivablePosition(self->motors[SGU], pCon, &val);
if (status == 0) {
return status;
}
ang->sgu = val;
status = GetDrivablePosition(self->motors[SGL], pCon, &val);
if (status == 0) {
return status;
}
ang->sgl = val;
return 1;
}
/*-------------------------------------------------------------*/
static float TASGetValue(void *pData, SConnection * pCon)
{
ptasMot self = (ptasMot) pData;
int status;
tasAngles angles;
double val;
assert(self);
if (self->math->mustRecalculate == 1) {
status = readTASMotAngles(self->math, pCon, &angles);
if (status != 1) {
return -999.99;
}
status =
calcTasQEPosition(&self->math->machine, angles,
&self->math->current);
if (status < 0) {
SCWrite(pCon, "ERROR: out of memory calculating Q-E variables",
eLogError);
return -999.99;
}
if (self->math->tasMode == ELASTIC) {
self->math->current.kf = self->math->current.ki;
}
self->math->mustRecalculate = 0;
}
val = getTasPar(self->math->current, self->code);
if (self->code > 5 && self->math->tasMode == ELASTIC) {
SCWrite(pCon,
"WARNING: value for this motor is meaningless in elastic mode",
eWarning);
}
return (float) val;
}
/*-----------------------------------------------------------------*/
static float TASQMGetValue(void *pData, SConnection * pCon)
{
ptasMot self = (ptasMot) pData;
int status;
tasAngles angles;
double val;
assert(self);
if (self->math->mustRecalculate == 1) {
status = readTASMotAngles(self->math, pCon, &angles);
if (status != 1) {
return -999.99;
}
status = calcTasPowderPosition(&self->math->machine,
angles, &self->math->current);
if (status < 0) {
SCWrite(pCon, "ERROR: out of memory calculating Q-E variables",
eLogError);
return -999.99;
}
if (self->math->tasMode == ELASTIC) {
self->math->current.kf = self->math->current.ki;
}
self->math->mustRecalculate = 0;
}
val = getTasPar(self->math->current, self->code);
return (float) val;
}
/*---------------------------------------------------------------------------------*/
static int TASCheckLimits(void *pData, float fVal, char *error,
int iErrLen)
{
/*
There is no meaningful implementation here. This gets called when starting the motor.
At that stage not all other values may be known. If the calculation fails,
this will die at status check time.
*/
return 1;
}
/*---------------------------------------------------------------------------------*/
static int TASHalt(void *pData)
{
ptasMot self = (ptasMot) pData;
int i, length = 12;
pIDrivable pDriv = NULL;
assert(self);
/**
* disregard non existing analyzer motors
*/
if (self->math->tasMode == ELASTIC) {
length = 8;
}
for (i = 0; i < length; i++) {
if (self->math->motors[i] != NULL) {
pDriv = GetDrivableInterface(self->math->motors[i]);
pDriv->Halt(self->math->motors[i]);
}
}
return 1;
}
/*--------------------------------------------------------------------------*/
static void writeMotPos(SConnection * pCon, int silent, char *name,
float val, float target)
{
char pBueffel[132];
if (silent != 1) {
snprintf(pBueffel, 131, "Driving %5s from %8.3f to %8.3f",
name, val, target);
SCWrite(pCon, pBueffel, eLog);
}
}
/*--------------------------------------------------------------------------*/
static float getMotorValue(pMotor mot, SConnection * pCon)
{
float val;
GetDrivablePosition(mot, pCon, &val);
return val;
}
/*--------------------- In devexec.c --------------------------------------*/
void InvokeNewTarget(pExeList self, char *name, float target);
/*--------------------------------------------------------------------------*/
static int startTASMotor(pMotor mot, SConnection * pCon, char *name,
double target, int silent, int stopFixed)
{
float val, fixed, precision = MOTPREC;
int status = OKOK;
char buffer[132];
pIDrivable pDriv = NULL;
pDummy dum = NULL;
pDynString mes = NULL;
dum = (pDummy)mot;
val = getMotorValue(mot, pCon);
if(strcmp(dum->pDescriptor->name,"Motor") == 0){
MotorGetPar(mot,"precision",&precision);
MotorGetPar(mot, "fixed", &fixed);
if (ABS(fixed - 1.0) < .1) {
if(stopFixed == 0){
snprintf(buffer, 131, "WARNING: %s is FIXED", name);
SCWrite(pCon, buffer, eLog);
}
return OKOK;
}
}
mot->stopped = 0;
if (ABS(val - target) > precision) {
pDriv = GetDrivableInterface(mot);
status = pDriv->SetValue(mot, pCon, (float) target);
if(status != OKOK){
SCPrintf(pCon,eLog,"ERROR: failed to drive %s to %f", name, target);
}
/*
to force updates on targets
*/
InvokeNewTarget(pServ->pExecutor, name, target);
}
writeMotPos(pCon, silent, name, val, target);
return status;
}
/*---------------------------------------------------------------------------*/
static int startMotors(ptasMot self, tasAngles angles,
SConnection * pCon, int driveQ, int driveTilt)
{
double curve;
int status, silent, stopFixed;
silent = self->math->silent;
stopFixed = self->math->stopFixed;
self->math->mustRecalculate = 1;
/*
monochromator
*/
status = startTASMotor(self->math->motors[A1], pCon, self->math->motname[A1],
angles.monochromator_two_theta / 2., silent, stopFixed);
if (status != OKOK) {
return status;
} else {
self->math->busy[A1] = 1;
}
status = startTASMotor(self->math->motors[A2], pCon, self->math->motname[A2],
angles.monochromator_two_theta, silent,stopFixed);
if (status != OKOK) {
return status;
} else {
self->math->busy[A2] = 1;
}
if (self->math->motors[MCV] != NULL && self->math->autofocus) {
curve = maCalcVerticalCurvature(self->math->machine.monochromator,
angles.monochromator_two_theta, getTasPar(self->math->target, EI), self->math->focusfn);
status = startTASMotor(self->math->motors[MCV], pCon, self->math->motname[MCV],
curve, silent,stopFixed);
if (status != OKOK) {
SCWrite(pCon,"WARNING: monochromator vertical curvature motor failed to start", eLog);
SCSetInterrupt(pCon,eContinue);
} else {
self->math->busy[MCV] = 1;
}
}
if (self->math->motors[MCH] != NULL && self->math->autofocus) {
curve = maCalcHorizontalCurvature(self->math->machine.monochromator,
angles.monochromator_two_theta, getTasPar(self->math->target, EI), self->math->focusfn);
status = startTASMotor(self->math->motors[MCH], pCon, self->math->motname[MCH],
curve, silent,stopFixed);
if (status != OKOK) {
SCWrite(pCon,"WARNING: monochromator horizontal curvature motor failed to start", eLog);
SCSetInterrupt(pCon,eContinue);
} else {
self->math->busy[MCH] = 1;
}
}
/*
analyzer
*/
if (self->math->tasMode != ELASTIC) {
status = startTASMotor(self->math->motors[A5], pCon, self->math->motname[A5],
angles.analyzer_two_theta / 2.0, silent,stopFixed);
if (status != OKOK) {
return status;
} else {
self->math->busy[A5] = 1;
}
status = startTASMotor(self->math->motors[A6], pCon, self->math->motname[A6],
angles.analyzer_two_theta, silent,stopFixed);
if (status != OKOK) {
return status;
} else {
self->math->busy[A6] = 1;
}
if (self->math->motors[ACV] != NULL && self->math->autofocus) {
curve = maCalcVerticalCurvature(self->math->machine.analyzer,
angles.analyzer_two_theta, getTasPar(self->math->target, EF), self->math->focusfn);
status = startTASMotor(self->math->motors[ACV], pCon, self->math->motname[ACV],
curve, silent,stopFixed);
if (status != OKOK) {
SCWrite(pCon,"WARNING: analyzer vertical curvature motor failed to start", eLog);
SCSetInterrupt(pCon,eContinue);
} else {
self->math->busy[ACV] = 1;
}
}
if (self->math->motors[ACH] != NULL && self->math->autofocus) {
curve = maCalcHorizontalCurvature(self->math->machine.analyzer,
angles.analyzer_two_theta, getTasPar(self->math->target, EF), self->math->focusfn);
status = startTASMotor(self->math->motors[ACH], pCon, self->math->motname[ACH],
curve, silent,stopFixed);
if (status != OKOK) {
SCWrite(pCon,"WARNING: analyzer horizontal curvature motor failed to start", eLog);
SCSetInterrupt(pCon,eContinue);
} else {
self->math->busy[ACH] = 1;
}
}
}
if (driveQ == 0) {
return OKOK;
}
/*
crystal
*/
status = startTASMotor(self->math->motors[A3], pCon, self->math->motname[A3],
angles.a3, silent,stopFixed);
if (status != OKOK) {
return status;
} else {
self->math->busy[A3] = 1;
}
status = startTASMotor(self->math->motors[A4], pCon, self->math->motname[A4],
angles.sample_two_theta, silent,stopFixed);
if (status != OKOK) {
return status;
} else {
self->math->busy[A4] = 1;
}
if (driveTilt == 1) {
status = startTASMotor(self->math->motors[SGL], pCon, self->math->motname[SGL],
angles.sgl, silent,stopFixed);
if (status != OKOK) {
return status;
} else {
self->math->busy[SGL] = 1;
}
status = startTASMotor(self->math->motors[SGU], pCon, self->math->motname[SGU],
angles.sgu, silent,stopFixed);
if (status != OKOK) {
return status;
} else {
self->math->busy[SGU] = 1;
}
}
self->math->mustDrive = 0;
return OKOK;
}
/*---------------------------------------------------------------------------*/
static int checkQMotorLimits(ptasMot self, SConnection * pCon,
tasAngles angles, int driveTilt)
{
int status, retVal = 1;
char error[131];
char pBueffel[256];
float val;
pIDrivable pDrivInt = NULL;
MotorGetPar(self->math->motors[A3], "fixed", &val);
if ((int) val != 1) {
pDrivInt = GetDrivableInterface(self->math->motors[A3]);
status = pDrivInt->CheckLimits(self->math->motors[A3],
angles.a3, error,131);
if (status != 1) {
retVal = 0;
snprintf(pBueffel, 256, "ERROR: limit violation an %s: %s", self->math->motname[A3], error);
SCWrite(pCon, pBueffel, eLogError);
}
}
pDrivInt = GetDrivableInterface(self->math->motors[A4]);
status = pDrivInt->CheckLimits(self->math->motors[A4],
angles.
sample_two_theta,
error, 131);
if (status != 1) {
retVal = 0;
snprintf(pBueffel, 256, "ERROR: limit violation an %s: %s", self->math->motname[A4], error);
SCWrite(pCon, pBueffel, eLogError);
}
if (driveTilt == 1) {
pDrivInt = GetDrivableInterface(self->math->motors[SGU]);
status = pDrivInt->CheckLimits(self->math->motors[SGU],
angles.sgu, error,
131);
if (status != 1) {
retVal = 0;
snprintf(pBueffel, 256, "ERROR: limit violation an %s: %s", self->math->motname[SGU], error);
SCWrite(pCon, pBueffel, eLogError);
}
pDrivInt = GetDrivableInterface(self->math->motors[SGL]);
status = pDrivInt->CheckLimits(self->math->motors[SGL],
angles.sgl, error,
131);
if (status != 1) {
retVal = 0;
snprintf(pBueffel, 256, "ERROR: limit violation an %s: %s", self->math->motname[SGU], error);
SCWrite(pCon, pBueffel, eLogError);
}
}
return retVal;
}
/*-----------------------------------------------------------------------------*/
static int calculateAndDrive(ptasMot self, SConnection * pCon)
{
tasAngles angles;
int status, driveQ = 1, driveTilt = 1;
MATRIX scatteringPlaneNormal = NULL;
if (self->math->ubValid == 0) {
SCWrite(pCon, "WARNING: UB matrix invalid", eWarning);
}
status = calcAllTasAngles(&self->math->machine, self->math->target,
&angles);
self->math->mustDrive = 0;
switch (status) {
case ENERGYTOBIG:
SCWrite(pCon, "ERROR: desired energy to big", eError);
return HWFault;
break;
case UBNOMEMORY:
SCWrite(pCon, "ERROR: out of memory calculating angles", eError);
driveQ = 0;
break;
case BADRMATRIX:
SCWrite(pCon, "ERROR: bad crystallographic parameters or bad UB",
eError);
driveQ = 0;
break;
case BADUBORQ:
SCWrite(pCon, "ERROR: bad UB matrix or bad Q-vector", eError);
driveQ = 0;
break;
case TRIANGLENOTCLOSED:
SCWrite(pCon, "ERROR: cannot close scattering triangle", eError);
driveQ = 0;
break;
}
/**
* check out of plane condition and permission
*/
if (self->math->outOfPlaneAllowed != 1) {
scatteringPlaneNormal = calcScatteringPlaneNormal(self->math->r1.qe,
self->math->r2.qe);
if (scatteringPlaneNormal == NULL) {
SCWrite(pCon, "ERROR: unable to calculate scattering plane normal",
eError);
driveQ = 0;
} else {
if (!isInPlane(scatteringPlaneNormal, self->math->target)) {
SCWrite(pCon,
"WARNING: scattering vector is out of plane and you disallowed me to try to drive there",
eWarning);
}
mat_free(scatteringPlaneNormal);
}
driveTilt = 0;
}
if(self->math->mustDriveQ == 0){
driveQ = 0;
}
if (driveQ != 0) {
if (!checkQMotorLimits(self, pCon, angles, driveTilt)) {
driveQ = 0;
}
}
if (driveQ == 0 && self->math->mustDriveQ == 1) {
SCWrite(pCon, "WARNING: NOT driving Q-vector because of errors",
eError);
}
/*
* reset the Q flag
*/
self->math->mustDriveQ = 0;
return startMotors(self, angles, pCon, driveQ, driveTilt);
}
/*-----------------------------------------------------------------------------*/
static int checkMotors(ptasMot self, SConnection * pCon)
{
int i, status, length = 12, count;
int mask[12];
pIDrivable pDrivInt = NULL;
self->math->mustRecalculate = 1;
if (self->math->tasMode == ELASTIC) {
length = 8;
}
memset(mask, 0, 12 * sizeof(int));
for (i = 0; i < length; i++) {
mask[i] = 1;
}
if (self->math->outOfPlaneAllowed == 0) {
mask[SGU] = 0;
mask[SGL] = 0;
}
for (i = 0; i < 12; i++) {
if(mask[i] != 0 && self->math->busy[i] != 0) {
pDrivInt = GetDrivableInterface(self->math->motors[i]);
status = pDrivInt->CheckStatus(self->math->motors[i], pCon);
if(status == HWIdle || status == OKOK || status == HWFault || status == HWPosFault){
self->math->busy[i] = 0;
}
}
}
for(i = 0, count = 0; i < 12; i++){
count += self->math->busy[i];
}
if(count == 0) {
return HWIdle;
} else {
return HWBusy;
}
}
/*------------------------------------------------------------------------------*/
static int TASGetStatus(void *pData, SConnection * pCon)
{
ptasMot self = (ptasMot) pData;
int status;
assert(self);
if (self->math->mustDrive == 1) {
status = calculateAndDrive(self, pCon);
if (status != OKOK) {
TASHalt(pData);
return HWBusy;
} else {
return HWBusy;
}
} else {
return checkMotors(self, pCon);
}
}
/*---------------------------------------------------------------------------*/
static int startQMMotors(ptasMot self, tasAngles angles,
SConnection * pCon)
{
float val;
double curve;
int status, silent, stopFixed;
silent = self->math->silent;
stopFixed = self->math->stopFixed;
/*
monochromator
*/
status = startTASMotor(self->math->motors[A1], pCon, self->math->motname[A1],
angles.monochromator_two_theta / 2., silent,stopFixed);
if (status != OKOK) {
return status;
} else {
self->math->busy[A1] = 1;
}
status = startTASMotor(self->math->motors[A2], pCon, self->math->motname[A2],
angles.monochromator_two_theta, silent,stopFixed);
if (status != OKOK) {
return status;
} else {
self->math->busy[A2] = 1;
}
if (self->math->motors[MCV] != NULL && self->math->autofocus) {
curve = maCalcVerticalCurvature(self->math->machine.monochromator,
angles.monochromator_two_theta, getTasPar(self->math->target, EI), self->math->focusfn);
status = startTASMotor(self->math->motors[MCV], pCon, self->math->motname[MCV],
curve, silent,stopFixed);
if (status != OKOK) {
return status;
} else {
self->math->busy[MCV] = 1;
}
}
if (self->math->motors[MCH] != NULL && self->math->autofocus) {
curve = maCalcHorizontalCurvature(self->math->machine.monochromator,
angles.monochromator_two_theta, getTasPar(self->math->target, EI), self->math->focusfn);
status = startTASMotor(self->math->motors[MCH], pCon, self->math->motname[MCH],
curve, silent,stopFixed);
if (status != OKOK) {
return status;
} else {
self->math->busy[MCH] = 1;
}
}
/*
analyzer
*/
status = startTASMotor(self->math->motors[A5], pCon, self->math->motname[A5],
angles.analyzer_two_theta / 2.0, silent,stopFixed);
if (status != OKOK) {
return status;
} else {
self->math->busy[A5] = 1;
}
status = startTASMotor(self->math->motors[A6], pCon, self->math->motname[A6],
angles.analyzer_two_theta, silent,stopFixed);
if (status != OKOK) {
return status;
} else {
self->math->busy[A6] = 1;
}
if (self->math->motors[ACV] != NULL && self->math->autofocus) {
curve = maCalcVerticalCurvature(self->math->machine.analyzer,
angles.analyzer_two_theta, getTasPar(self->math->target, EF), self->math->focusfn);
status = startTASMotor(self->math->motors[ACV], pCon, self->math->motname[ACV],
curve, silent,stopFixed);
if (status != OKOK) {
return status;
} else {
self->math->busy[ACV] = 1;
}
}
if (self->math->motors[ACH] != NULL && self->math->autofocus) {
curve = maCalcHorizontalCurvature(self->math->machine.analyzer,
angles.analyzer_two_theta, getTasPar(self->math->target, EF), self->math->focusfn);
status = startTASMotor(self->math->motors[ACH], pCon, self->math->motname[ACH],
curve, silent,stopFixed);
if (status != OKOK) {
return status;
} else {
self->math->busy[ACH] = 1;
}
}
/*
crystal
*/
status = startTASMotor(self->math->motors[A4], pCon, self->math->motname[A4],
angles.sample_two_theta, silent,stopFixed);
if (status != OKOK) {
return status;
} else {
self->math->busy[A4] = 1;
}
self->math->mustDrive = 0;
return OKOK;
}
/*-----------------------------------------------------------------------------*/
static int calculateQMAndDrive(ptasMot self, SConnection * pCon)
{
tasAngles angles;
int status;
status = calcTasPowderAngles(&self->math->machine, self->math->target,
&angles);
switch (status) {
case ENERGYTOBIG:
SCWrite(pCon, "ERROR: desired energy to big", eError);
return HWFault;
break;
case TRIANGLENOTCLOSED:
SCWrite(pCon, "ERROR: cannot close scattering triangle", eError);
return HWFault;
break;
default:
return startQMMotors(self, angles, pCon);
}
return HWFault;
}
/*------------------------------------------------------------------------------*/
static int TASQMGetStatus(void *pData, SConnection * pCon)
{
ptasMot self = (ptasMot) pData;
int status;
assert(self);
if (self->math->mustDrive == 1) {
status = calculateQMAndDrive(self, pCon);
if (status != OKOK) {
return HWFault;
} else {
return HWBusy;
}
} else {
return checkMotors(self, pCon);
}
}
/*================== Life and Death =========================================*/
static void KillTasMot(void *pData)
{
ptasMot self = (ptasMot) pData;
if (self == NULL) {
return;
}
if (self->pDes != NULL) {
DeleteDescriptor(self->pDes);
self->pDes = NULL;
}
if (self->pDriv != NULL) {
free(self->pDriv);
self->pDriv = NULL;
}
free(self);
}
/*---------------------------------------------------------------------------*/
static void *GetTasInterface(void *pData, int interfaceID)
{
ptasMot self = (ptasMot) pData;
if (self != NULL && interfaceID == DRIVEID) {
return self->pDriv;
} else {
return NULL;
}
}
/*-------------------------------------------------------------------------*/
int InstallTasMotor(SicsInterp * pSics, ptasUB self, int tasVar,
char *name)
{
ptasMot pNew = NULL;
pNew = (ptasMot) malloc(sizeof(tasMot));
if (!pNew) {
return 0;
}
memset(pNew, 0, sizeof(tasMot));
pNew->pDes = CreateDescriptor("TasMot");
pNew->pDriv = CreateDrivableInterface();
if (!pNew->pDes || !pNew->pDriv) {
free(pNew);
return 0;
}
pNew->pDes->GetInterface = GetTasInterface;
pNew->pDriv->Halt = TASHalt;
pNew->pDriv->CheckLimits = TASCheckLimits;
pNew->pDriv->SetValue = TASSetValue;
pNew->pDriv->CheckStatus = TASGetStatus;
pNew->pDriv->GetValue = TASGetValue;
pNew->math = self;
pNew->code = tasVar;
return AddCommand(pSics, name, TasMot, KillTasMot, pNew);
}
/*-------------------------------------------------------------------------*/
int InstallTasQMMotor(SicsInterp * pSics, ptasUB self)
{
ptasMot pNew = NULL;
pNew = (ptasMot) malloc(sizeof(tasMot));
if (!pNew) {
return 0;
}
memset(pNew, 0, sizeof(tasMot));
pNew->pDes = CreateDescriptor("TasMot");
pNew->pDriv = CreateDrivableInterface();
if (!pNew->pDes || !pNew->pDriv) {
free(pNew);
return 0;
}
pNew->pDes->GetInterface = GetTasInterface;
pNew->pDriv->Halt = TASHalt;
pNew->pDriv->CheckLimits = TASCheckLimits;
pNew->pDriv->SetValue = TASSetValue;
pNew->pDriv->CheckStatus = TASQMGetStatus;
pNew->pDriv->GetValue = TASQMGetValue;
pNew->math = self;
pNew->code = QM;
return AddCommand(pSics, "qm", TasMot, KillTasMot, pNew);
}
/*------------------------------------------------------------------------*/
int TasMot(SConnection * pCon, SicsInterp * pSics, void *pData,
int argc, char *argv[])
{
ptasMot self = (ptasMot) pData;
float value;
char pBueffel[132];
if (argc > 1) {
strtolower(argv[1]);
if (strcmp(argv[1], "target") == 0) {
snprintf(pBueffel, 131, "%s.target = %f",
argv[0], getTasPar(self->math->target, self->code));
SCWrite(pCon, pBueffel, eValue);
return 1;
} else {
snprintf(pBueffel, 131, "ERROR: tasmot does not understand %s",
argv[1]);
SCWrite(pCon, pBueffel, eError);
return 0;
}
}
value = self->pDriv->GetValue(self, pCon);
snprintf(pBueffel, 131, "%s = %f", argv[0], value);
SCWrite(pCon, pBueffel, eValue);
return 1;
}
Reference in New Issue View Git Blame Copy Permalink