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;
|
||
|
||
}
|