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();
|
||
|
||
}
|