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_ */
|