102 lines
2.7 KiB
C++
102 lines
2.7 KiB
C++
|
/*
|
|||
|
|
* HipEncoder.h
|
|||
|
|
*
|
|||
|
|
* Created on: 26 <EFBFBD><EFBFBD><EFBFBD><EFBFBD>. 2020 <EFBFBD>.
|
|||
|
|
* Author: user
|
|||
|
|
*/
|
|||
|
|
|
|||
|
|
#ifndef UMLIBRARY_DRIVER_HIPENCODER_HH_
|
|||
|
|
#define UMLIBRARY_DRIVER_HIPENCODER_HH_
|
|||
|
|
|
|||
|
|
#include "IEncoder.hh"
|
|||
|
|
#include "IResolver.hh"
|
|||
|
|
|
|||
|
|
#include "IncrementCounter.hh"
|
|||
|
|
#include "PositionMixer.hh"
|
|||
|
|
#include "PositionMixerv2.hh"
|
|||
|
|
|
|||
|
|
#include "../peripheral/ICounter.hh"
|
|||
|
|
|
|||
|
|
#include "../communication/hiperface/HipPosition.hh"
|
|||
|
|
|
|||
|
|
#include "../math/math_inc.hh"
|
|||
|
|
#include "../common/DoubleBuffer.hpp"
|
|||
|
|
|
|||
|
|
namespace driver {
|
|||
|
|
|
|||
|
|
class HipEncoder: public IEncoder {
|
|||
|
|
private:
|
|||
|
|
communication::hiperface::HipPositionClient hip_position;
|
|||
|
|
PositionMixer_v2 mixer;
|
|||
|
|
|
|||
|
|
PositionComponents prev_position; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
unsigned long prev_absolute_position = 0; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
|
|||
|
|
DoubleBuffer<PositionData> position;
|
|||
|
|
|
|||
|
|
bool ready = false;
|
|||
|
|
bool failure = false;
|
|||
|
|
|
|||
|
|
enum SynchronizationState {
|
|||
|
|
request_hip_position,
|
|||
|
|
wait_position_ready,
|
|||
|
|
request_next_hip_position,
|
|||
|
|
wait_next_hip_position,
|
|||
|
|
normal_work
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
SynchronizationState work_state = request_hip_position;
|
|||
|
|
|
|||
|
|
//todo <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
float phase_error;
|
|||
|
|
|
|||
|
|
//todo <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
float atan;
|
|||
|
|
|
|||
|
|
//todo <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
float pos_code;
|
|||
|
|
|
|||
|
|
float sin_value = 0.0f;
|
|||
|
|
float cos_value = 0.0f;
|
|||
|
|
float inc_counter = 0.0f;
|
|||
|
|
|
|||
|
|
bool scrt_valid = false;
|
|||
|
|
bool sync_error = false;
|
|||
|
|
|
|||
|
|
public:
|
|||
|
|
|
|||
|
|
float getAngle() const override { return ready ? position.read().angle : NAN; }
|
|||
|
|
float getTurn() const override { return ready ? position.read().turn : NAN; }
|
|||
|
|
|
|||
|
|
std::pair<float, float> getPosition() const;
|
|||
|
|
|
|||
|
|
float getAbsoluteAngle() const {
|
|||
|
|
PositionData data = position.read();
|
|||
|
|
return ready ? data.angle + data.turn*math::constants::pi2 : NAN;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
const bool & isReady() const { return ready; }
|
|||
|
|
|
|||
|
|
const bool & isFailure() const { return failure; }
|
|||
|
|
|
|||
|
|
const bool & isScrtValid() const { return scrt_valid; }
|
|||
|
|
const bool & isSyncError() const { return sync_error; }
|
|||
|
|
|
|||
|
|
void process();
|
|||
|
|
|
|||
|
|
const float & getSin() const { return sin_value; }
|
|||
|
|
|
|||
|
|
const float & getCos() const { return cos_value; }
|
|||
|
|
|
|||
|
|
const float & getCounter() const { return inc_counter; }
|
|||
|
|
|
|||
|
|
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 );
|
|||
|
|
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
} /* namespace driver */
|
|||
|
|
|
|||
|
|
#endif /* UMLIBRARY_DRIVER_HIPENCODER_HH_ */
|