123 lines
3.9 KiB
C++
123 lines
3.9 KiB
C++
|
/*
|
|||
|
|
* TorqueLimitation.cpp
|
|||
|
|
*
|
|||
|
|
* Created on: 29 <EFBFBD><EFBFBD><EFBFBD>. 2020 <EFBFBD>.
|
|||
|
|
* 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;
|
|||
|
|
|
|||
|
|
}
|