123 lines
3.9 KiB
C++
123 lines
3.9 KiB
C++
/*
|
||
* TorqueLimitation.cpp
|
||
*
|
||
* Created on: 29 дек. 2020 г.
|
||
* Author: titov
|
||
*/
|
||
|
||
#include "TorqueLimitation.hh"
|
||
#include "../math/math_inc.hh"
|
||
|
||
void processing::TorqueLimitation::configure( const Setting & setting ) {
|
||
|
||
_K = setting.overload_coefficient;
|
||
_Kp1 = _K + 1;
|
||
|
||
_OffsetUp_k = setting.positive_force_offset_k;
|
||
_OffsetUp_b = setting.positive_force_offset_b;
|
||
_OffsetDown_k = setting.negative_force_offset_k;
|
||
_OffsetDown_b = setting.negative_force_offset_b;
|
||
|
||
_max_torque = setting.max_torque;
|
||
|
||
set(_max_torque);
|
||
|
||
}
|
||
|
||
void processing::TorqueLimitation::process() {
|
||
|
||
using namespace std;
|
||
|
||
float friction_smp = friction;
|
||
float load_smp = load;
|
||
float dynamic_torque_smp = dynamic_torque;
|
||
|
||
float up_max_torque = math::function::saturation(
|
||
lim_act_up - _K * ( load_smp - friction_smp ), torque_lim_act_up + fmaxf(dynamic_torque_smp + friction_smp, 0), torque_lim_act_up );
|
||
float down_max_torque = math::function::saturation(
|
||
-lim_act_down - _K * ( load_smp - friction_smp ), -torque_lim_act_down, -torque_lim_act_down + fminf(dynamic_torque_smp + friction_smp, 0) );
|
||
|
||
torque_up_limit.set( actual_up_limit = up_max_torque );
|
||
torque_down_limit.set( actual_down_limit = - down_max_torque );
|
||
}
|
||
|
||
bool processing::TorqueLimitation::Setting::isValid() {
|
||
|
||
using namespace std;
|
||
|
||
const Setting not_valid;
|
||
|
||
if( overload_coefficient < 0.0f )
|
||
overload_coefficient = not_valid.overload_coefficient;
|
||
|
||
if( max_torque < 0.0f )
|
||
max_torque = not_valid.max_torque;
|
||
|
||
if( not isfinite( positive_force_offset_k ) )
|
||
positive_force_offset_k = not_valid.positive_force_offset_k;
|
||
|
||
if( not isfinite( negative_force_offset_k ) )
|
||
negative_force_offset_k = not_valid.negative_force_offset_k;
|
||
|
||
if( not isfinite( positive_force_offset_b ) )
|
||
positive_force_offset_b = not_valid.positive_force_offset_b;
|
||
|
||
if( not isfinite( negative_force_offset_b ) )
|
||
negative_force_offset_b = not_valid.negative_force_offset_b;
|
||
|
||
return overload_coefficient != not_valid.overload_coefficient
|
||
and max_torque != not_valid.max_torque
|
||
and positive_force_offset_k != not_valid.positive_force_offset_k
|
||
and negative_force_offset_k != not_valid.negative_force_offset_k
|
||
and positive_force_offset_b != not_valid.positive_force_offset_b
|
||
and negative_force_offset_b != not_valid.negative_force_offset_b;
|
||
|
||
}
|
||
|
||
processing::TorqueLimitation::Setting::Setting() : overload_coefficient(NAN), max_torque(0.0f),
|
||
positive_force_offset_k(0.0f), positive_force_offset_b(NAN),
|
||
negative_force_offset_k(0.0f), negative_force_offset_b(NAN) {}
|
||
|
||
void processing::TorqueLimitation::setSampleTime( float ts_in_second ) {}
|
||
|
||
void processing::TorqueLimitation::set( control::Value limit ) {
|
||
|
||
using namespace std;
|
||
|
||
float torque_limited = fminf( fabsf( limit ), _max_torque );
|
||
|
||
torque_lim_act_up = torque_limited * _OffsetUp_k + _OffsetUp_b;
|
||
torque_lim_act_down = torque_limited * _OffsetDown_k + _OffsetDown_b;
|
||
|
||
lim_act_up = torque_lim_act_up * _Kp1;
|
||
lim_act_down = torque_lim_act_down * _Kp1;
|
||
|
||
demand_limit = torque_limited;
|
||
}
|
||
|
||
control::Value processing::TorqueLimitation::get() const {
|
||
|
||
return demand_limit;
|
||
|
||
}
|
||
|
||
processing::TorqueLimitation::TorqueLimitation(
|
||
vector::ITechValue & up_limit, vector::ITechValue & down_limit,
|
||
systemic::ISignal & friction, systemic::ISignal & dynamic_torque, systemic::ISignal & load ) :
|
||
torque_up_limit(up_limit), torque_down_limit(down_limit),
|
||
friction(friction), dynamic_torque(dynamic_torque), load(load),
|
||
lim_act_up(0.0f), lim_act_down(0.0f), torque_lim_act_up(0.0f),
|
||
torque_lim_act_down(0.0f) {}
|
||
|
||
const float & processing::TorqueLimitation::getActualUpLimit() const {
|
||
|
||
return actual_up_limit;
|
||
|
||
}
|
||
|
||
const float & processing::TorqueLimitation::getActualDownLimit() const {
|
||
|
||
return actual_down_limit;
|
||
|
||
}
|