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