104 lines
2.4 KiB
C++
104 lines
2.4 KiB
C++
/*
|
|
* EncoderQuadratureCounter.cpp
|
|
*
|
|
* Created on: 22 ÿíâ. 2020 ã.
|
|
* Author: user
|
|
*/
|
|
|
|
#include "EncoderQuadratureCounter.hh"
|
|
|
|
float driver::derail::EncoderQuadratureCounter::getAngle() const {
|
|
|
|
return failure ? NAN : angle;
|
|
|
|
}
|
|
|
|
float driver::derail::EncoderQuadratureCounter::getTurn() const {
|
|
|
|
return failure ? NAN : turn_counter;
|
|
|
|
}
|
|
|
|
bool driver::derail::EncoderQuadratureCounter::configure( Setting & set ) {
|
|
|
|
if( set.line_in_turn == 0 or set.max_turn_value == 0 ) return false;
|
|
|
|
max_turn_value = set.max_turn_value;
|
|
line_in_turn = set.line_in_turn;
|
|
|
|
_1_line_to_rad = math::constants::pi2 / line_in_turn;
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
bool driver::derail::EncoderQuadratureCounter::isFailure() const {
|
|
|
|
return failure;
|
|
|
|
}
|
|
|
|
void driver::derail::EncoderQuadratureCounter::process() {
|
|
|
|
if( not (failure = counter.isError()) ) {
|
|
|
|
long temp_position = angle_position + counter.getDeltaCounter();
|
|
|
|
//todo ïëîõî åñëè temp_position < -line_in_turn...
|
|
if( temp_position < 0 ) {
|
|
temp_position += line_in_turn;
|
|
|
|
if( turn_counter > 0 ) turn_counter--;
|
|
else {
|
|
turn_counter = max_turn_value - 1;
|
|
}
|
|
} else if( temp_position >= line_in_turn ) {
|
|
temp_position -= line_in_turn;
|
|
|
|
if( turn_counter < max_turn_value - 1 ) turn_counter++;
|
|
else {
|
|
turn_counter = 0;
|
|
}
|
|
}
|
|
|
|
angle_position = temp_position;
|
|
|
|
angle = _1_line_to_rad * angle_position;
|
|
|
|
data.write( std::pair<float, float>( turn_counter, angle ) );
|
|
}
|
|
|
|
}
|
|
|
|
driver::derail::EncoderQuadratureCounter::EncoderQuadratureCounter(
|
|
peripheral::ICounter & counter) :
|
|
counter(counter) {}
|
|
|
|
void driver::derail::EncoderQuadratureCounter::resetFailure() {
|
|
|
|
counter.resetError();
|
|
|
|
failure = counter.isError();
|
|
|
|
}
|
|
|
|
void driver::derail::EncoderQuadratureCounter::setAbsolurePosition(
|
|
float angle, unsigned long turn ) {
|
|
|
|
while( angle < 0.f ) angle += math::constants::pi2;
|
|
while( angle >= math::constants::pi2 ) angle -= math::constants::pi2;
|
|
|
|
angle_position = angle / _1_line_to_rad;
|
|
|
|
while( turn >= max_turn_value ) turn -= max_turn_value;
|
|
|
|
turn_counter = turn;
|
|
|
|
}
|
|
|
|
std::pair<float, float> driver::derail::EncoderQuadratureCounter::getPosition() const {
|
|
|
|
return failure ? std::pair<float, float>( 0, NAN ) : data.read();
|
|
|
|
}
|