163 lines
5.5 KiB
C++
163 lines
5.5 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_ */
|