91 lines
2.7 KiB
C++
91 lines
2.7 KiB
C++
|
/*
|
|||
|
|
* IncrementCounter.h
|
|||
|
|
*
|
|||
|
|
* Created on: 2 <EFBFBD><EFBFBD><EFBFBD>. 2020 <EFBFBD>.
|
|||
|
|
* Author: user
|
|||
|
|
*/
|
|||
|
|
|
|||
|
|
#ifndef UMLIBRARY_DRIVER_INCREMENTCOUNTER_HH_
|
|||
|
|
#define UMLIBRARY_DRIVER_INCREMENTCOUNTER_HH_
|
|||
|
|
|
|||
|
|
#include "../peripheral/ICounter.hh"
|
|||
|
|
|
|||
|
|
#include "../math/math_inc.hh"
|
|||
|
|
|
|||
|
|
namespace driver {
|
|||
|
|
|
|||
|
|
//!<21><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
|
|||
|
|
struct PositionCode {
|
|||
|
|
long angle_data; //!< <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
|
|||
|
|
unsigned long turn; //!< <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
|
|||
|
|
|
|||
|
|
//
|
|||
|
|
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 };
|
|||
|
|
}
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
//!<21><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
|
|||
|
|
class IncrementCounter {
|
|||
|
|
private:
|
|||
|
|
peripheral::ICounter & counter;
|
|||
|
|
|
|||
|
|
/// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
|
|||
|
|
const unsigned long line_in_turn;
|
|||
|
|
|
|||
|
|
/// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
|
|||
|
|
const unsigned long max_turn_value;
|
|||
|
|
|
|||
|
|
/// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
|
|||
|
|
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; }
|
|||
|
|
|
|||
|
|
/// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
|
|||
|
|
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_ */
|