- First working version of the triple axis UB matrix code
This commit is contained in:
koennecke
644
tasdrive.c
Normal file
644
tasdrive.c
Normal file
@@ -0,0 +1,644 @@
|
||||
/*---------------------------------------------------------------------
|
||||
SICS interface to the triple axis drivable motors.
|
||||
|
||||
copyright: see file COPYRIGHT
|
||||
|
||||
Mark Koennecke, May 2005
|
||||
--------------------------------------------------------------------*/
|
||||
#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 int TASSetValue(void *pData, SConnection *pCon,
|
||||
float value){
|
||||
ptasMot self = (ptasMot)pData;
|
||||
assert(self);
|
||||
|
||||
setTasPar(&self->math->target,self->math->tasMode,self->code,value);
|
||||
self->math->mustDrive = 1;
|
||||
return OKOK;
|
||||
}
|
||||
/*----------------------------------------------------------------*/
|
||||
static int readTASMotAngles(ptasUB self, SConnection *pCon,
|
||||
ptasAngles ang){
|
||||
int status;
|
||||
float val, theta;
|
||||
|
||||
/*
|
||||
Monochromator
|
||||
*/
|
||||
status = MotorGetSoftPosition(self->motors[A1],pCon,&val);
|
||||
if(status == 0){
|
||||
return status;
|
||||
}
|
||||
theta = val;
|
||||
status = MotorGetSoftPosition(self->motors[A2],pCon,&val);
|
||||
if(status == 0){
|
||||
return status;
|
||||
}
|
||||
ang->monochromator_two_theta = val;
|
||||
if(ABS(val/2. - theta) > .1){
|
||||
SCWrite(pCon,"WARNING: theta monochromator not half of two theta",eWarning);
|
||||
}
|
||||
|
||||
/*
|
||||
Analyzer
|
||||
*/
|
||||
status = MotorGetSoftPosition(self->motors[A5],pCon,&val);
|
||||
if(status == 0){
|
||||
return status;
|
||||
}
|
||||
theta = val;
|
||||
status = MotorGetSoftPosition(self->motors[A6],pCon,&val);
|
||||
if(status == 0){
|
||||
return status;
|
||||
}
|
||||
ang->analyzer_two_theta = val;
|
||||
if(ABS(val/2. - theta) > .1){
|
||||
SCWrite(pCon,"WARNING: theta analyzer not half of two theta",eWarning);
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
crystal
|
||||
*/
|
||||
status = MotorGetSoftPosition(self->motors[A3],pCon,&val);
|
||||
if(status == 0){
|
||||
return status;
|
||||
}
|
||||
ang->a3 = val;
|
||||
status = MotorGetSoftPosition(self->motors[A4],pCon,&val);
|
||||
if(status == 0){
|
||||
return status;
|
||||
}
|
||||
ang->sample_two_theta = val;
|
||||
status = MotorGetSoftPosition(self->motors[SGU],pCon,&val);
|
||||
if(status == 0){
|
||||
return status;
|
||||
}
|
||||
ang->sgu = val;
|
||||
status = MotorGetSoftPosition(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",eError);
|
||||
return -999.99;
|
||||
}
|
||||
self->math->mustRecalculate = 0;
|
||||
}
|
||||
val = getTasPar(self->math->current,self->code);
|
||||
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",eError);
|
||||
return -999.99;
|
||||
}
|
||||
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;
|
||||
|
||||
assert(self);
|
||||
for(i = 0; i < 12; i++){
|
||||
if(self->math->motors[i] != NULL){
|
||||
self->math->motors[i]->pDrivInt->Halt(self->math->motors[i]);
|
||||
}
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
/*---------------------------------------------------------------------------*/
|
||||
static int startMotors(ptasMot self, tasAngles angles,
|
||||
SConnection *pCon){
|
||||
float val;
|
||||
double curve;
|
||||
int status;
|
||||
|
||||
/*
|
||||
monochromator
|
||||
*/
|
||||
val = self->math->motors[A1]->pDrivInt->GetValue(self->math->motors[A1],pCon);
|
||||
if(ABS(val - angles.monochromator_two_theta/2.) > MOTPREC){
|
||||
status = self->math->motors[A1]->pDrivInt->SetValue(self->math->motors[A1],
|
||||
pCon,
|
||||
angles.monochromator_two_theta/2.);
|
||||
if(status != OKOK){
|
||||
return status;
|
||||
}
|
||||
}
|
||||
val = self->math->motors[A2]->pDrivInt->GetValue(self->math->motors[A2],pCon);
|
||||
if(ABS(val - angles.monochromator_two_theta) > MOTPREC){
|
||||
status = self->math->motors[A2]->pDrivInt->SetValue(self->math->motors[A2],
|
||||
pCon,
|
||||
angles.monochromator_two_theta);
|
||||
if(status != OKOK){
|
||||
return status;
|
||||
}
|
||||
}
|
||||
if(self->math->motors[MCV] != NULL){
|
||||
curve = maCalcVerticalCurvature(self->math->machine.monochromator,
|
||||
angles.monochromator_two_theta);
|
||||
val = self->math->motors[MCV]->pDrivInt->GetValue(self->math->motors[MCV],pCon);
|
||||
if(ABS(val - curve) > MOTPREC){
|
||||
status = self->math->motors[MCV]->pDrivInt->SetValue(self->math->motors[MCV],
|
||||
pCon,
|
||||
curve);
|
||||
if(status != OKOK){
|
||||
return status;
|
||||
}
|
||||
}
|
||||
}
|
||||
if(self->math->motors[MCH] != NULL){
|
||||
curve = maCalcHorizontalCurvature(self->math->machine.monochromator,
|
||||
angles.monochromator_two_theta);
|
||||
val = self->math->motors[MCH]->pDrivInt->GetValue(self->math->motors[MCH],pCon);
|
||||
if(ABS(val - curve) > MOTPREC){
|
||||
status = self->math->motors[MCH]->pDrivInt->SetValue(self->math->motors[MCH],
|
||||
pCon,
|
||||
curve);
|
||||
if(status != OKOK){
|
||||
return status;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
analyzer
|
||||
*/
|
||||
val = self->math->motors[A5]->pDrivInt->GetValue(self->math->motors[A5],pCon);
|
||||
if(ABS(val - angles.analyzer_two_theta/2.) > MOTPREC){
|
||||
status = self->math->motors[A5]->pDrivInt->SetValue(self->math->motors[A5],
|
||||
pCon,
|
||||
angles.analyzer_two_theta/2.);
|
||||
if(status != OKOK){
|
||||
return status;
|
||||
}
|
||||
}
|
||||
val = self->math->motors[A6]->pDrivInt->GetValue(self->math->motors[A6],pCon);
|
||||
if(ABS(val - angles.analyzer_two_theta) > MOTPREC){
|
||||
status = self->math->motors[A6]->pDrivInt->SetValue(self->math->motors[A6],
|
||||
pCon,
|
||||
angles.analyzer_two_theta);
|
||||
if(status != OKOK){
|
||||
return status;
|
||||
}
|
||||
}
|
||||
if(self->math->motors[ACV] != NULL){
|
||||
curve = maCalcVerticalCurvature(self->math->machine.analyzer,
|
||||
angles.analyzer_two_theta);
|
||||
val = self->math->motors[ACV]->pDrivInt->GetValue(self->math->motors[ACV],pCon);
|
||||
if(ABS(val - curve) > MOTPREC){
|
||||
status = self->math->motors[ACV]->pDrivInt->SetValue(self->math->motors[ACV],
|
||||
pCon,
|
||||
curve);
|
||||
if(status != OKOK){
|
||||
return status;
|
||||
}
|
||||
}
|
||||
}
|
||||
if(self->math->motors[ACH] != NULL){
|
||||
curve = maCalcHorizontalCurvature(self->math->machine.analyzer,
|
||||
angles.analyzer_two_theta);
|
||||
val = self->math->motors[ACH]->pDrivInt->GetValue(self->math->motors[ACH],pCon);
|
||||
if(ABS(val - curve) > MOTPREC){
|
||||
status = self->math->motors[ACH]->pDrivInt->SetValue(self->math->motors[ACH],
|
||||
pCon,
|
||||
curve);
|
||||
if(status != OKOK){
|
||||
return status;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
crystal
|
||||
*/
|
||||
val = self->math->motors[A3]->pDrivInt->GetValue(self->math->motors[A3],pCon);
|
||||
if(ABS(val - angles.a3) > MOTPREC){
|
||||
status = self->math->motors[A3]->pDrivInt->SetValue(self->math->motors[A3],
|
||||
pCon,
|
||||
angles.a3);
|
||||
if(status != OKOK){
|
||||
return status;
|
||||
}
|
||||
}
|
||||
val = self->math->motors[A4]->pDrivInt->GetValue(self->math->motors[A4],pCon);
|
||||
if(ABS(val - angles.sample_two_theta) > MOTPREC){
|
||||
status = self->math->motors[A4]->pDrivInt->SetValue(self->math->motors[A4],
|
||||
pCon,
|
||||
angles.sample_two_theta);
|
||||
if(status != OKOK){
|
||||
return status;
|
||||
}
|
||||
}
|
||||
val = self->math->motors[SGL]->pDrivInt->GetValue(self->math->motors[SGL],pCon);
|
||||
if(ABS(val - angles.sgl) > MOTPREC){
|
||||
status = self->math->motors[SGL]->pDrivInt->SetValue(self->math->motors[SGL],
|
||||
pCon,
|
||||
angles.sgl);
|
||||
if(status != OKOK){
|
||||
return status;
|
||||
}
|
||||
}
|
||||
val = self->math->motors[SGU]->pDrivInt->GetValue(self->math->motors[SGU],pCon);
|
||||
if(ABS(val - angles.sgu) > MOTPREC){
|
||||
status = self->math->motors[SGU]->pDrivInt->SetValue(self->math->motors[SGU],
|
||||
pCon,
|
||||
angles.sgu);
|
||||
if(status != OKOK){
|
||||
return status;
|
||||
}
|
||||
}
|
||||
self->math->mustDrive = 0;
|
||||
return OKOK;
|
||||
}
|
||||
/*-----------------------------------------------------------------------------*/
|
||||
static int calculateAndDrive(ptasMot self, SConnection *pCon){
|
||||
tasAngles angles;
|
||||
int status;
|
||||
|
||||
status = calcAllTasAngles(&self->math->machine, self->math->target,
|
||||
&angles);
|
||||
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);
|
||||
return HWFault;
|
||||
break;
|
||||
case BADRMATRIX:
|
||||
SCWrite(pCon,"ERROR: bad crystallographic parameters or bad UB",eError);
|
||||
return HWFault;
|
||||
break;
|
||||
case TRIANGLENOTCLOSED:
|
||||
SCWrite(pCon,"ERROR: cannot close scattering triangle",eError);
|
||||
return HWFault;
|
||||
break;
|
||||
default:
|
||||
return startMotors(self,angles,pCon);
|
||||
}
|
||||
return HWFault;
|
||||
}
|
||||
/*-----------------------------------------------------------------------------*/
|
||||
static int checkMotors(ptasMot self, SConnection *pCon){
|
||||
int i, status;
|
||||
|
||||
self->math->mustRecalculate = 1;
|
||||
for(i = 0; i < 12; i++){
|
||||
if(self->math->motors[i] != NULL){
|
||||
status = self->math->motors[i]->pDrivInt->CheckStatus(self->math->motors[i],pCon);
|
||||
if(status != HWIdle && status != OKOK){
|
||||
return status;
|
||||
}
|
||||
}
|
||||
}
|
||||
return HWIdle;
|
||||
}
|
||||
/*------------------------------------------------------------------------------*/
|
||||
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){
|
||||
return HWFault;
|
||||
} else {
|
||||
return HWBusy;
|
||||
}
|
||||
} else {
|
||||
return checkMotors(self, pCon);
|
||||
}
|
||||
}
|
||||
/*---------------------------------------------------------------------------*/
|
||||
static int startQMMotors(ptasMot self, tasAngles angles,
|
||||
SConnection *pCon){
|
||||
float val;
|
||||
double curve;
|
||||
int status;
|
||||
|
||||
/*
|
||||
monochromator
|
||||
*/
|
||||
val = self->math->motors[A1]->pDrivInt->GetValue(self->math->motors[A1],pCon);
|
||||
if(ABS(val - angles.monochromator_two_theta/2.) > MOTPREC){
|
||||
status = self->math->motors[A1]->pDrivInt->SetValue(self->math->motors[A1],
|
||||
pCon,
|
||||
angles.monochromator_two_theta/2.);
|
||||
if(status != OKOK){
|
||||
return status;
|
||||
}
|
||||
}
|
||||
val = self->math->motors[A2]->pDrivInt->GetValue(self->math->motors[A2],pCon);
|
||||
if(ABS(val - angles.monochromator_two_theta) > MOTPREC){
|
||||
status = self->math->motors[A2]->pDrivInt->SetValue(self->math->motors[A2],
|
||||
pCon,
|
||||
angles.monochromator_two_theta);
|
||||
if(status != OKOK){
|
||||
return status;
|
||||
}
|
||||
}
|
||||
if(self->math->motors[MCV] != NULL){
|
||||
curve = maCalcVerticalCurvature(self->math->machine.monochromator,
|
||||
angles.monochromator_two_theta);
|
||||
val = self->math->motors[MCV]->pDrivInt->GetValue(self->math->motors[MCV],pCon);
|
||||
if(ABS(val - curve) > MOTPREC){
|
||||
status = self->math->motors[MCV]->pDrivInt->SetValue(self->math->motors[MCV],
|
||||
pCon,
|
||||
curve);
|
||||
if(status != OKOK){
|
||||
return status;
|
||||
}
|
||||
}
|
||||
}
|
||||
if(self->math->motors[MCH] != NULL){
|
||||
curve = maCalcHorizontalCurvature(self->math->machine.monochromator,
|
||||
angles.monochromator_two_theta);
|
||||
val = self->math->motors[MCH]->pDrivInt->GetValue(self->math->motors[MCH],pCon);
|
||||
if(ABS(val - curve) > MOTPREC){
|
||||
status = self->math->motors[MCH]->pDrivInt->SetValue(self->math->motors[MCH],
|
||||
pCon,
|
||||
curve);
|
||||
if(status != OKOK){
|
||||
return status;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
analyzer
|
||||
*/
|
||||
val = self->math->motors[A5]->pDrivInt->GetValue(self->math->motors[A5],pCon);
|
||||
if(ABS(val - angles.analyzer_two_theta/2.) > MOTPREC){
|
||||
status = self->math->motors[A5]->pDrivInt->SetValue(self->math->motors[A5],
|
||||
pCon,
|
||||
angles.analyzer_two_theta/2.);
|
||||
if(status != OKOK){
|
||||
return status;
|
||||
}
|
||||
}
|
||||
val = self->math->motors[A6]->pDrivInt->GetValue(self->math->motors[A6],pCon);
|
||||
if(ABS(val - angles.analyzer_two_theta) > MOTPREC){
|
||||
status = self->math->motors[A6]->pDrivInt->SetValue(self->math->motors[A6],
|
||||
pCon,
|
||||
angles.analyzer_two_theta);
|
||||
if(status != OKOK){
|
||||
return status;
|
||||
}
|
||||
}
|
||||
if(self->math->motors[ACV] != NULL){
|
||||
curve = maCalcVerticalCurvature(self->math->machine.analyzer,
|
||||
angles.analyzer_two_theta);
|
||||
val = self->math->motors[ACV]->pDrivInt->GetValue(self->math->motors[ACV],pCon);
|
||||
if(ABS(val - curve) > MOTPREC){
|
||||
status = self->math->motors[ACV]->pDrivInt->SetValue(self->math->motors[ACV],
|
||||
pCon,
|
||||
curve);
|
||||
if(status != OKOK){
|
||||
return status;
|
||||
}
|
||||
}
|
||||
}
|
||||
if(self->math->motors[ACH] != NULL){
|
||||
curve = maCalcHorizontalCurvature(self->math->machine.analyzer,
|
||||
angles.analyzer_two_theta);
|
||||
val = self->math->motors[ACH]->pDrivInt->GetValue(self->math->motors[ACH],pCon);
|
||||
if(ABS(val - curve) > MOTPREC){
|
||||
status = self->math->motors[ACH]->pDrivInt->SetValue(self->math->motors[ACH],
|
||||
pCon,
|
||||
curve);
|
||||
if(status != OKOK){
|
||||
return status;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
crystal
|
||||
*/
|
||||
val = self->math->motors[A4]->pDrivInt->GetValue(self->math->motors[A4],pCon);
|
||||
if(ABS(val - angles.sample_two_theta) > MOTPREC){
|
||||
status = self->math->motors[A4]->pDrivInt->SetValue(self->math->motors[A4],
|
||||
pCon,
|
||||
angles.sample_two_theta);
|
||||
if(status != OKOK){
|
||||
return status;
|
||||
}
|
||||
}
|
||||
|
||||
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 {
|
||||
SCWrite(pCon,"ERROR: tasmot does not understand this subcommand",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
Block a user