223 lines
5.3 KiB
C++
223 lines
5.3 KiB
C++
/*
|
|
* SearchCondition.cpp
|
|
*
|
|
* Created on: 5 èþë. 2020 ã.
|
|
* Author: LeonidTitov
|
|
*/
|
|
|
|
#include "SearchCondition.hh"
|
|
|
|
#include <cmath>
|
|
#include <cstring>
|
|
|
|
void technological::function::SearchCondition::apply(const Input &input)
|
|
{
|
|
acc_limit->set(input.acc_lim);
|
|
torque_limit->set(input.torque_lim);
|
|
speed_limit->set(input.speed_lim);
|
|
control_system->set((setpoint = (input.delta_stp + control_system->get())));
|
|
searching = true;
|
|
}
|
|
|
|
bool technological::function::SearchCondition::hadnle( const Input & input ) {
|
|
|
|
if( control_system ) {
|
|
|
|
apply(input);
|
|
|
|
} else {
|
|
|
|
ScopeLock<Locable<vector::ITechValue>, Locable<vector::ITechValue>,
|
|
Locable<vector::ITechValue>,
|
|
Locable<technological::adapter::TieInterface<vector::ITechValue> > > lock(
|
|
acc_limit, speed_limit, torque_limit, wrapper);
|
|
|
|
if( lock && (control_system = wrapper->try_connect()) ) {
|
|
|
|
stored.acceleration_limit = acc_limit->get();
|
|
stored.speed_limit = speed_limit->get();
|
|
stored.torque_limit = torque_limit->get();
|
|
|
|
apply(input);
|
|
|
|
lock.freeze();
|
|
}
|
|
|
|
}
|
|
|
|
return bool(control_system);
|
|
}
|
|
|
|
bool technological::function::SearchCondition::run( const char * value,
|
|
std::size_t size ) {
|
|
|
|
if( sizeof(Input) != size )
|
|
return not(run_result = SizeError);
|
|
|
|
Input input;
|
|
std::memcpy( &input, value, size );
|
|
|
|
if( not validate(input) )
|
|
return not(run_result = InvalidInput);
|
|
|
|
if( not hadnle(input) )
|
|
return not(run_result = ControlSystemBusy);
|
|
|
|
return not(run_result = Empty);
|
|
}
|
|
|
|
void technological::function::SearchCondition::stop() {
|
|
|
|
run_result = Empty;
|
|
|
|
if( not control_system )
|
|
return;
|
|
|
|
searching = false;
|
|
setpoint = result.position = result.value = NAN;
|
|
|
|
control_system = nullptr;
|
|
wrapper->disconnect();
|
|
|
|
torque_limit->set( stored.torque_limit );
|
|
torque_limit.unlock();
|
|
|
|
acc_limit->set( stored.acceleration_limit );
|
|
acc_limit.unlock();
|
|
|
|
speed_limit->set( stored.speed_limit );
|
|
speed_limit.unlock();
|
|
|
|
wrapper.unlock();
|
|
|
|
}
|
|
|
|
bool technological::function::SearchCondition::getResult( char * value,
|
|
std::size_t size ) const {
|
|
|
|
if( size != sizeof(Output) )
|
|
return false;
|
|
|
|
Output output;
|
|
|
|
output = result;
|
|
|
|
std::memcpy( value, &output, sizeof(Output) );
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
short technological::function::SearchCondition::getState() const {
|
|
|
|
using namespace std;
|
|
|
|
if(control_system) {
|
|
|
|
if( searching )
|
|
return ITechFunction::EXECUTE;
|
|
|
|
else if( isfinite(result.value) )
|
|
return ITechFunction::FINISHED;
|
|
|
|
else
|
|
return ITechFunction::FAILURE;
|
|
|
|
} else
|
|
return ITechFunction::DISABLE;
|
|
|
|
}
|
|
|
|
void technological::function::SearchCondition::process() {
|
|
|
|
using namespace std;
|
|
|
|
if( searching and control_system ) {
|
|
|
|
if( condition ) {
|
|
result.value = value;
|
|
result.position = control_system->get();
|
|
|
|
searching = false;
|
|
|
|
control_system->set( control_system->get() );
|
|
|
|
} else if( fabsf( control_system->get() - setpoint ) < retention_accuracy ) {
|
|
|
|
result.value = NAN;
|
|
result.position = NAN;
|
|
|
|
searching = false;
|
|
|
|
}
|
|
}
|
|
}
|
|
|
|
technological::function::SearchCondition::SearchCondition(
|
|
ResourceKeeper< technological::adapter::TieInterface<vector::ITechValue> > & control_system_wrapper,
|
|
ResourceKeeper<vector::ITechValue> & speed_limit,
|
|
ResourceKeeper<vector::ITechValue> & torque_limit,
|
|
ResourceKeeper<vector::ITechValue> & acc_limit,
|
|
systemic::IStatus &condition, systemic::ISignal & value) : wrapper(control_system_wrapper),
|
|
speed_limit(speed_limit), torque_limit(torque_limit), acc_limit(acc_limit),
|
|
condition(condition), value(value),
|
|
setpoint(NAN), searching(false), retention_accuracy(0.0f),
|
|
stored(), run_result(Empty) {
|
|
|
|
result.value = NAN;
|
|
result.position = NAN;
|
|
|
|
}
|
|
|
|
void technological::function::SearchCondition::configure( Setting & config ) {
|
|
|
|
retention_accuracy = config.retention_accuracy;
|
|
|
|
}
|
|
|
|
bool technological::function::SearchCondition::validate( Input & input ) {
|
|
|
|
using namespace std;
|
|
|
|
if( input.acc_lim <= 0.0f or input.speed_lim <= 0.0f or input.torque_lim <= 0.0f )
|
|
return false;
|
|
|
|
if( input.delta_stp == 0.0f or not isfinite( input.delta_stp ) )
|
|
return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
technological::function::SearchCondition::Setting::Setting() : retention_accuracy(0.0f) {}
|
|
|
|
bool technological::function::SearchCondition::Setting::isValid() {
|
|
|
|
using namespace std;
|
|
|
|
const Setting not_valid;
|
|
|
|
if( retention_accuracy <= 0.0f or not isfinite(retention_accuracy) )
|
|
retention_accuracy = not_valid.retention_accuracy;
|
|
|
|
return retention_accuracy != not_valid.retention_accuracy;
|
|
|
|
}
|
|
|
|
bool technological::function::SearchCondition::isSizeError() const {
|
|
|
|
return run_result == SizeError;
|
|
|
|
}
|
|
|
|
bool technological::function::SearchCondition::isInvalidInput() const {
|
|
|
|
return run_result == InvalidInput;
|
|
|
|
}
|
|
|
|
bool technological::function::SearchCondition::isControlSystemBusy() const {
|
|
|
|
return run_result == ControlSystemBusy;
|
|
|
|
}
|