156 lines
4.3 KiB
C++
156 lines
4.3 KiB
C++
/*
|
||
* HipEncoder.cpp
|
||
*
|
||
* Created on: 26 февр. 2020 г.
|
||
* Author: user
|
||
*/
|
||
|
||
#include "HipEncoder.hh"
|
||
|
||
struct IsPositionEqual {
|
||
const float max_delta;
|
||
|
||
const driver::PositionComponents & first;
|
||
const driver::PositionComponents & second;
|
||
|
||
IsPositionEqual(const driver::PositionComponents & first, const driver::PositionComponents & second, float max_delta) :
|
||
first(first), second(second), max_delta(max_delta) {}
|
||
|
||
//todo придумать сравнение получше
|
||
operator bool() {
|
||
|
||
return first.sin - second.sin > - max_delta and first.sin - second.sin < max_delta and
|
||
first.cos - second.cos > - max_delta and first.cos - second.cos < max_delta and
|
||
first.increment_position == second.increment_position;
|
||
|
||
}
|
||
|
||
};
|
||
|
||
static const float delta_equal = 20.f;
|
||
|
||
void driver::HipEncoder::process() {
|
||
|
||
if( failure = not mixer.isValid() ) {
|
||
|
||
ready = false;
|
||
mixer.reset();
|
||
|
||
work_state = request_hip_position; //переход в режим калибровки
|
||
|
||
}
|
||
|
||
switch( work_state ) {
|
||
case request_hip_position: {
|
||
|
||
if( hip_position.sendRequest() ) work_state = wait_position_ready;
|
||
|
||
} break;
|
||
case wait_position_ready: {
|
||
|
||
if( hip_position.isAnswerReady() ) {
|
||
|
||
if( hip_position.isOk() ) {
|
||
|
||
(void) mixer.calculatePosition();
|
||
const PositionComponents current_position = mixer.getPositionComponents();
|
||
|
||
prev_position = current_position;
|
||
prev_absolute_position = hip_position.getPositionCode();
|
||
|
||
work_state = request_next_hip_position;
|
||
|
||
} else work_state = request_hip_position;
|
||
|
||
}
|
||
|
||
} break;
|
||
case request_next_hip_position: {
|
||
|
||
if( hip_position.sendRequest() ) work_state = wait_next_hip_position;
|
||
|
||
} break;
|
||
case wait_next_hip_position: {
|
||
|
||
if( hip_position.isAnswerReady() ) {
|
||
|
||
if( hip_position.isOk() ) {
|
||
|
||
const unsigned long absolute_position = hip_position.getPositionCode();
|
||
|
||
(void) mixer.calculatePosition();
|
||
const PositionComponents current_position = mixer.getPositionComponents();
|
||
|
||
if( IsPositionEqual( current_position, prev_position, delta_equal ) and
|
||
prev_absolute_position == absolute_position ) {
|
||
|
||
work_state = normal_work;
|
||
|
||
mixer.setAbsolutePosition( absolute_position );
|
||
position.write( mixer.calculatePosition() );
|
||
|
||
ready = true;
|
||
|
||
} else {
|
||
|
||
prev_position = current_position;
|
||
prev_absolute_position = absolute_position;
|
||
|
||
work_state = request_next_hip_position;
|
||
|
||
}
|
||
|
||
} else work_state = request_next_hip_position;
|
||
|
||
}
|
||
|
||
} break;
|
||
case normal_work: {
|
||
|
||
position.write( mixer.calculatePosition() );
|
||
|
||
scrt_valid = mixer.isScrtValid();
|
||
phase_error = mixer.isPhaseError();
|
||
|
||
{
|
||
const PositionComponents current_position = mixer.getPositionComponents();
|
||
|
||
sin_value = current_position.sin;
|
||
cos_value = current_position.cos;
|
||
inc_counter = current_position.increment_position;
|
||
}
|
||
|
||
|
||
} break;
|
||
}
|
||
|
||
|
||
|
||
}
|
||
|
||
driver::HipEncoder::HipEncoder(IResolver& resolver,
|
||
peripheral::ICounter& counter,
|
||
HiperfaceNetworkDriver& hip_driver,
|
||
unsigned short address,
|
||
unsigned long sin_period_resolution,
|
||
unsigned long max_turn_value,
|
||
unsigned short resolver_hyst, unsigned short scrt_hyst,
|
||
float sector_sync_deviation ) :
|
||
mixer(counter, sin_period_resolution, max_turn_value, resolver, resolver_hyst, scrt_hyst, sector_sync_deviation ),
|
||
hip_position( hip_driver, address ) {
|
||
PositionData data;
|
||
|
||
data.angle = NAN;
|
||
data.turn = 0;
|
||
|
||
position.write( data );
|
||
|
||
}
|
||
|
||
std::pair<float, float> driver::HipEncoder::getPosition() const {
|
||
|
||
PositionData data = position.read();
|
||
return ready ? std::pair<float, float>( data.turn, data.angle ) : std::pair<float, float>( 0, NAN );
|
||
|
||
}
|