93 lines
4.3 KiB
C++
93 lines
4.3 KiB
C++
/*
|
||
* PositionMixer.h
|
||
*
|
||
* Created on: 2 мар. 2020 г.
|
||
* Author: user
|
||
*/
|
||
|
||
#ifndef UMLIBRARY_DRIVER_POSITIONMIXER_HH_
|
||
#define UMLIBRARY_DRIVER_POSITIONMIXER_HH_
|
||
|
||
#include "IncrementCounter.hh"
|
||
#include "IResolver.hh"
|
||
#include "HiperfacePositionData.hh"
|
||
|
||
#include <cmath>
|
||
|
||
namespace driver {
|
||
|
||
//!Класс реализует вычисление углового положения на основе инкрементального счетчика и резольвера.
|
||
class PositionMixer {
|
||
private:
|
||
//Источники данных о положении:
|
||
IncrementCounter increment_counter;
|
||
driver::IResolver & resolver;
|
||
|
||
const unsigned short step_per_turn; //!< Битов абсолютной (цифровой) позиции в механическом обороте.
|
||
|
||
const float resolver_to_rad; //!< Коэффициент преобразования координаты по резольверу в абсолютную.
|
||
|
||
//Это значение должно быть гарантировано больше чем значение в радианах, когда компараторы еще не переключились,
|
||
//а угол по резольверу перешел через логическую границу переключения.
|
||
const float mistrust_angle;
|
||
|
||
//Константы для корректировки углового положения:
|
||
//Ближайшая точка от нуля справа.
|
||
const float plus_zero_point_rad;
|
||
|
||
//Ближайшая точка от нуля слева.
|
||
const float minus_zero_point_rad;
|
||
|
||
//Ноль радиан слева.
|
||
const float minus_zero_rad;
|
||
|
||
// unsigned long getTurnFromAbsolutePosition(unsigned long absolute_position) const {
|
||
// return absolute_position >> step_per_turn;
|
||
// }
|
||
//
|
||
// unsigned long getAngleFromAbsolutePosition(unsigned long absolute_position) const {
|
||
// return absolute_position & static_cast<unsigned long>( ( 1 << step_per_turn ) - 1 );
|
||
// }
|
||
|
||
//Код угла, в рамках которого происходит уточнение положения по резольверу.
|
||
short pos_code = 0;
|
||
|
||
//Показывает несоответсвие кода угла сигналам от резольвера.
|
||
bool phase_error_flag = false;
|
||
|
||
public:
|
||
/// Разрядность датчика.
|
||
// const unsigned short total_number_of_step_power = 24;
|
||
|
||
/// Битов абсолютной (цифровой) позиции в периоде синуса/косинуса. Всегда.
|
||
static const unsigned short periods_in_turn_power = 5;
|
||
|
||
/// Количество отсчетов инкрементального энкодера в одном периоде синуса/косинуса, степень двойки. Всегда.
|
||
static const unsigned short line_in_period_power = 2;
|
||
|
||
/// Проверка источников позиции - потеря квадратурного сигнала - отказ.
|
||
bool isValid() const { return increment_counter.isValid(); }
|
||
|
||
/// Сброс счетчиков.
|
||
void reset() { increment_counter.resetCounter(); }
|
||
|
||
/// Предоставляет смикшированные данные в конечном виде.
|
||
PositionData getPosition();
|
||
|
||
/// Предоставляет отдельные данные о положении со всех источников.
|
||
PositionComponents getPositionComponents();
|
||
|
||
void setAbsolutePosition(unsigned long absolute_position);
|
||
|
||
PositionMixer(peripheral::ICounter & counter, unsigned long sin_period_resolution, unsigned long max_turn_value, driver::IResolver & resolver, float mistrust_angle)
|
||
: increment_counter(counter, 1 << (sin_period_resolution + line_in_period_power), max_turn_value), resolver(resolver),
|
||
resolver_to_rad(1.f/(1 << sin_period_resolution)), step_per_turn(sin_period_resolution + periods_in_turn_power),
|
||
mistrust_angle(mistrust_angle), plus_zero_point_rad(mistrust_angle*resolver_to_rad), minus_zero_point_rad((math::constants::pi_2 - mistrust_angle)*resolver_to_rad),
|
||
minus_zero_rad(math::constants::pi_2*resolver_to_rad) {}
|
||
|
||
};
|
||
|
||
} /* namespace driver */
|
||
|
||
#endif /* UMLIBRARY_DRIVER_POSITIONMIXER_HH_ */
|