233 lines
5.5 KiB
C++
233 lines
5.5 KiB
C++
/*
|
||
* MoveToPoint.cpp
|
||
*
|
||
* Created on: 8 июл. 2020 г.
|
||
* Author: LeonidTitov
|
||
*/
|
||
|
||
#include "MoveToPoint.hh"
|
||
|
||
#include <cmath>
|
||
#include <cstring>
|
||
|
||
bool technological::function::MoveToPoint::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 handle(input) )
|
||
return not(run_result = ControlSystemBusy);
|
||
|
||
return not(run_result = Empty);
|
||
|
||
}
|
||
|
||
void technological::function::MoveToPoint::stop() {
|
||
|
||
if( not control_system )
|
||
return;
|
||
|
||
moving = false;
|
||
|
||
setpoint = result.position = result.setpoint = 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();
|
||
|
||
on_end();
|
||
|
||
wrapper.unlock();
|
||
|
||
}
|
||
|
||
bool technological::function::MoveToPoint::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::MoveToPoint::getState() const {
|
||
|
||
using namespace std;
|
||
|
||
if( control_system and not moving )
|
||
return ITechFunction::FINISHED;
|
||
|
||
if( control_system and moving )
|
||
return ITechFunction::EXECUTE;
|
||
|
||
if( not control_system )
|
||
return ITechFunction::DISABLE;
|
||
|
||
}
|
||
|
||
void technological::function::MoveToPoint::process() {
|
||
|
||
using namespace std;
|
||
|
||
if( moving and control_system ) {
|
||
|
||
if( fabsf( control_system->get() - setpoint ) < retention_accuracy ) {
|
||
if( not on_position_timer.isActive() )
|
||
on_position_timer.start(timeout);
|
||
} else
|
||
on_position_timer.stop();
|
||
|
||
if( on_position_timer.delayFinished() ) {
|
||
|
||
result.position = control_system->get();
|
||
result.setpoint = setpoint;
|
||
|
||
moving = false;
|
||
|
||
} else in_progress();
|
||
}
|
||
}
|
||
|
||
technological::function::MoveToPoint::MoveToPoint(
|
||
ResourceKeeper< technological::adapter::TieInterface<vector::ITechValue> > & control_system_wrapper,
|
||
ResourceKeeper<vector::ITechValue> & speed_limit,
|
||
ResourceKeeper<vector::ITechValue> & torque_limit,
|
||
ResourceKeeper<vector::ITechValue> & acc_limit ) :
|
||
wrapper(control_system_wrapper),
|
||
speed_limit(speed_limit), torque_limit(torque_limit), acc_limit(acc_limit),
|
||
setpoint(NAN), moving(false), retention_accuracy(0.0f),
|
||
stored(), run_result(Empty) {
|
||
|
||
result.position = NAN;
|
||
result.setpoint = NAN;
|
||
|
||
}
|
||
|
||
technological::function::MoveToPoint::Setting::Setting() : retention_accuracy(0.0f), on_position_timeout(0.0f) {}
|
||
|
||
bool technological::function::MoveToPoint::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;
|
||
|
||
if( on_position_timeout <= 0.0f or not isfinite(retention_accuracy) )
|
||
on_position_timeout = not_valid.on_position_timeout;
|
||
|
||
return retention_accuracy != not_valid.retention_accuracy
|
||
and on_position_timeout != not_valid.on_position_timeout;
|
||
|
||
}
|
||
|
||
void technological::function::MoveToPoint::configure( Setting & config ) {
|
||
|
||
retention_accuracy = config.retention_accuracy;
|
||
timeout = systemic::seconds2time( config.on_position_timeout );
|
||
|
||
}
|
||
|
||
void technological::function::MoveToPoint::apply( 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.position_stp);
|
||
|
||
moving = true;
|
||
|
||
on_position_timer.stop();
|
||
|
||
on_start(input, control_system->get(), timeout);
|
||
|
||
}
|
||
|
||
bool technological::function::MoveToPoint::handle( 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 and (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::MoveToPoint::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( not isfinite( input.position_stp ) )
|
||
return false;
|
||
|
||
return true;
|
||
}
|
||
|
||
bool technological::function::MoveToPoint::isSizeError() const {
|
||
|
||
return run_result == SizeError;
|
||
|
||
}
|
||
|
||
bool technological::function::MoveToPoint::isInvalidInput() const {
|
||
|
||
return run_result == InvalidInput;
|
||
|
||
}
|
||
|
||
bool technological::function::MoveToPoint::isControlSystemBusy() const {
|
||
|
||
return run_result == ControlSystemBusy;
|
||
|
||
}
|