114 lines
3.7 KiB
C++
114 lines
3.7 KiB
C++
/*
|
||
* ForceEstimation.hh
|
||
*
|
||
* Created on: 4 апр. 2022 г.
|
||
* Author: titov
|
||
*/
|
||
|
||
#ifndef UMLIBRARY_PROCESSING_FORCEESTIMATION_HH_
|
||
#define UMLIBRARY_PROCESSING_FORCEESTIMATION_HH_
|
||
|
||
#include "../systemic/ISignal.hh"
|
||
#include "../systemic/IProcess.hh"
|
||
#include "acs/VectorAsyncInterface.hh"
|
||
|
||
namespace processing {
|
||
|
||
//!Модуль осуществляет обработку датчика усилия.
|
||
class ForceEstimation : public systemic::IProcess {
|
||
|
||
systemic::ISignal & speed;
|
||
systemic::ISignal & position;
|
||
systemic::ISignal & force;
|
||
|
||
float force_prev = 0.0f;
|
||
|
||
struct Estimator : vector::ITechValue {
|
||
|
||
float force_est = 0.0f;
|
||
float khardness_est = 0.0f;
|
||
float position_est = 0.0f;
|
||
float touch_est = 0.0f;
|
||
|
||
float L1P = 0.0f;
|
||
float L2P = 0.0f;
|
||
|
||
float L1N = 0.0f;
|
||
float L2N = 0.0f;
|
||
|
||
float permit_error = 0.0f;
|
||
float intolerable_displacement = 0.0f;
|
||
|
||
float dead_displacement = 0.0f;
|
||
|
||
float prev_displacement = 0.0f;
|
||
float prev_force_error = 0.0f;
|
||
|
||
void estimate( float value_position, float value_force );
|
||
float predict( float value_position );
|
||
void reset();
|
||
|
||
float transmission_ratio;
|
||
float _1_transmission_ratio;
|
||
|
||
void set( float khardness ) {
|
||
khardness_est = khardness * transmission_ratio;
|
||
intolerable_displacement = permit_error / khardness_est;
|
||
}
|
||
|
||
float get() const {
|
||
return khardness_est * _1_transmission_ratio;
|
||
}
|
||
|
||
} est;
|
||
|
||
float force_est;
|
||
float force_rate_est;
|
||
|
||
float _Ts = 0.0f;
|
||
float delay;
|
||
float min_force;
|
||
|
||
public:
|
||
struct Setting {
|
||
|
||
float min_force; //!<Минимальное усилие, с которого начинается работа наблюдателя (Н).
|
||
float khardness; //!<Коэффициент жесткости (Н/м).
|
||
float force_delay; //!<Задержка сигнала усилия (с).
|
||
|
||
float transmission_ratio; //!<Передаточный коэффициент (м/рад).
|
||
|
||
float L1P; //!<Коэффициет следящей системы оценки усилия. Коррекция оценки усилия при положительном смещении
|
||
float L2P; //!<Коэффициет следящей системы оценки усилия. Коррекция коэффициента жесткости при положительном смещении.
|
||
|
||
float L1N; //!<Коэффициет следящей системы оценки усилия. Коррекция оценки усилия при отрицательном смещении
|
||
float L2N; //!<Коэффициет следящей системы оценки усилия. Коррекция коэффициента жесткости при отрицательном смещении.
|
||
|
||
float permit_error; //!<Допустимая ошибка следящей системы.
|
||
|
||
Setting();
|
||
bool isValid();
|
||
};
|
||
|
||
void configure( Setting & config );
|
||
|
||
public:
|
||
ForceEstimation( systemic::ISignal & speed, systemic::ISignal & force, systemic::ISignal & position );
|
||
|
||
const float & getEstimatedForce() const;
|
||
const float & getEstimatedForceRate() const;
|
||
const float & getEstimatedKhardness() const;
|
||
|
||
vector::ITechValue & getKhardness();
|
||
|
||
void process();
|
||
void setSampleTime( float ts_in_second );
|
||
|
||
};
|
||
|
||
}
|
||
|
||
|
||
|
||
#endif /* UMLIBRARY_PROCESSING_FORCEESTIMATION_HH_ */
|