77 lines
1.5 KiB
C++
77 lines
1.5 KiB
C++
|
/*
|
|||
|
|
* TorqueEstimation.cpp
|
|||
|
|
*
|
|||
|
|
* Created on: 29 <EFBFBD><EFBFBD><EFBFBD>. 2020 <EFBFBD>.
|
|||
|
|
* Author: titov
|
|||
|
|
*/
|
|||
|
|
|
|||
|
|
#include "LoadEstimation.hh"
|
|||
|
|
|
|||
|
|
bool processing::LoadEstimation::Setting::isValid() {
|
|||
|
|
|
|||
|
|
const Setting non_valid;
|
|||
|
|
|
|||
|
|
if( Jp <= 0.0f )
|
|||
|
|
Jp = non_valid.Jp;
|
|||
|
|
|
|||
|
|
if( L1 <= 0.0f )
|
|||
|
|
L1 = non_valid.L1;
|
|||
|
|
|
|||
|
|
if( L2 <= 0.0f )
|
|||
|
|
L2 = non_valid.L2;
|
|||
|
|
|
|||
|
|
return Jp != non_valid.Jp
|
|||
|
|
and L1 != non_valid.L1
|
|||
|
|
and L2 != non_valid.L2;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
processing::LoadEstimation::Setting::Setting() : Jp(0.0f), L1(0.0f), L2(0.0f) {}
|
|||
|
|
|
|||
|
|
void processing::LoadEstimation::configure( const Setting & setting ) {
|
|||
|
|
|
|||
|
|
_L1Base = setting.L1;
|
|||
|
|
_L2Base = setting.L2;
|
|||
|
|
_Base_J = 1.0f / setting.Jp;
|
|||
|
|
|
|||
|
|
_L1Ts = _L1Base * _Ts;
|
|||
|
|
_L2Ts = _L2Base * _Ts;
|
|||
|
|
_Ts_J = _Base_J * _Ts;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void processing::LoadEstimation::process() {
|
|||
|
|
|
|||
|
|
const float error = ( speed - est_speed );
|
|||
|
|
|
|||
|
|
est_speed += error * _L1Ts + ( torque - est_load ) * _Ts_J;
|
|||
|
|
est_load -= error * _L2Ts;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void processing::LoadEstimation::setSampleTime( float ts_in_second ) {
|
|||
|
|
|
|||
|
|
_Ts = ts_in_second;
|
|||
|
|
|
|||
|
|
_L1Ts = _L1Base * _Ts;
|
|||
|
|
_L2Ts = _L2Base * _Ts;
|
|||
|
|
_Ts_J = _Base_J * _Ts;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
const float & processing::LoadEstimation::getEstimatedLoad() const {
|
|||
|
|
|
|||
|
|
return est_load;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
const float & processing::LoadEstimation::getEstimatedSpeed() const {
|
|||
|
|
|
|||
|
|
return est_speed;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
processing::LoadEstimation::LoadEstimation( systemic::ISignal & speed,
|
|||
|
|
systemic::ISignal & torque ) : speed(speed), torque(torque) {}
|
|||
|
|
|