- First working version of the triple axis UB matrix code
This commit is contained in:
418
tasub.c
418
tasub.c
@ -10,6 +10,7 @@
|
||||
#include "sics.h"
|
||||
#include "lld.h"
|
||||
#include "tasub.h"
|
||||
#include "tasdrive.h"
|
||||
/*------------------- motor indexes in motor data structure ---------*/
|
||||
#define A1 0
|
||||
#define A2 1
|
||||
@ -43,8 +44,9 @@ static void saveReflections(ptasUB self, char *name, FILE *fd){
|
||||
while(status == 1){
|
||||
LLDnodeDataTo(self->reflectionList,&r);
|
||||
fprintf(fd,"%s addref %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f\n",
|
||||
name, r.h, r.k, r.l, r.a3, r.two_theta, r.sgu, r.sgl,
|
||||
KtoEnergy(r.ki), KtoEnergy(r.kf));
|
||||
name, r.qe.qh, r.qe.qk, r.qe.ql, r.angles.a3, r.angles.sample_two_theta,
|
||||
r.angles.sgu, r.angles.sgl,
|
||||
KtoEnergy(r.qe.ki), KtoEnergy(r.qe.kf));
|
||||
status = LLDnodePtr2Next(self->reflectionList);
|
||||
}
|
||||
}
|
||||
@ -68,6 +70,20 @@ static int tasUBSave(void *pData, char *name, FILE *fd){
|
||||
fprintf(fd,"%s const kf\n",name);
|
||||
}
|
||||
fprintf(fd,"%s ss %d\n", name,self->machine.ss_sample);
|
||||
fprintf(fd," %s setub %f %f %f %f %f %f %f %f %f\n",
|
||||
name,
|
||||
self->machine.UB[0][0], self->machine.UB[0][1], self->machine.UB[0][2],
|
||||
self->machine.UB[1][0], self->machine.UB[1][1], self->machine.UB[1][2],
|
||||
self->machine.UB[2][0], self->machine.UB[2][1], self->machine.UB[2][2]);
|
||||
fprintf(fd," %s setnormal %f %f %f\n",
|
||||
name,
|
||||
self->machine.planeNormal[0][0], self->machine.planeNormal[1][0],
|
||||
self->machine.planeNormal[2][0]);
|
||||
fprintf(fd,"%s settarget %f %f %f %f %f %f\n",
|
||||
name,
|
||||
self->target.qh, self->target.qk, self->target.ql, self->target.qm,
|
||||
self->target.ki, self->target.kf);
|
||||
fprintf(fd,"%s update\n", name);
|
||||
return 1;
|
||||
}
|
||||
/*------------------------------------------------------------------*/
|
||||
@ -108,7 +124,7 @@ static ptasUB MakeTasUB(){
|
||||
pNew->targetEn = .0;
|
||||
pNew->actualEn = .0;
|
||||
pNew->mustRecalculate = 1;
|
||||
|
||||
|
||||
return pNew;
|
||||
}
|
||||
/*-------------------------------------------------------------------*/
|
||||
@ -139,58 +155,20 @@ static int readTASAngles(ptasUB self, SConnection *pCon,
|
||||
/*
|
||||
Monochromator
|
||||
*/
|
||||
status = MotorGetSoftPosition(self->motors[A1],pCon,&val);
|
||||
if(status == 0){
|
||||
return status;
|
||||
}
|
||||
ang->monochromator.theta = val;
|
||||
status = MotorGetSoftPosition(self->motors[A2],pCon,&val);
|
||||
if(status == 0){
|
||||
return status;
|
||||
}
|
||||
ang->monochromator.two_theta = val;
|
||||
if(self->motors[MCV] != NULL){
|
||||
status = MotorGetSoftPosition(self->motors[MCV],pCon,&val);
|
||||
if(status == 0){
|
||||
return status;
|
||||
}
|
||||
ang->monochromator.vertical_curvature = val;
|
||||
}
|
||||
if(self->motors[MCH] != NULL){
|
||||
status = MotorGetSoftPosition(self->motors[MCH],pCon,&val);
|
||||
if(status == 0){
|
||||
return status;
|
||||
}
|
||||
ang->monochromator.horizontal_curvature = val;
|
||||
}
|
||||
ang->monochromator_two_theta = val;
|
||||
|
||||
/*
|
||||
Analyzer
|
||||
*/
|
||||
status = MotorGetSoftPosition(self->motors[A5],pCon,&val);
|
||||
if(status == 0){
|
||||
return status;
|
||||
}
|
||||
ang->analyzer.theta = val;
|
||||
status = MotorGetSoftPosition(self->motors[A6],pCon,&val);
|
||||
if(status == 0){
|
||||
return status;
|
||||
}
|
||||
ang->analyzer.two_theta = val;
|
||||
if(self->motors[ACV] != NULL){
|
||||
status = MotorGetSoftPosition(self->motors[ACV],pCon,&val);
|
||||
if(status == 0){
|
||||
return status;
|
||||
}
|
||||
ang->analyzer.vertical_curvature = val;
|
||||
}
|
||||
if(self->motors[ACH] != NULL){
|
||||
status = MotorGetSoftPosition(self->motors[ACH],pCon,&val);
|
||||
if(status == 0){
|
||||
return status;
|
||||
}
|
||||
ang->analyzer.horizontal_curvature = val;
|
||||
}
|
||||
ang->analyzer_two_theta = val;
|
||||
|
||||
/*
|
||||
crystal
|
||||
@ -233,7 +211,12 @@ static int testMotor(ptasUB pNew, SConnection *pCon, char *name, int idx){
|
||||
int TasUBFactory(SConnection *pCon,SicsInterp *pSics, void *pData,
|
||||
int argc, char *argv[]){
|
||||
ptasUB pNew = NULL;
|
||||
int status = 0;
|
||||
int status = 0, i;
|
||||
char pBueffel[132];
|
||||
char names[][3] = {"ei","ki",
|
||||
"qh","qk","ql",
|
||||
"ef","kf",
|
||||
"en"};
|
||||
|
||||
if(argc < 2) {
|
||||
SCWrite(pCon,"ERROR: need name to install tasUB",eError);
|
||||
@ -284,9 +267,23 @@ int TasUBFactory(SConnection *pCon,SicsInterp *pSics, void *pData,
|
||||
SCWrite(pCon,"ERROR: duplicate tasUB command not created",eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*
|
||||
TODO: install all the virtual motors
|
||||
install virtual motors
|
||||
*/
|
||||
for(i = 0; i < 8; i++){
|
||||
status = InstallTasMotor(pSics,pNew,i+1,names[i]);
|
||||
if(status != 1){
|
||||
snprintf(pBueffel,131,"ERROR: failed to create TAS motor %s", names[i]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
}
|
||||
}
|
||||
status = InstallTasQMMotor(pSics,pNew);
|
||||
if(status != 1){
|
||||
snprintf(pBueffel,131,"ERROR: failed to create TAS motor qm");
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
}
|
||||
|
||||
return 1;
|
||||
}
|
||||
/*-----------------------------------------------------------------*/
|
||||
@ -304,6 +301,10 @@ static int setCrystalParameters(pmaCrystal crystal, SConnection *pCon,
|
||||
return 1;
|
||||
}
|
||||
|
||||
if(!SCMatchRights(pCon,usMugger)){
|
||||
return 0;
|
||||
}
|
||||
|
||||
strtolower(argv[2]);
|
||||
if(strcmp(argv[2],"dd") == 0){
|
||||
crystal->dd = d;
|
||||
@ -411,6 +412,11 @@ static int tasReadCell(SConnection *pCon, ptasUB self, int argc, char *argv[]){
|
||||
eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
if(!SCMatchRights(pCon,usUser)){
|
||||
return 0;
|
||||
}
|
||||
|
||||
status = Tcl_GetDouble(pTcl,argv[2],&self->cell.a);
|
||||
if(status != TCL_OK){
|
||||
snprintf(pBueffel,255,"ERROR: failed to convert %s to number",argv[2]);
|
||||
@ -487,7 +493,8 @@ static void listReflections(ptasUB self, SConnection *pCon){
|
||||
count++;
|
||||
LLDnodeDataTo(self->reflectionList,&r);
|
||||
snprintf(line,255,"%3d %6.2f %6.2f %6.2f %7.2f %7.2f %6.2f %6.2f %6.2f %6.2f\n",
|
||||
count, r.h, r.k, r.l, r.a3, r.two_theta, r.sgu, r.sgl, r.ki, r.kf);
|
||||
count, r.qe.qh, r.qe.qk, r.qe.ql, r.angles.a3, r.angles.sample_two_theta,
|
||||
r.angles.sgu, r.angles.sgl, r.qe.ki, r.qe.kf);
|
||||
Tcl_DStringAppend(&list,line,-1);
|
||||
status = LLDnodePtr2Next(self->reflectionList);
|
||||
}
|
||||
@ -513,19 +520,24 @@ static int addReflection(ptasUB self, SicsInterp *pSics,
|
||||
eError);
|
||||
return 0;
|
||||
}
|
||||
status = Tcl_GetDouble(InterpGetTcl(pSics),argv[2],&r.h);
|
||||
|
||||
if(!SCMatchRights(pCon,usUser)){
|
||||
return 0;
|
||||
}
|
||||
|
||||
status = Tcl_GetDouble(InterpGetTcl(pSics),argv[2],&r.qe.qh);
|
||||
if(status != TCL_OK){
|
||||
snprintf(pBueffel,255,"ERROR: failed to convert %s to number",argv[2]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
status = Tcl_GetDouble(InterpGetTcl(pSics),argv[3],&r.k);
|
||||
status = Tcl_GetDouble(InterpGetTcl(pSics),argv[3],&r.qe.qk);
|
||||
if(status != TCL_OK){
|
||||
snprintf(pBueffel,255,"ERROR: failed to convert %s to number",argv[3]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
status = Tcl_GetDouble(InterpGetTcl(pSics),argv[4],&r.l);
|
||||
status = Tcl_GetDouble(InterpGetTcl(pSics),argv[4],&r.qe.ql);
|
||||
if(status != TCL_OK){
|
||||
snprintf(pBueffel,255,"ERROR: failed to convert %s to number",argv[4]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
@ -533,66 +545,56 @@ static int addReflection(ptasUB self, SicsInterp *pSics,
|
||||
}
|
||||
|
||||
if(argc >= 11){
|
||||
status = Tcl_GetDouble(InterpGetTcl(pSics),argv[5],&r.a3);
|
||||
status = Tcl_GetDouble(InterpGetTcl(pSics),argv[5],&r.angles.a3);
|
||||
if(status != TCL_OK){
|
||||
snprintf(pBueffel,255,"ERROR: failed to convert %s to number",argv[5]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
status = Tcl_GetDouble(InterpGetTcl(pSics),argv[5],&r.a3);
|
||||
if(status != TCL_OK){
|
||||
snprintf(pBueffel,255,"ERROR: failed to convert %s to number",argv[5]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
status = Tcl_GetDouble(InterpGetTcl(pSics),argv[6],&r.two_theta);
|
||||
status = Tcl_GetDouble(InterpGetTcl(pSics),argv[6],&r.angles.sample_two_theta);
|
||||
if(status != TCL_OK){
|
||||
snprintf(pBueffel,255,"ERROR: failed to convert %s to number",argv[6]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
status = Tcl_GetDouble(InterpGetTcl(pSics),argv[7],&r.sgu);
|
||||
status = Tcl_GetDouble(InterpGetTcl(pSics),argv[7],&r.angles.sgu);
|
||||
if(status != TCL_OK){
|
||||
snprintf(pBueffel,255,"ERROR: failed to convert %s to number",argv[7]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
status = Tcl_GetDouble(InterpGetTcl(pSics),argv[8],&r.sgl);
|
||||
status = Tcl_GetDouble(InterpGetTcl(pSics),argv[8],&r.angles.sgl);
|
||||
if(status != TCL_OK){
|
||||
snprintf(pBueffel,255,"ERROR: failed to convert %s to number",argv[8]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
status = Tcl_GetDouble(InterpGetTcl(pSics),argv[9],&r.ki);
|
||||
status = Tcl_GetDouble(InterpGetTcl(pSics),argv[9],&r.qe.ki);
|
||||
if(status != TCL_OK){
|
||||
snprintf(pBueffel,255,"ERROR: failed to convert %s to number",argv[9]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
r.ki = energyToK(r.ki);
|
||||
status = Tcl_GetDouble(InterpGetTcl(pSics),argv[10],&r.kf);
|
||||
r.qe.ki = energyToK(r.qe.ki);
|
||||
status = Tcl_GetDouble(InterpGetTcl(pSics),argv[10],&r.qe.kf);
|
||||
if(status != TCL_OK){
|
||||
snprintf(pBueffel,255,"ERROR: failed to convert %s to number",argv[10]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
r.kf = energyToK(r.kf);
|
||||
r.qe.kf = energyToK(r.qe.kf);
|
||||
} else {
|
||||
if(argc > 5){
|
||||
SCWrite(pCon,
|
||||
"WARNING: not all angles given on command line, using positions instead",
|
||||
eWarning);
|
||||
}
|
||||
status = readTASAngles(self,pCon,&angles);
|
||||
status = readTASAngles(self,pCon,&r.angles);
|
||||
if(status != 1){
|
||||
return status;
|
||||
}
|
||||
r.a3 = angles.a3;
|
||||
r.two_theta = angles.sample_two_theta;
|
||||
r.sgu = angles.sgu;
|
||||
r.sgl = angles.sgl;
|
||||
maCalcK(self->machine.monochromator,angles.monochromator,&r.ki);
|
||||
maCalcK(self->machine.analyzer,angles.analyzer,&r.kf);
|
||||
r.qe.ki = maCalcK(self->machine.monochromator,angles.monochromator_two_theta);
|
||||
r.qe.kf = maCalcK(self->machine.analyzer,angles.analyzer_two_theta);
|
||||
}
|
||||
LLDnodeAppend(self->reflectionList,&r);
|
||||
Tcl_DStringInit(&list);
|
||||
@ -601,10 +603,12 @@ static int addReflection(ptasUB self, SicsInterp *pSics,
|
||||
Tcl_DStringAppend(&list,pBueffel,-1);
|
||||
snprintf(pBueffel,255,
|
||||
" %6.2f %6.2f %6.2f %7.2f %7.2f %6.2f %6.2f %6.2f %6.2f\n",
|
||||
r.h, r.k, r.l, r.a3, r.two_theta, r.sgu, r.sgl, r.ki, r.kf);
|
||||
r.qe.qh, r.qe.qk, r.qe.ql, r.angles.a3, r.angles.sample_two_theta,
|
||||
r.angles.sgu, r.angles.sgl, r.qe.ki, r.qe.kf);
|
||||
Tcl_DStringAppend(&list,pBueffel,-1);
|
||||
SCWrite(pCon,Tcl_DStringValue(&list),eValue);
|
||||
Tcl_DStringFree(&list);
|
||||
SCparChange(pCon);
|
||||
return 1;
|
||||
}
|
||||
/*-----------------------------------------------------------------*/
|
||||
@ -662,28 +666,30 @@ static void printReflectionDiagnostik(ptasUB self, SConnection *pCon,
|
||||
Tcl_DStringAppend(&list,line,-1);
|
||||
snprintf(line,255,
|
||||
"INPUT %6.2f %6.2f %6.2f %7.2f %7.2f %6.2f %6.2f %6.2f %6.2f\n",
|
||||
r.h, r.k, r.l, r.a3, r.two_theta, r.sgu, r.sgl, r.ki, r.kf);
|
||||
r.qe.qh, r.qe.qk, r.qe.ql, r.angles.a3,
|
||||
r.angles.sample_two_theta, r.angles.sgu, r.angles.sgl, r.qe.ki, r.qe.kf);
|
||||
Tcl_DStringAppend(&list,line,-1);
|
||||
qe.ki = r.ki;
|
||||
qe.kf = r.kf;
|
||||
qe.qk = r.h;
|
||||
qe.qk = r.k;
|
||||
qe.ql = r.l;
|
||||
qe.ki = r.qe.ki;
|
||||
qe.kf = r.qe.kf;
|
||||
qe.qh = r.qe.qh;
|
||||
qe.qk = r.qe.qk;
|
||||
qe.ql = r.qe.ql;
|
||||
calcAllTasAngles(&self->machine,qe,&angles);
|
||||
snprintf(line,255,
|
||||
"QE->ANG %6.2f %6.2f %6.2f %7.2f %7.2f %6.2f %6.2f %6.2f %6.2f\n",
|
||||
r.h, r.k, r.l, angles.a3, angles.sample_two_theta,
|
||||
angles.sgu, angles.sgl, r.ki, r.kf);
|
||||
r.qe.qh, r.qe.qk, r.qe.ql,
|
||||
angles.a3, angles.sample_two_theta,
|
||||
angles.sgu, angles.sgl, r.qe.ki, r.qe.kf);
|
||||
Tcl_DStringAppend(&list,line,-1);
|
||||
angles.a3 = r.a3;
|
||||
angles.sample_two_theta = r.two_theta;
|
||||
angles.sgu = r.sgu;
|
||||
angles.sgl = r.sgl;
|
||||
angles.a3 = r.angles.a3;
|
||||
angles.sample_two_theta = r.angles.sample_two_theta;
|
||||
angles.sgu = r.angles.sgu;
|
||||
angles.sgl = r.angles.sgl;
|
||||
calcTasQEPosition(&self->machine,angles,&qe);
|
||||
snprintf(line,255,
|
||||
"ANG->QE %6.2f %6.2f %6.2f %7.2f %7.2f %6.2f %6.2f %6.2f %6.2f\n",
|
||||
qe.qh, qe.qk, qe.ql, r.a3, r.two_theta,
|
||||
r.sgu, r.sgl, qe.ki, qe.kf);
|
||||
qe.qh, qe.qk, qe.ql, angles.a3, angles.sample_two_theta,
|
||||
angles.sgu, angles.sgl, qe.ki, qe.kf);
|
||||
Tcl_DStringAppend(&list,line,-1);
|
||||
SCWrite(pCon,Tcl_DStringValue(&list),eValue);
|
||||
Tcl_DStringFree(&list);
|
||||
@ -714,6 +720,11 @@ static int calcUB(ptasUB self, SConnection *pCon, SicsInterp *pSics,
|
||||
eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
if(!SCMatchRights(pCon,usUser)){
|
||||
return 0;
|
||||
}
|
||||
|
||||
status = Tcl_GetInt(InterpGetTcl(pSics),argv[2],&idx);
|
||||
if(status != TCL_OK){
|
||||
snprintf(pBueffel,255,"ERROR: failed to convert %s to number",argv[2]);
|
||||
@ -761,6 +772,7 @@ static int calcUB(ptasUB self, SConnection *pCon, SicsInterp *pSics,
|
||||
self->machine.planeNormal = calcPlaneNormal(r1,r2);
|
||||
listUB(UB,pCon);
|
||||
listDiagnostik(self,pCon);
|
||||
SCparChange(pCon);
|
||||
return 1;
|
||||
}
|
||||
/*------------------------------------------------------------------*/
|
||||
@ -820,19 +832,240 @@ static int calcRefAngles(ptasUB self, SConnection *pCon,
|
||||
SCWrite(pCon,"ERROR: Out of memory calculating angles",eError);
|
||||
return 0;
|
||||
break;
|
||||
case BADRMATRIX:
|
||||
SCWrite(pCon,"ERROR: bad crystallographic parameters or bad UB",eError);
|
||||
return 0;
|
||||
break;
|
||||
case TRIANGLENOTCLOSED:
|
||||
SCWrite(pCon,"ERROR: scattering triangle not closed",eError);
|
||||
return 0;
|
||||
break;
|
||||
}
|
||||
snprintf(pBueffel,255," %8.2f %8.2f %8.2f %8.2f %8.2f %8.2f %8.2f %8.2f",
|
||||
angles.monochromator.theta, angles.monochromator.two_theta,
|
||||
snprintf(pBueffel,255," %8.2f %8.2f %8.2f %8.2f %8.2f %8.2f",
|
||||
angles.monochromator_two_theta,
|
||||
angles.a3, angles.sample_two_theta,
|
||||
angles.sgl, angles.sgu,
|
||||
angles.analyzer.theta,angles.analyzer.two_theta);
|
||||
angles.analyzer_two_theta);
|
||||
SCWrite(pCon,pBueffel,eValue);
|
||||
return 1;
|
||||
}
|
||||
/*------------------------------------------------------------------*/
|
||||
static int setUB(SConnection *pCon, SicsInterp *pSics, ptasUB self,
|
||||
int argc, char *argv[]){
|
||||
double value;
|
||||
char pBueffel[256];
|
||||
int status;
|
||||
|
||||
if(argc < 11){
|
||||
SCWrite(pCon,"ERROR: not enough arguments for setting UB",
|
||||
eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
if(!SCMatchRights(pCon,usUser)){
|
||||
return 0;
|
||||
}
|
||||
|
||||
status = Tcl_GetDouble(InterpGetTcl(pSics),argv[2],&value);
|
||||
if(status != TCL_OK){
|
||||
snprintf(pBueffel,255,"ERROR: failed to convert %s to number",argv[2]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
self->machine.UB[0][0] = value;
|
||||
|
||||
status = Tcl_GetDouble(InterpGetTcl(pSics),argv[3],&value);
|
||||
if(status != TCL_OK){
|
||||
snprintf(pBueffel,255,"ERROR: failed to convert %s to number",argv[3]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
self->machine.UB[0][1] = value;
|
||||
|
||||
status = Tcl_GetDouble(InterpGetTcl(pSics),argv[4],&value);
|
||||
if(status != TCL_OK){
|
||||
snprintf(pBueffel,255,"ERROR: failed to convert %s to number",argv[4]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
self->machine.UB[0][2] = value;
|
||||
|
||||
status = Tcl_GetDouble(InterpGetTcl(pSics),argv[5],&value);
|
||||
if(status != TCL_OK){
|
||||
snprintf(pBueffel,255,"ERROR: failed to convert %s to number",argv[5]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
self->machine.UB[1][0] = value;
|
||||
|
||||
status = Tcl_GetDouble(InterpGetTcl(pSics),argv[6],&value);
|
||||
if(status != TCL_OK){
|
||||
snprintf(pBueffel,255,"ERROR: failed to convert %s to number",argv[6]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
self->machine.UB[1][1] = value;
|
||||
|
||||
status = Tcl_GetDouble(InterpGetTcl(pSics),argv[7],&value);
|
||||
if(status != TCL_OK){
|
||||
snprintf(pBueffel,255,"ERROR: failed to convert %s to number",argv[7]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
self->machine.UB[1][2] = value;
|
||||
|
||||
status = Tcl_GetDouble(InterpGetTcl(pSics),argv[8],&value);
|
||||
if(status != TCL_OK){
|
||||
snprintf(pBueffel,255,"ERROR: failed to convert %s to number",argv[8]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
self->machine.UB[2][0] = value;
|
||||
|
||||
status = Tcl_GetDouble(InterpGetTcl(pSics),argv[9],&value);
|
||||
if(status != TCL_OK){
|
||||
snprintf(pBueffel,255,"ERROR: failed to convert %s to number",argv[9]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
self->machine.UB[2][1] = value;
|
||||
|
||||
status = Tcl_GetDouble(InterpGetTcl(pSics),argv[10],&value);
|
||||
if(status != TCL_OK){
|
||||
snprintf(pBueffel,255,"ERROR: failed to convert %s to number",argv[10]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
self->machine.UB[2][2] = value;
|
||||
SCSendOK(pCon);
|
||||
SCparChange(pCon);
|
||||
|
||||
return 1;
|
||||
}
|
||||
/*------------------------------------------------------------------*/
|
||||
static int setNormal(SConnection *pCon, SicsInterp *pSics, ptasUB self,
|
||||
int argc, char *argv[]){
|
||||
double value;
|
||||
char pBueffel[256];
|
||||
int status;
|
||||
|
||||
if(argc < 5){
|
||||
SCWrite(pCon,"ERROR: not enough arguments for setting plane normal",
|
||||
eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
if(!SCMatchRights(pCon,usUser)){
|
||||
return 0;
|
||||
}
|
||||
|
||||
status = Tcl_GetDouble(InterpGetTcl(pSics),argv[2],&value);
|
||||
if(status != TCL_OK){
|
||||
snprintf(pBueffel,255,"ERROR: failed to convert %s to number",argv[2]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
self->machine.planeNormal[0][0] = value;
|
||||
|
||||
status = Tcl_GetDouble(InterpGetTcl(pSics),argv[3],&value);
|
||||
if(status != TCL_OK){
|
||||
snprintf(pBueffel,255,"ERROR: failed to convert %s to number",argv[3]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
self->machine.planeNormal[1][0] = value;
|
||||
|
||||
status = Tcl_GetDouble(InterpGetTcl(pSics),argv[4],&value);
|
||||
if(status != TCL_OK){
|
||||
snprintf(pBueffel,255,"ERROR: failed to convert %s to number",argv[4]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
self->machine.planeNormal[2][0] = value;
|
||||
SCSendOK(pCon);
|
||||
SCparChange(pCon);
|
||||
|
||||
return 1;
|
||||
}
|
||||
/*------------------------------------------------------------------*/
|
||||
static int setTarget(SConnection *pCon, SicsInterp *pSics, ptasUB self,
|
||||
int argc, char *argv[]){
|
||||
double value;
|
||||
char pBueffel[256];
|
||||
int status;
|
||||
|
||||
if(argc < 8){
|
||||
SCWrite(pCon,"ERROR: not enough arguments for setting qe target",
|
||||
eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
if(!SCMatchRights(pCon,usUser)){
|
||||
return 0;
|
||||
}
|
||||
|
||||
status = Tcl_GetDouble(InterpGetTcl(pSics),argv[2],&self->target.qh);
|
||||
if(status != TCL_OK){
|
||||
snprintf(pBueffel,255,"ERROR: failed to convert %s to number",argv[2]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
status = Tcl_GetDouble(InterpGetTcl(pSics),argv[3],&self->target.qk);
|
||||
if(status != TCL_OK){
|
||||
snprintf(pBueffel,255,"ERROR: failed to convert %s to number",argv[3]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
status = Tcl_GetDouble(InterpGetTcl(pSics),argv[4],&self->target.ql);
|
||||
if(status != TCL_OK){
|
||||
snprintf(pBueffel,255,"ERROR: failed to convert %s to number",argv[4]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
status = Tcl_GetDouble(InterpGetTcl(pSics),argv[5],&self->target.qm);
|
||||
if(status != TCL_OK){
|
||||
snprintf(pBueffel,255,"ERROR: failed to convert %s to number",argv[5]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
status = Tcl_GetDouble(InterpGetTcl(pSics),argv[6],&self->target.ki);
|
||||
if(status != TCL_OK){
|
||||
snprintf(pBueffel,255,"ERROR: failed to convert %s to number",argv[6]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
status = Tcl_GetDouble(InterpGetTcl(pSics),argv[7],&self->target.kf);
|
||||
if(status != TCL_OK){
|
||||
snprintf(pBueffel,255,"ERROR: failed to convert %s to number",argv[7]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
SCSendOK(pCon);
|
||||
|
||||
return 1;
|
||||
}
|
||||
/*------------------------------------------------------------------*/
|
||||
static int tasUpdate(SConnection *pCon, ptasUB self){
|
||||
int status;
|
||||
tasAngles angles;
|
||||
|
||||
status = readTASAngles(self,pCon,&angles);
|
||||
if(status != 1){
|
||||
return status;
|
||||
}
|
||||
status = calcTasQEPosition(&self->machine, angles, &self->current);
|
||||
if(status < 0){
|
||||
SCWrite(pCon,"ERROR: out of memory calculating Q-E variables",eError);
|
||||
return 0;
|
||||
}
|
||||
self->mustRecalculate = 0;
|
||||
SCSendOK(pCon);
|
||||
return 1;
|
||||
}
|
||||
/*-------------------------------------------------------------------*/
|
||||
int TasUBWrapper(SConnection *pCon,SicsInterp *pSics, void *pData,
|
||||
int argc, char *argv[]){
|
||||
@ -877,6 +1110,14 @@ int TasUBWrapper(SConnection *pCon,SicsInterp *pSics, void *pData,
|
||||
return calcUB(self,pCon,pSics,argc,argv);
|
||||
} else if(strcmp(argv[1],"calcref") == 0){
|
||||
return calcRefAngles(self,pCon,pSics,argc,argv);
|
||||
} else if(strcmp(argv[1],"setub") == 0){
|
||||
return setUB(pCon,pSics,self,argc,argv);
|
||||
} else if(strcmp(argv[1],"setnormal") == 0){
|
||||
return setNormal(pCon,pSics,self,argc,argv);
|
||||
} else if(strcmp(argv[1],"settarget") == 0){
|
||||
return setTarget(pCon,pSics,self,argc,argv);
|
||||
} else if(strcmp(argv[1],"update") == 0){
|
||||
return tasUpdate(pCon,self);
|
||||
} else if(strcmp(argv[1],"const") == 0){
|
||||
if(argc > 2){
|
||||
strtolower(argv[2]);
|
||||
@ -919,6 +1160,7 @@ int TasUBWrapper(SConnection *pCon,SicsInterp *pSics, void *pData,
|
||||
return 0;
|
||||
}
|
||||
self->machine.ss_sample = newSS;
|
||||
tasUpdate(pCon,self);
|
||||
SCSendOK(pCon);
|
||||
return 1;
|
||||
} else {
|
||||
|
Reference in New Issue
Block a user