63 lines
1.7 KiB
C++
63 lines
1.7 KiB
C++
/*
|
||
* IEncoder.cpp
|
||
*
|
||
* Created on: 1 февр. 2022 г.
|
||
* Author: titov
|
||
*/
|
||
|
||
#include "IEncoder.hh"
|
||
|
||
#include "../math/math_inc.hh"
|
||
|
||
std::pair<driver::IEncoder::Turn, driver::IEncoder::Angle> driver::IEncoder::delta(
|
||
std::pair<Turn, Angle> point_0, std::pair<Turn, Angle> point_1 ) {
|
||
|
||
Angle point_1_angle_on_point_0_turn = in_turn( point_1, point_0.first );
|
||
point_0.second -= point_1_angle_on_point_0_turn;
|
||
return normalize( point_0 );
|
||
|
||
}
|
||
|
||
driver::IEncoder::Angle driver::IEncoder::in_turn(std::pair<Turn, Angle> point, Turn base) {
|
||
|
||
Turn delta = point.first - base;
|
||
|
||
return point.second + delta * math::constants::pi2;
|
||
|
||
}
|
||
|
||
std::pair<driver::IEncoder::Turn, driver::IEncoder::Angle> driver::IEncoder::normalize( std::pair<Turn, Angle> point ) {
|
||
|
||
using namespace std;
|
||
|
||
while( point.second > math::constants::pi2 and isfinite(point.second) ) {
|
||
|
||
point.second = fmaxf( point.second - math::constants::pi2, 0.0f );
|
||
point.first += 1;
|
||
|
||
}
|
||
|
||
while( point.second < 0.0f and isfinite(point.second) ) {
|
||
|
||
point.second = fminf( point.second + math::constants::pi2, math::constants::pi2 );
|
||
point.first -= 1;
|
||
|
||
}
|
||
|
||
return point;
|
||
|
||
}
|
||
|
||
std::pair<driver::IEncoder::Turn, driver::IEncoder::Angle> stable_normalize( std::pair<driver::IEncoder::Turn, driver::IEncoder::Angle> point ) {
|
||
short extra_turn = point.second / math::constants::pi2;
|
||
driver::IEncoder::Angle angle_in_turn = point.second - extra_turn * math::constants::pi2;
|
||
|
||
if( angle_in_turn < 0.0f ) {
|
||
extra_turn -= 1;
|
||
angle_in_turn += math::constants::pi2;
|
||
}
|
||
|
||
return std::pair<driver::IEncoder::Turn, driver::IEncoder::Angle>( extra_turn, angle_in_turn );
|
||
|
||
}
|