91 lines
3.0 KiB
C++
91 lines
3.0 KiB
C++
/*
|
||
* IncrementCounter.h
|
||
*
|
||
* Created on: 2 мар. 2020 г.
|
||
* Author: user
|
||
*/
|
||
|
||
#ifndef UMLIBRARY_DRIVER_INCREMENTCOUNTER_HH_
|
||
#define UMLIBRARY_DRIVER_INCREMENTCOUNTER_HH_
|
||
|
||
#include "../peripheral/ICounter.hh"
|
||
|
||
#include "../math/math_inc.hh"
|
||
|
||
namespace driver {
|
||
|
||
//!Код позиции.
|
||
struct PositionCode {
|
||
long angle_data; //!< Положение внутри оборта в отсчетах.
|
||
unsigned long turn; //!< Номер оборота.
|
||
|
||
//
|
||
static PositionCode applyOffset(const PositionCode & pos, const PositionCode & offset,
|
||
const long line_in_turn, const unsigned long max_turn_value) {
|
||
|
||
long temp_angle_data = pos.angle_data + offset.angle_data;
|
||
|
||
long temp_turn = temp_angle_data / line_in_turn;
|
||
|
||
temp_angle_data = temp_angle_data - temp_turn * line_in_turn;
|
||
|
||
if( temp_angle_data < 0 ) {
|
||
temp_angle_data += line_in_turn;
|
||
temp_turn -= 1;
|
||
}
|
||
|
||
temp_turn += pos.turn + offset.turn;
|
||
temp_turn -= ( temp_turn / max_turn_value ) * max_turn_value;
|
||
|
||
return { temp_angle_data, temp_turn };
|
||
}
|
||
};
|
||
|
||
//!Класс реализует инкрементальный датчик положения, на основе модуля квадратурного энкодера.
|
||
class IncrementCounter {
|
||
private:
|
||
peripheral::ICounter & counter;
|
||
|
||
/// Количество отсчетов в одном механическом обороте датчика.
|
||
const unsigned long line_in_turn;
|
||
|
||
/// Количество механических оборотов многооборотного датчика.
|
||
const unsigned long max_turn_value;
|
||
|
||
/// Коэффициент преобразования кода угла в угловое положение.
|
||
const float _1_line_to_rad = math::constants::pi2 / line_in_turn;
|
||
|
||
PositionCode pos_code = { .angle_data = 0, .turn = 0 };
|
||
|
||
public:
|
||
|
||
float position_to_rad(unsigned long position) const { return _1_line_to_rad*position; }
|
||
|
||
unsigned long position_to_increments(const PositionCode & code) const { return code.turn*line_in_turn + code.angle_data; }
|
||
|
||
/// Обновление показаний датчика.
|
||
operator PositionCode();
|
||
|
||
PositionCode toCorrectPositionCode(PositionCode code) {
|
||
if (code.turn >= max_turn_value) {
|
||
code.turn -= static_cast<int>(code.turn/max_turn_value)*max_turn_value;
|
||
}
|
||
|
||
return { code.angle_data, code.turn };
|
||
}
|
||
|
||
bool isValid() const { return not counter.isError(); }
|
||
|
||
void resetCounter() { pos_code.angle_data = pos_code.turn = 0; counter.resetError(); }
|
||
|
||
void setCounter(const PositionCode & position_code) { pos_code = position_code; }
|
||
|
||
IncrementCounter(peripheral::ICounter & counter, unsigned long line_in_turn, unsigned long max_turn_value)
|
||
: counter(counter), line_in_turn(line_in_turn), max_turn_value(max_turn_value) {}
|
||
|
||
};
|
||
|
||
} /* namespace driver */
|
||
|
||
#endif /* UMLIBRARY_DRIVER_INCREMENTCOUNTER_HH_ */
|