114 lines
3.1 KiB
C++
114 lines
3.1 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_ */
|