53 lines
1.3 KiB
C++
53 lines
1.3 KiB
C++
|
/*
|
|||
|
|
* ForceControllerAutotuningFeatureEstimate.cpp
|
|||
|
|
*
|
|||
|
|
* Created on: 4 <EFBFBD><EFBFBD><EFBFBD> 2022 <EFBFBD>.
|
|||
|
|
* Author: titov
|
|||
|
|
*/
|
|||
|
|
|
|||
|
|
#include "ForceControllerAutotuningFeatureEstimate.hh"
|
|||
|
|
|
|||
|
|
processing::ForceControllerAutotuningFeatureEstimate::ForceControllerAutotuningFeatureEstimate(
|
|||
|
|
systemic::ISignal & force, systemic::ISignal & dforce,
|
|||
|
|
const control::regulator::ForceController & fc ) : force(force), dforce(dforce), fc(fc),
|
|||
|
|
dynamic_error(0.0f), aggregated_dynamic_error(0.0f) {}
|
|||
|
|
|
|||
|
|
const float & processing::ForceControllerAutotuningFeatureEstimate::getDynamicError() const {
|
|||
|
|
|
|||
|
|
return dynamic_error;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
const float & processing::ForceControllerAutotuningFeatureEstimate::getEndMismatch() const {
|
|||
|
|
|
|||
|
|
return fc.getEndMismatch();
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void processing::ForceControllerAutotuningFeatureEstimate::reset() {
|
|||
|
|
|
|||
|
|
dynamic_error = 0.0f;
|
|||
|
|
aggregated_dynamic_error = 0.0f;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void processing::ForceControllerAutotuningFeatureEstimate::process() {
|
|||
|
|
|
|||
|
|
dynamic_error = fc.getDynamicError(dforce);
|
|||
|
|
aggregated_dynamic_error += dynamic_error * _Ts;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
const float & processing::ForceControllerAutotuningFeatureEstimate::getAggregatedDynamicError() const {
|
|||
|
|
|
|||
|
|
return aggregated_dynamic_error;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void processing::ForceControllerAutotuningFeatureEstimate::setSampleTime(
|
|||
|
|
float ts_in_second ) {
|
|||
|
|
|
|||
|
|
_Ts = ts_in_second;
|
|||
|
|
|
|||
|
|
}
|