diff --git a/src/Stage2Filter.cpp b/src/Stage2Filter.cpp index ca37098383bb3a06e4aed26cca85d855d7ab7597..e062b990c16728f6a000c0704db28f1baec641c9 100644 --- a/src/Stage2Filter.cpp +++ b/src/Stage2Filter.cpp @@ -283,42 +283,18 @@ bool Stage2Filter::correct_position(itpp::vec Focus){ #ifdef _DBG_STAGE2FILTER cout << __func__ << " ... search within discrete positions ...." << endl; #endif - vector<nominal_pos_t>::iterator it; - bool newpos_in_discrete = false; - for ( it = _sel.nominal_pos.begin(); it != _sel.nominal_pos.end(); ++it){ - #ifdef _DBG_STAGE2FILTER - std::cout << " ------:: " << __func__ << " "; - std::cout << (*it).pos << " " << (*it).delta; - std::cout << std::endl; - #endif - - if ( fabs(newpos - (*it).pos) <= (*it).delta ) { - #ifdef _DBG_STAGE2FILTER - cout << __func__ << " ... FOUND discrete position .... for " << newpos << endl; - #endif - - newpos_in_discrete = true; - break; - } - } - if ( !newpos_in_discrete ) { - #ifdef _DBG_STAGE2FILTER - cout << __func__ << " ... search approx discrete position ...." << endl; - #endif - - vector<double> dist; - vector<double> target; - for ( it = _sel.nominal_pos.begin(); it != _sel.nominal_pos.end(); ++it){ - dist.push_back(fabs(newpos - (*it).pos)); - target.push_back((*it).pos); - } - - int minElementIndex = std::min_element(dist.begin(),dist.end()) - dist.begin(); - newpos = target[minElementIndex]; + // + // Codice di sotto: cosi' non si 'esce' dal selector corrente: + // + if ( fabs(d2) > fabs(_sel.nominal_pos[_sel.selector].delta) ) { + double v1 = fabs(newpos - (_sel.nominal_pos[_sel.selector].pos + _sel.nominal_pos[_sel.selector].delta) ); + double v2 = fabs(newpos - (_sel.nominal_pos[_sel.selector].pos - _sel.nominal_pos[_sel.selector].delta) ); + if ( v1 < v2 ) newpos = _sel.nominal_pos[_sel.selector].pos + 0.99 * _sel.nominal_pos[_sel.selector].delta; + else newpos = _sel.nominal_pos[_sel.selector].pos - 0.99 * _sel.nominal_pos[_sel.selector].delta; #ifdef _DBG_STAGE2FILTER - cout << __func__ << " ... NEW approx discrete position ....: " << newpos << endl; + cout << __func__ << " ... NEW approx discrete position ....: " << newpos << " on selector #:" << _sel.selector << endl; #endif }