63 lines
1.7 KiB
C++
63 lines
1.7 KiB
C++
|
/*
|
|||
|
|
* IEncoder.cpp
|
|||
|
|
*
|
|||
|
|
* Created on: 1 <EFBFBD><EFBFBD><EFBFBD><EFBFBD>. 2022 <EFBFBD>.
|
|||
|
|
* 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 );
|
|||
|
|
|
|||
|
|
}
|