91 lines
2.7 KiB
C++
91 lines
2.7 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_ */
|