93 lines
3.4 KiB
C++
93 lines
3.4 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_ */
|