Commit 8f9b3a59 authored by Alessandro Abrami's avatar Alessandro Abrami
Browse files

2021/06/08:

	BRANCH(withFilters): primo funzionamento ok.
parent 91441ce8
MYNOTES.txt
MSG.txt
ken-pespccdstage-srv_mag_pesp.txt
pespccdstage-srv_mag_pesp.txt
......@@ -25,6 +25,9 @@ CXXFLAGS += -D_DBG_PARAMETERS
#CXXFLAGS += -D_WITHOUT_DIPLACEMENT
CXXFLAGS += -D_DBG_STAGE2FILTER
CXXFLAGS += -D_DBG_TARDI_FUNCS
##----------------------------------------------------------------------------
#
#
......
......@@ -182,6 +182,11 @@ void PespCCDStage::delete_device()
pstage = NULL;
}
if (p_stage2flt) {
delete p_stage2flt;
p_stage2flt = NULL;
}
CORBA::string_free(attr_StageType_read[0]);
......@@ -371,6 +376,14 @@ typedef struct {
pstage = NULL;
*attr_StageType_read = Tango::string_dup("Error");
}
//===============================================================
// - - - - FILTERS
//
//p_stage2flt = new Stage2Filter(std::string dev, axis_nick, itpp::vec R /*pto in xOz*/, double df);
p_stage2flt = new PespStage_ns::Stage2Filter(this, "tango://srv-tango-padres-01:20000/pos_mag/mover1d/flt_mrc_pos_mag.01", "X", __cr, double(40));
//===============================================================
} else {
pstage = NULL;
string s("Error :?");
......@@ -576,7 +589,6 @@ typedef struct {
first_write_YagSelector = true;
first_write_BeamOnYagDisplacement = true;
if (init_ok){
set_state(Tango::ON);
set_status("Ready.");
......@@ -896,27 +908,34 @@ void PespCCDStage::write_FocusX(Tango::WAttribute &attr)
if (get_state() == Tango::FAULT) return;
itpp::vec newXZ(2);
try {
itpp::vec newccd;
INFO_STREAM << "PespCCDStage::write_FocusX("<<w_val <<")"<< endl;
itpp::vec newccd;
INFO_STREAM << "PespCCDStage::write_FocusX("<<w_val <<")"<< endl;
itpp::vec newXZ(2);
newXZ(0)=w_val;
newXZ(1)=(*attr_FocusZ_read);
newccd=pstage->calculateXZ2NL(newXZ);
newXZ(0)=w_val;
newXZ(1)=(*attr_FocusZ_read);
newccd=pstage->calculateXZ2NL(newXZ);
//AAA
#ifndef _DONT_WRITECENTERX
double v = newccd(0);
Tango::DeviceAttribute daN(ccdstageN.move_attr, v);
ccdstageN.dp->write_attribute(daN);
v = newccd(1);
Tango::DeviceAttribute daL(ccdstageL.move_attr, v);
ccdstageL.dp->write_attribute(daL);
#endif
#ifndef _DONT_WRITECENTERX
double v = newccd(0);
Tango::DeviceAttribute daN(ccdstageN.move_attr, v);
ccdstageN.dp->write_attribute(daN);
v = newccd(1);
Tango::DeviceAttribute daL(ccdstageL.move_attr, v);
ccdstageL.dp->write_attribute(daL);
#endif
} catch(...) {
ERROR_STREAM << "Write attribute FAILED !!!" << endl;
}
//FF
itpp::vec Focus = newXZ;
if (!p_stage2flt->correct_position(Focus)){
INFO_STREAM << "PespCCDStage::write_FocusX() error on filters"<< endl;
}
} catch(...) {
ERROR_STREAM << "Write attribute FAILED !!!" << endl;
}
/*----- PROTECTED REGION END -----*/ // PespCCDStage::write_FocusX
}
......@@ -956,27 +975,33 @@ void PespCCDStage::write_FocusZ(Tango::WAttribute &attr)
if (get_state() == Tango::FAULT) return;
itpp::vec newXZ(2);
try {
itpp::vec newccd;
INFO_STREAM << "PespCCDStage::write_FocusZ("<<w_val <<")"<< endl;
itpp::vec newccd;
INFO_STREAM << "PespCCDStage::write_FocusZ("<<w_val <<")"<< endl;
itpp::vec newXZ(2);
newXZ(0)=(*attr_FocusX_read);
newXZ(1)=w_val;
newXZ(0)=(*attr_FocusX_read);
newXZ(1)=w_val;
newccd=pstage->calculateXZ2NL(newXZ);
newccd=pstage->calculateXZ2NL(newXZ);
//AAA
double v = newccd(0);
Tango::DeviceAttribute daN(ccdstageN.move_attr, v);
ccdstageN.dp->write_attribute(daN);
v = newccd(1);
Tango::DeviceAttribute daL(ccdstageL.move_attr, v);
ccdstageL.dp->write_attribute(daL);
double v = newccd(0);
Tango::DeviceAttribute daN(ccdstageN.move_attr, v);
ccdstageN.dp->write_attribute(daN);
v = newccd(1);
Tango::DeviceAttribute daL(ccdstageL.move_attr, v);
ccdstageL.dp->write_attribute(daL);
} catch(...) {
ERROR_STREAM << "Write attribute FAILED !!!" << endl;
}
} catch(...) {
ERROR_STREAM << "Write attribute FAILED !!!" << endl;
}
//FF
itpp::vec Focus = newXZ;
if (!p_stage2flt->correct_position(Focus)){
INFO_STREAM << "PespCCDStage::write_FocusZ() error on filters"<< endl;
}
/*----- PROTECTED REGION END -----*/ // PespCCDStage::write_FocusZ
}
......@@ -1016,33 +1041,39 @@ void PespCCDStage::write_FocusDistance(Tango::WAttribute &attr)
if (get_state() == Tango::FAULT) return;
itpp::vec newXZ(2);
try {
itpp::vec newccd;
INFO_STREAM << "PespCCDStage::write_FocusDistance("<<w_val <<")"<< endl;
itpp::vec newccd;
INFO_STREAM << "PespCCDStage::write_FocusDistance("<<w_val <<")"<< endl;
itpp::vec newXZ(2);
itpp::vec newBR(2);
itpp::vec newBR(2);
newBR(0)=(*attr_FocusAngle_read);
newBR(1)=w_val;
newBR(0)=(*attr_FocusAngle_read);
newBR(1)=w_val;
newXZ=pstage->calculateBR2XZ(newBR);
newccd=pstage->calculateXZ2NL(newXZ);
newXZ=pstage->calculateBR2XZ(newBR);
newccd=pstage->calculateXZ2NL(newXZ);
//AAA
#ifndef _DONT_WRITE_BR
double v = newccd(0);
Tango::DeviceAttribute daN(ccdstageN.move_attr, v);
ccdstageN.dp->write_attribute(daN);
v = newccd(1);
Tango::DeviceAttribute daL(ccdstageL.move_attr, v);
ccdstageL.dp->write_attribute(daL);
#endif
#ifndef _DONT_WRITE_BR
double v = newccd(0);
Tango::DeviceAttribute daN(ccdstageN.move_attr, v);
ccdstageN.dp->write_attribute(daN);
v = newccd(1);
Tango::DeviceAttribute daL(ccdstageL.move_attr, v);
ccdstageL.dp->write_attribute(daL);
#endif
} catch(...) {
ERROR_STREAM << "Write attribute FAILED !!!" << endl;
}
} catch(...) {
ERROR_STREAM << "Write attribute FAILED !!!" << endl;
}
//FF
itpp::vec Focus = newXZ;
if (!p_stage2flt->correct_position(Focus)){
INFO_STREAM << "PespCCDStage::write_FocusDistance() error on filters"<< endl;
}
/*----- PROTECTED REGION END -----*/ // PespCCDStage::write_FocusDistance
}
......@@ -1082,32 +1113,38 @@ void PespCCDStage::write_FocusAngle(Tango::WAttribute &attr)
if (get_state() == Tango::FAULT) return;
itpp::vec newXZ(2);
try {
itpp::vec newccd;
INFO_STREAM << "PespCCDStage::write_FocusAngle("<<w_val <<")"<< endl;
itpp::vec newccd;
INFO_STREAM << "PespCCDStage::write_FocusAngle("<<w_val <<")"<< endl;
itpp::vec newXZ(2);
itpp::vec newBR(2);
itpp::vec newBR(2);
newBR(0)=w_val;
newBR(1)=(*attr_FocusDistance_read);
newBR(0)=w_val;
newBR(1)=(*attr_FocusDistance_read);
newXZ=pstage->calculateBR2XZ(newBR);
newccd=pstage->calculateXZ2NL(newXZ);
newXZ=pstage->calculateBR2XZ(newBR);
newccd=pstage->calculateXZ2NL(newXZ);
//AAA
#ifndef _DONT_WRITE_BR
double v = newccd(0);
Tango::DeviceAttribute daN(ccdstageN.move_attr, v);
ccdstageN.dp->write_attribute(daN);
v = newccd(1);
Tango::DeviceAttribute daL(ccdstageL.move_attr, v);
ccdstageL.dp->write_attribute(daL);
#endif
#ifndef _DONT_WRITE_BR
double v = newccd(0);
Tango::DeviceAttribute daN(ccdstageN.move_attr, v);
ccdstageN.dp->write_attribute(daN);
v = newccd(1);
Tango::DeviceAttribute daL(ccdstageL.move_attr, v);
ccdstageL.dp->write_attribute(daL);
#endif
} catch(...) {
ERROR_STREAM << "Write attribute FAILED !!!" << endl;
}
} catch(...) {
ERROR_STREAM << "Write attribute FAILED !!!" << endl;
}
//FF
itpp::vec Focus = newXZ;
if (!p_stage2flt->correct_position(Focus)){
INFO_STREAM << "PespCCDStage::write_FocusDistance() error on filters"<< endl;
}
/*----- PROTECTED REGION END -----*/ // PespCCDStage::write_FocusAngle
}
......@@ -1200,6 +1237,20 @@ void PespCCDStage::write_BeamOnYagDisplacement(Tango::WAttribute &attr)
}
first_write_BeamOnYagDisplacement = false;
//
// il Focus in XZ non e' cambiato, ma e' cambiato lo stage sottostante (a causa del nuovo displacement)
// pertanto e' cambiata anche la posizione del tubo/naso
// quindi richiedo il cambiamento anche se con lo stesso Focus.
//FF
itpp::vec Focus = oldXZ;
if (!p_stage2flt->correct_position(Focus)){
INFO_STREAM << "PespCCDStage::write_FocusDistance() error on filters"<< endl;
}
/*----- PROTECTED REGION END -----*/ // PespCCDStage::write_BeamOnYagDisplacement
}
//--------------------------------------------------------
......@@ -1348,6 +1399,19 @@ void PespCCDStage::write_YagSelector(Tango::WAttribute &attr)
first_write_YagSelector = false;
//
// il Focus in XZ non e' cambiato, ma e' cambiato lo stage sottostante (a causa del nuovo yag)
// pertanto e' cambiata anche la posizione del tubo/naso
// quindi richiedo il cambiamento anche se con lo stesso Focus.
//FF
if (attr_UpdateOnSelector && !first_write_YagSelector){
itpp::vec Focus = oldXZ;
if (!p_stage2flt->correct_position(Focus)){
INFO_STREAM << "PespCCDStage::write_FocusDistance() error on filters"<< endl;
}
}
/*----- PROTECTED REGION END -----*/ // PespCCDStage::write_YagSelector
}
//--------------------------------------------------------
......@@ -1481,6 +1545,12 @@ Tango::DevBoolean PespCCDStage::set_angle_distance(const Tango::DevVarDoubleArra
ERROR_STREAM << "PespCCDStage::set_angle_distance Write CCDStageAxisL attribute FAILED !!!" << endl;
}
//FF
itpp::vec Focus = newXZ;
if (!p_stage2flt->correct_position(Focus)){
INFO_STREAM << "PespCCDStage::write_FocusDistance() error on filters"<< endl;
}
argout = true;
/*----- PROTECTED REGION END -----*/ // PespCCDStage::set_angle_distance
......@@ -1684,7 +1754,9 @@ void PespCCDStage::updateattributes(){
*attr_FocusDistance_read = br[1];
if (!p_stage2flt->update()) {
ERROR_STREAM << "Read filters FAILED !!!" << endl;
}
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
......@@ -1768,5 +1840,13 @@ void PespCCDStage::updatestatus(){
}
//FF
//
double PespCCDStage::filter_distance(){ // filter distance from R
return 10;
}
/*----- PROTECTED REGION END -----*/ // PespCCDStage::namespace_ending
} // namespace
......@@ -60,6 +60,8 @@ typedef struct {
} mover_t;
#include "Stage2Filter.h"
/*----- PROTECTED REGION END -----*/ // PespCCDStage.h
......@@ -82,6 +84,8 @@ class PespCCDStage : public TANGO_BASE_CLASS
/*----- PROTECTED REGION ID(PespCCDStage::Data Members) ENABLED START -----*/
// Add your own data members
private:
PespStage_ns::Stage2Filter* p_stage2flt;
public:
mover_t ccdstageN;
mover_t ccdstageL;
......@@ -399,6 +403,9 @@ protected :
void updateattributes();
void updatestatus();
public:
double filter_distance(); // filter distance from R
/*----- PROTECTED REGION END -----*/ // PespCCDStage::Additional Method prototypes
};
......
#include "PespStage.h"
namespace PespStage_ns
{
/********************************************************************
*
* retta parametrica ortogonale a 'rtta' e passante per il punto 'pto'
*
********************************************************************/
itpp::mat PespStage::rettap_orto(itpp::mat rtta, itpp::vec pto) {
itpp::mat rmat(2,2);
double m = sqrt( pow(rtta(1,1),2) + pow(rtta(0,1),2) );
rmat(0,0) = pto(0);
rmat(1,0) = pto(1);
rmat(0,1) = -rtta(1,1)/m;
rmat(1,1) = rtta(0,1)/m;
return rmat;
}
/**********************************************************
*
* retta parametrica da O (0,0) a P in xOz
*
**********************************************************/
itpp::mat PespStage::rettap_P_xOz(itpp::vec p) {
itpp::mat rmat(2,2);
rmat(0,0) = 0;
rmat(1,0) = 0;
rmat(0,1) = p(0);
rmat(1,1) = p(1);
return rmat;
}
/**********************************************************
*
* distanza di riferimento, ovvero tra i due punti che hanno generato la retta
*
**********************************************************/
double PespStage::rettap_dBase(itpp::mat retta) {
//retta(0,0) ... primo punto asse x
//retta(1,0) ... primo punto asse z
//retta(0,1) ... distanza in x tra i due pti
//retta(1,1) ... distanza in z tra i due pti
double dbase = sqrt( pow(retta(1,1),2) + pow(retta(0,1),2) );
return dbase;
}
/**********************************************************
*
* intercetta tra due rette parametriche
*
**********************************************************/
itpp::vec PespStage::intercetta(itpp::mat rettap_1, itpp::mat rettap_2){
itpp::vec r(2);
itpp::mat m = inv(rettap_2) * rettap_1;
if (m(0,1) != 0.0 ){
double t1 = (1 - m(0,0))/m(0,1);
itpp::vec t(2);
t(0) = 1;
t(1) = t1;
r = rettap_1 * t;
} else {
r(0) = nan("");
r(1) = nan("");
}
return r;
}
/**********************************************************
*
* distanza tra due pti
*
**********************************************************/
double PespStage::d_p1p2(itpp::vec p1, itpp::vec p2){
return sqrt(pow(p1(0) - p2(0),2) + pow(p1(1) - p2(1),2));
}
} // End of namespace
......@@ -47,6 +47,18 @@ public:
double get_displacement() { return displacement;};
void set_displacement(double v) { displacement = v;};
itpp::mat rettap_orto(itpp::mat rtta, itpp::vec pto);
virtual itpp::mat rettapTubo_xOz(itpp::vec Focus) = 0; // retta parametrica dell'asse del tubo/naso: da R a Focus
itpp::mat rettap_P_xOz(itpp::vec p);
double rettap_dBase(itpp::mat retta); // distanza di riferimento, ovvero tra i due punti che hanno generato la retta
itpp::vec intercetta(itpp::mat rettap_1, itpp::mat rettap_2);
double d_p1p2(itpp::vec p1, itpp::vec p2); //distanza tra due pti
};
//=======================================================================
......@@ -70,6 +82,9 @@ public:
itpp::vec get_yag_NL() { return _offset; };
void select_yag(int k) { _k = k; _offset = _yagsNL[k]; };
int get_select_yag() { return _k; };
itpp::mat rettapTubo_xOz(itpp::vec Focus); // retta parametrica dell'asse del tubo/naso
};
//=======================================================================
......@@ -89,8 +104,6 @@ public:
itpp::mat rettap_nQl(itpp::vec qp);
itpp::mat rettap_xOz(itpp::vec p);
itpp::mat rettap_orto(itpp::mat rtta, itpp::vec pto);
itpp::vec pp2p(itpp::vec pp);
void set_rotcenter(itpp::vec cr); // in xOz
......@@ -105,6 +118,10 @@ public:
itpp::vec calculateBR2XZ(itpp::vec p);
itpp::vec calculateXZ2BR(itpp::vec Focus);
itpp::vec Focus2Qp(itpp::vec Focus); // calc Q' da Focus (with displacement)
itpp::mat rettapTubo_xOz(itpp::vec Focus); // retta parametrica dell'asse del tubo/naso
};
} // End of namespace
......
......@@ -194,6 +194,15 @@ double m, x1, z1, x2, z2;
return ris;
}
/**********************************************************
*
* retta parametrica dell'asse del tubo/naso
*
**********************************************************/
itpp::mat PrestoStage::rettapTubo_xOz(itpp::vec Focus){
}
/*----- PROTECTED REGION END -----*/ // PespCCDStage::namespace_ending
} // namespace
#include "Stage2Filter.h"
#include "PespCCDStage.h"
typedef struct {
double a; //x
double b; //y
double c; //z
double d; //cost.
} rettan_t;
static rettan_t retta_p2n(itpp::mat rettap){
rettan_t rettan;
rettan.b = 0; // siamo nel piano xz
cout << ">>>>>------- " << __func__ << " rettap= " << rettap;
cout << endl;
rettan.a = 1;
rettan.c = -(rettap(0,1) / rettap(1,1));
rettan.d = -rettap(0,0)+ (rettap(0,1) / rettap(1,1)) * rettap(1,0);
return rettan;
}
namespace PespStage_ns
{
Stage2Filter::Stage2Filter(PespCCDStage_ns::PespCCDStage *ccdstage, std::string dev, std::string axis_nick, itpp::vec R /*pto in xOz*/, double df){
#ifdef _DBG_STAGE2FILTER
std::cout << " ------:: " << __func__ << " :: ctor(";
std::cout << "dev=" << dev << " ";
std::cout << "axis_nick=" << axis_nick << " ";
std::cout << "R=" << R << " ";
std::cout << "df=" << df << " ";
std::cout << ")" << std::endl;
#endif
_ccdstage = ccdstage;
_sel.dev = dev;
_sel.axis_nickName = axis_nick;
_sel.move_attr = axis_nick;
_sel.configured = false;
_R = R;
_df = df;
_enable = false;
/* check if device is responding */
try {
_sel.dp = new Tango::DeviceProxy(_sel.dev);
#ifdef _DBG_STAGE2FILTER
std::cout << "Connected to device '" + _sel.dev + "'"<< std::endl;
#endif
float pingtime;
pingtime = _sel.dp->ping();
}
catch(Tango::DevFailed e) {
#ifdef _DBG_STAGE2FILTER
std::cout << "Failed to connect to device '" + _sel.dev + "'"<< std::endl;
#endif
_sel.dp = NULL;
return;
}
// get number of selectors
//
_sel.no_of_instruments = -1;
Tango::DeviceData dd = _sel.dp->command_inout("GetInstrumentList");
vector<string> vs;
dd >> vs;
_sel.no_of_instruments = vs.size();
vector<string>::iterator it;
#ifdef _DBG_STAGE2FILTER
std::cout << "len(GetInstrumentList)=" << _sel.no_of_instruments << std::endl;
for (it = vs.begin(); it != vs.end(); ++it)
std::cout << "(GetInstrumentList)=" << *it << std::endl;
#endif
// get nominal pos
//
Tango::DeviceData din;
Tango::DevVarLongStringArray vlsa;
vector<double> vd;
vlsa.lvalue.length(1);
vlsa.svalue.length(1);
vlsa.svalue[0] = CORBA::string_dup(_sel.axis_nickName.c_str());