104 lines
2.5 KiB
C++
104 lines
2.5 KiB
C++
|
#include "EncoderMultiturnWraper.hh"
|
||
|
|
|
||
|
|
bool driver::detail::EncoderMultiturnWraper::configure(Setting &config)
|
||
|
|
{
|
||
|
|
|
||
|
|
bool result;
|
||
|
|
if( result = config.isValid() ) {
|
||
|
|
turn_counter.setMaxCount(config.num_turn);
|
||
|
|
turn_counter = 0;
|
||
|
|
max_angle_delta = config.max_angle_delta;
|
||
|
|
};
|
||
|
|
|
||
|
|
return result;
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
driver::detail::EncoderMultiturnWraper::TurnCounter::operator float() { return num_count; }
|
||
|
|
|
||
|
|
driver::detail::EncoderMultiturnWraper::TurnCounter & driver::detail::EncoderMultiturnWraper::TurnCounter::operator++() {
|
||
|
|
if(not(++num_count < max_count)) {
|
||
|
|
num_count = 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
return *this;
|
||
|
|
}
|
||
|
|
|
||
|
|
driver::detail::EncoderMultiturnWraper::TurnCounter & driver::detail::EncoderMultiturnWraper::TurnCounter::operator--() {
|
||
|
|
if(not(--num_count < max_count)) {
|
||
|
|
num_count = max_count - 1;
|
||
|
|
}
|
||
|
|
|
||
|
|
return *this;
|
||
|
|
}
|
||
|
|
|
||
|
|
driver::detail::EncoderMultiturnWraper::TurnCounter & driver::detail::EncoderMultiturnWraper::TurnCounter::operator=(unsigned short new_num_count) {
|
||
|
|
num_count = new_num_count;
|
||
|
|
|
||
|
|
while(not(num_count < max_count)) num_count -= max_count;
|
||
|
|
|
||
|
|
return *this;
|
||
|
|
}
|
||
|
|
|
||
|
|
void driver::detail::EncoderMultiturnWraper::TurnCounter::setMaxCount(unsigned short new_max_count) {
|
||
|
|
max_count = new_max_count;
|
||
|
|
|
||
|
|
while(not(num_count < max_count)) num_count -= max_count;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
void driver::detail::EncoderMultiturnWraper::process() {
|
||
|
|
|
||
|
|
using namespace std;
|
||
|
|
|
||
|
|
float angle = single_turn_encoder.getAngle();
|
||
|
|
|
||
|
|
if( isfinite(angle) ) {
|
||
|
|
|
||
|
|
if( isfinite(prev_angle) ) {
|
||
|
|
|
||
|
|
float angle_delta = angle - prev_angle;
|
||
|
|
|
||
|
|
if( angle_delta > math::constants::pi ) {
|
||
|
|
|
||
|
|
--turn_counter;
|
||
|
|
angle_delta -= math::constants::pi2;
|
||
|
|
|
||
|
|
} else if( angle_delta < - math::constants::pi ) {
|
||
|
|
|
||
|
|
++turn_counter;
|
||
|
|
angle_delta += math::constants::pi2;
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
if( fabsf(angle_delta) > max_angle_delta )
|
||
|
|
++error_counter;
|
||
|
|
|
||
|
|
else
|
||
|
|
error_counter = 0;
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
prev_angle = angle;
|
||
|
|
|
||
|
|
} else
|
||
|
|
++error_counter;
|
||
|
|
|
||
|
|
data.write( std::pair<Turn, Angle>( turn_counter, angle ) );
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
void driver::detail::EncoderMultiturnWraper::EncoderMultiturnWraper::reset() {
|
||
|
|
|
||
|
|
error_counter = 0;
|
||
|
|
prev_angle = NAN;
|
||
|
|
turn_counter = 0;
|
||
|
|
data.write( std::pair<Turn, Angle>( 0, NAN ) );
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
driver::detail::EncoderMultiturnWraper::EncoderMultiturnWraper(
|
||
|
|
IEncoder & encoder, unsigned short max_error ) : single_turn_encoder(encoder), error_counter(max_error) {
|
||
|
|
|
||
|
|
}
|