163 lines
4.6 KiB
C++
163 lines
4.6 KiB
C++
/*
|
|
* TrackingPositionEncoder.h
|
|
*
|
|
* Created on: 22 ìàÿ 2018 ã.
|
|
* Author: krugliy
|
|
*/
|
|
|
|
#ifndef SOURCE_PROCESSING_TRACKINGPOSITIONENCODER_H_
|
|
#define SOURCE_PROCESSING_TRACKINGPOSITIONENCODER_H_
|
|
|
|
#include "../driver/IEncoder.hh"
|
|
|
|
#include "../math/math_inc.hh"
|
|
|
|
#include <stdint.h>
|
|
|
|
namespace processing {
|
|
|
|
//!Ìîäóëü îñóùåñòâëÿåò âîññòàíîâëåíèå ïóòåâîé èíôîðìàöèè ïî äàííûì ïîëó÷åííûì îò öèôðîâîãî ýíêîäåðà.
|
|
/*Â îñíîâå àëãîðèòìà âîññòàíîâëåíèÿ ëåæèò íàáëþäàòåëü ïåðâîãî ïîðÿäêà.
|
|
* Ïðîèçâîäèòüñÿ ó÷åò òàêèõ ôàêòîðîâ êàê:
|
|
* * Äèñêðåòèçàöèÿ äàííûõ ýíêîäåðà ïî óðîâíþ;
|
|
* * Êâàíòîâàíèå äàííûõ ýíêîäåðà ïî âðåìåíè;
|
|
* * Ïðîïóñê äàííûõ è èñïðàâëåíèå åäèíè÷íûõ ñáîåâ ýíêîäåðà;
|
|
* * Ñìåùåíèå "íóëåâîãî" îáîðîòà äàííûõ äëÿ ðàáîòû â êîìôîðòíîé øêàëå ïîçèöèè.
|
|
*/
|
|
class TrackingPositionEncoder {
|
|
public:
|
|
typedef driver::IEncoder TrackerInterface;
|
|
|
|
struct Setting {
|
|
float L1; //!<Êîýôôèöèåíò ñëåäÿùåé ñèñòåìû, êîððåêöèÿ ïîçèöèè.
|
|
float L2; //!<Êîýôôèöèåíò ñëåäÿùåé ñèñòåìû, êîððåêöèÿ ñêîðîñòè.
|
|
float max_speed; //!<Ìàêñèìàëüíî äîïóñòèìàÿ ñêîðîñòü ýíêîäåðà.
|
|
float quant_error; //!<Êâàíòîâàíèå îøèáêè ñëåäÿùåé ñèñòåìû.
|
|
float error_demping; //!<Êîýôôèöèåíò äåìïôèðîâàíèÿ ñèñòåìû.
|
|
float turn_offset; //!<Ñìåùåíèå îáîðîòîâ ñèñòåìû êîîðäèíàò.
|
|
|
|
bool isValid() {
|
|
|
|
using namespace std;
|
|
|
|
const Setting notValidSetting;
|
|
|
|
if( L1 < 0.0f )
|
|
L1 = notValidSetting.L1;
|
|
|
|
if( L2 < 0.0f )
|
|
L2 = notValidSetting.L2;
|
|
|
|
if( L1 > L2 ) {
|
|
L1 = notValidSetting.L1;
|
|
L2 = notValidSetting.L2;
|
|
}
|
|
|
|
if( max_speed <= 0.0f )
|
|
max_speed = notValidSetting.max_speed;
|
|
|
|
if( quant_error < 0.0f )
|
|
quant_error = notValidSetting.quant_error;
|
|
|
|
if( error_demping < 0.0f or error_demping > 1.0f )
|
|
error_demping = notValidSetting.error_demping;
|
|
|
|
if( not isfinite( turn_offset ) )
|
|
turn_offset = notValidSetting.turn_offset;
|
|
|
|
bool result =
|
|
L1 != notValidSetting.L1
|
|
and L2 != notValidSetting.L2
|
|
and max_speed != notValidSetting.max_speed
|
|
and quant_error != notValidSetting.quant_error
|
|
and error_demping != notValidSetting.error_demping
|
|
and turn_offset != notValidSetting.turn_offset;
|
|
|
|
return result;
|
|
|
|
}
|
|
|
|
Setting() : L1(NAN), L2(NAN), turn_offset(NAN),
|
|
max_speed(NAN), quant_error(-1.0f), error_demping(-1.0f) {}
|
|
|
|
};
|
|
|
|
private:
|
|
driver::IEncoder & encoder;
|
|
|
|
float prev_n1_encoder_angle;
|
|
float prev_n2_encoder_angle;
|
|
|
|
float position_error;
|
|
float est_position;
|
|
float est_speed;
|
|
float est_angle;
|
|
float est_turn;
|
|
|
|
float set_turn_offset;
|
|
float turn_offset;
|
|
|
|
float _Ts;
|
|
float _1_Ts;
|
|
float _L1Ts;
|
|
float _L2Ts;
|
|
float _Tun; //!<Äîïóñòèìîå âðåìÿ áåç äàííûõ.
|
|
float _DTs; //!<Ìàêñèìàëüíîå ïðèðàùåíèå óãëà çà öèêë.
|
|
float _Qe; //!<Êâàíîâàíèå îøèáêè ñëåäÿùåé ñèñòåìû.
|
|
float _1_Qe; //!<Êâàíîâàíèå îøèáêè ñëåäÿùåé ñèñòåìû.
|
|
float _demping; //!<
|
|
|
|
unsigned int maxcount_error;
|
|
unsigned int count_error;
|
|
|
|
unsigned int repeat_counter;
|
|
unsigned int max_repeat;
|
|
|
|
float prev_angle; //!<Ïðåäûäóùåå çíà÷åíèå óãëà îò ýíêîäåðà.
|
|
float prev_turn; //!<Ïðåäûäóùåå çíà÷åíèå îáîðîòîâ îò ýíêîäåðà.
|
|
bool prev_sample_actual; //!<Ïðåäûäóùåå èçìåðåíèå àêòóàëüíî.
|
|
|
|
uint32_t max_turn;
|
|
uint32_t half_turn;
|
|
|
|
float turnover(float raw_turn);
|
|
void miss(const float demping);
|
|
void angle_tuning(float raw_angle, float raw_turn, float &encoder_angle,
|
|
float &encoder_turn);
|
|
|
|
const bool tuning;
|
|
|
|
public:
|
|
TrackingPositionEncoder( driver::IEncoder & encoder, uint32_t max_turn, bool tuning = true );
|
|
~TrackingPositionEncoder() = default;
|
|
|
|
void configure( const Setting & setting );
|
|
|
|
void setSampleTime( float Ts );
|
|
void process();
|
|
|
|
//!Ìåòîä ïîçâîëÿåò óñòàíîâèòü íîâîå ñìåùåíèå íóëåâîãî îáîðîòà.
|
|
void setTurnOffset( float turn_offset );
|
|
|
|
//!Òåêóùàÿ îöåíêà ïîçèöèè.
|
|
const float & position() { return est_position; }
|
|
//!Òåêóùàÿ îöåíêà ñêîðîñòè
|
|
const float & speed() { return est_speed; }
|
|
|
|
const float & pos_error() { return position_error; }
|
|
|
|
//!Îöåíêà îáîðîòîâ.
|
|
const float & turn() { return est_turn; }
|
|
|
|
//!Îöåíêà óãëà.
|
|
const float & angle() { return est_angle; }
|
|
|
|
//!Ôóíêöèÿ âîçðàùàåò ïðèçíàê îòêàçà â îáðàáîòêå ïîëîæåíèÿ/ñêîðîñòè.
|
|
bool isFailure() const { return count_error == maxcount_error; }
|
|
|
|
};
|
|
|
|
} /* namespace processing */
|
|
|
|
#endif /* SOURCE_PROCESSING_TRACKINGPOSITIONENCODER_H_ */
|