PSI UPDATE
r2720 | ffr | 2008-10-13 15:40:07 +1100 (Mon, 13 Oct 2008) | 2 lines
This commit is contained in:
Ferdi Franceschini
387
tasdrive.c
387
tasdrive.c
@@ -4,6 +4,9 @@
|
||||
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.
|
||||
--------------------------------------------------------------------*/
|
||||
#include <assert.h>
|
||||
#include "sics.h"
|
||||
@@ -217,127 +220,106 @@ static float getMotorValue(pMotor mot, SConnection *pCon){
|
||||
|
||||
MotorGetSoftPosition(mot,pCon,&val);
|
||||
return val;
|
||||
}
|
||||
}
|
||||
/*--------------------------------------------------------------------------*/
|
||||
static int startTASMotor(pMotor mot, SConnection *pCon, char *name,
|
||||
double target, int silent){
|
||||
float val, fixed;
|
||||
int status = OKOK;
|
||||
char buffer[132];
|
||||
|
||||
val = getMotorValue(mot,pCon);
|
||||
MotorGetPar(mot,"fixed",&fixed);
|
||||
if(ABS(fixed - 1.0) < .1) {
|
||||
snprintf(buffer,131,"WARNING: %s is FIXED", name);
|
||||
SCWrite(pCon,buffer,eWarning);
|
||||
return OKOK;
|
||||
}
|
||||
if(ABS(val - target) > MOTPREC){
|
||||
status = mot->pDrivInt->SetValue(mot,pCon,(float)target);
|
||||
if(status != OKOK){
|
||||
return status;
|
||||
}
|
||||
}
|
||||
writeMotPos(pCon,silent,name,val, target);
|
||||
return status;
|
||||
}
|
||||
/*---------------------------------------------------------------------------*/
|
||||
static int startMotors(ptasMot self, tasAngles angles,
|
||||
SConnection *pCon, int driveQ, int driveTilt){
|
||||
float val;
|
||||
double curve;
|
||||
int status, silent;
|
||||
|
||||
silent = self->math->silent;
|
||||
self->math->mustRecalculate = 1;
|
||||
/*
|
||||
monochromator
|
||||
*/
|
||||
val = getMotorValue(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;
|
||||
}
|
||||
status = startTASMotor(self->math->motors[A1],pCon, "a1",
|
||||
angles.monochromator_two_theta/2.,silent);
|
||||
if(status != OKOK){
|
||||
return status;
|
||||
}
|
||||
writeMotPos(pCon,silent,"a1",val, angles.monochromator_two_theta/2.);
|
||||
|
||||
val = getMotorValue(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;
|
||||
}
|
||||
status = startTASMotor(self->math->motors[A2],pCon, "a2",
|
||||
angles.monochromator_two_theta,silent);
|
||||
if(status != OKOK){
|
||||
return status;
|
||||
}
|
||||
writeMotPos(pCon,silent,"a2",val, angles.monochromator_two_theta);
|
||||
|
||||
|
||||
if(self->math->motors[MCV] != NULL){
|
||||
curve = maCalcVerticalCurvature(self->math->machine.monochromator,
|
||||
angles.monochromator_two_theta);
|
||||
|
||||
val = getMotorValue(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;
|
||||
}
|
||||
status = startTASMotor(self->math->motors[MCV],pCon, "mcv",
|
||||
curve,silent);
|
||||
if(status != OKOK){
|
||||
return status;
|
||||
}
|
||||
writeMotPos(pCon,silent,"mcv",val, curve);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
if(self->math->motors[MCH] != NULL){
|
||||
curve = maCalcHorizontalCurvature(self->math->machine.monochromator,
|
||||
angles.monochromator_two_theta);
|
||||
|
||||
val = getMotorValue(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;
|
||||
}
|
||||
angles.monochromator_two_theta);
|
||||
status = startTASMotor(self->math->motors[MCH],pCon, "mch",
|
||||
curve,silent);
|
||||
if(status != OKOK){
|
||||
return status;
|
||||
}
|
||||
writeMotPos(pCon,silent,"mch",val, curve);
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
analyzer
|
||||
*/
|
||||
if(self->math->tasMode != ELASTIC){
|
||||
val = getMotorValue(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;
|
||||
}
|
||||
}
|
||||
writeMotPos(pCon,silent,self->math->motors[A5]->name,
|
||||
val, angles.analyzer_two_theta/2.);
|
||||
|
||||
val = getMotorValue(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;
|
||||
}
|
||||
}
|
||||
writeMotPos(pCon,silent,"a6",val, angles.analyzer_two_theta);
|
||||
status = startTASMotor(self->math->motors[A5],pCon, "a5",
|
||||
angles.analyzer_two_theta/2.0,silent);
|
||||
if(status != OKOK){
|
||||
return status;
|
||||
}
|
||||
status = startTASMotor(self->math->motors[A6],pCon, "a6",
|
||||
angles.analyzer_two_theta,silent);
|
||||
if(status != OKOK){
|
||||
return status;
|
||||
}
|
||||
|
||||
if(self->math->motors[ACV] != NULL){
|
||||
curve = maCalcVerticalCurvature(self->math->machine.analyzer,
|
||||
angles.analyzer_two_theta);
|
||||
val = getMotorValue(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;
|
||||
}
|
||||
}
|
||||
writeMotPos(pCon,silent,"acv",val, curve);
|
||||
}
|
||||
status = startTASMotor(self->math->motors[ACV],pCon, "acv",
|
||||
curve,silent);
|
||||
if(status != OKOK){
|
||||
return status;
|
||||
}
|
||||
}
|
||||
if(self->math->motors[ACH] != NULL){
|
||||
curve = maCalcHorizontalCurvature(self->math->machine.analyzer,
|
||||
angles.analyzer_two_theta);
|
||||
val = getMotorValue(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;
|
||||
}
|
||||
}
|
||||
writeMotPos(pCon,silent,"ach",val, curve);
|
||||
}
|
||||
status = startTASMotor(self->math->motors[ACH],pCon, "ach",
|
||||
curve,silent);
|
||||
if(status != OKOK){
|
||||
return status;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if(driveQ == 0){
|
||||
@@ -347,50 +329,28 @@ static int startMotors(ptasMot self, tasAngles angles,
|
||||
/*
|
||||
crystal
|
||||
*/
|
||||
val = getMotorValue(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;
|
||||
}
|
||||
status = startTASMotor(self->math->motors[A3],pCon, "a3",
|
||||
angles.a3,silent);
|
||||
if(status != OKOK){
|
||||
return status;
|
||||
}
|
||||
writeMotPos(pCon,silent,"a3",val, angles.a3);
|
||||
|
||||
val = getMotorValue(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;
|
||||
}
|
||||
status = startTASMotor(self->math->motors[A4],pCon, "a4",
|
||||
angles.sample_two_theta,silent);
|
||||
if(status != OKOK){
|
||||
return status;
|
||||
}
|
||||
writeMotPos(pCon,silent,"a4",val, angles.sample_two_theta);
|
||||
|
||||
|
||||
if(driveTilt == 1){
|
||||
val = getMotorValue(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;
|
||||
}
|
||||
}
|
||||
writeMotPos(pCon,silent,"sgl",val, angles.sgl);
|
||||
|
||||
val = getMotorValue(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;
|
||||
}
|
||||
}
|
||||
writeMotPos(pCon,silent,"sgu",val, angles.sgu);
|
||||
status = startTASMotor(self->math->motors[SGL],pCon, "sgl",
|
||||
angles.sgl,silent);
|
||||
if(status != OKOK){
|
||||
return status;
|
||||
}
|
||||
status = startTASMotor(self->math->motors[SGU],pCon, "sgu",
|
||||
angles.sgu,silent);
|
||||
if(status != OKOK){
|
||||
return status;
|
||||
}
|
||||
}
|
||||
self->math->mustDrive = 0;
|
||||
return OKOK;
|
||||
@@ -401,17 +361,21 @@ static int checkQMotorLimits(ptasMot self, SConnection *pCon,
|
||||
int status, retVal = 1;
|
||||
char error[131];
|
||||
char pBueffel[256];
|
||||
float val;
|
||||
|
||||
status = self->math->motors[A3]->pDrivInt->CheckLimits(self->math->motors[A3],
|
||||
angles.a3,
|
||||
error,
|
||||
131);
|
||||
if(status != 1) {
|
||||
retVal = 0;
|
||||
snprintf(pBueffel,256,"ERROR: limit violation an a3: %s", error);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
MotorGetPar(self->math->motors[A3],"fixed",&val);
|
||||
if((int)val != 1){
|
||||
status = self->math->motors[A3]->pDrivInt->CheckLimits(self->math->motors[A3],
|
||||
angles.a3,
|
||||
error,
|
||||
131);
|
||||
if(status != 1) {
|
||||
retVal = 0;
|
||||
snprintf(pBueffel,256,"ERROR: limit violation an a3: %s", error);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
status = self->math->motors[A4]->pDrivInt->CheckLimits(self->math->motors[A4],
|
||||
angles.sample_two_theta,
|
||||
error,
|
||||
@@ -512,13 +476,23 @@ static int calculateAndDrive(ptasMot self, SConnection *pCon){
|
||||
/*-----------------------------------------------------------------------------*/
|
||||
static int checkMotors(ptasMot self, SConnection *pCon){
|
||||
int i, status, length = 12;
|
||||
int mask[12];
|
||||
|
||||
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(self->math->motors[i] != NULL){
|
||||
if(self->math->motors[i] != NULL && mask[i] != 0){
|
||||
status = self->math->motors[i]->pDrivInt->CheckStatus(self->math->motors[i],
|
||||
pCon);
|
||||
if(status != HWIdle && status != OKOK){
|
||||
@@ -550,117 +524,86 @@ static int startQMMotors(ptasMot self, tasAngles angles,
|
||||
SConnection *pCon){
|
||||
float val;
|
||||
double curve;
|
||||
int status;
|
||||
int status, silent;
|
||||
|
||||
silent = self->math->silent;
|
||||
/*
|
||||
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;
|
||||
}
|
||||
status = startTASMotor(self->math->motors[A1],pCon, "a1",
|
||||
angles.monochromator_two_theta/2.,silent);
|
||||
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;
|
||||
}
|
||||
status = startTASMotor(self->math->motors[A2],pCon, "a2",
|
||||
angles.monochromator_two_theta,silent);
|
||||
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;
|
||||
}
|
||||
angles.monochromator_two_theta);
|
||||
status = startTASMotor(self->math->motors[MCV],pCon, "mcv",
|
||||
curve,silent);
|
||||
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;
|
||||
}
|
||||
angles.monochromator_two_theta);
|
||||
status = startTASMotor(self->math->motors[MCH],pCon, "mch",
|
||||
curve,silent);
|
||||
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.);
|
||||
status = startTASMotor(self->math->motors[A5],pCon, "a5",
|
||||
angles.analyzer_two_theta/2.0,silent);
|
||||
if(status != OKOK){
|
||||
return status;
|
||||
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);
|
||||
status = startTASMotor(self->math->motors[A6],pCon, "a6",
|
||||
angles.analyzer_two_theta,silent);
|
||||
if(status != OKOK){
|
||||
return status;
|
||||
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[ACV] != NULL){
|
||||
curve = maCalcVerticalCurvature(self->math->machine.analyzer,
|
||||
angles.analyzer_two_theta);
|
||||
status = startTASMotor(self->math->motors[ACV],pCon, "acv",
|
||||
curve,silent);
|
||||
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;
|
||||
}
|
||||
if(self->math->motors[ACH] != NULL){
|
||||
curve = maCalcHorizontalCurvature(self->math->machine.analyzer,
|
||||
angles.analyzer_two_theta);
|
||||
status = startTASMotor(self->math->motors[ACH],pCon, "ach",
|
||||
curve,silent);
|
||||
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;
|
||||
}
|
||||
status = startTASMotor(self->math->motors[A4],pCon, "a4",
|
||||
angles.sample_two_theta,silent);
|
||||
if(status != OKOK){
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
self->math->mustDrive = 0;
|
||||
return OKOK;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user