99 lines
2.9 KiB
C++
99 lines
2.9 KiB
C++
|
/*
|
|||
|
|
* MoveToPointWithTimeout.cpp
|
|||
|
|
*
|
|||
|
|
* Created on: 8 <EFBFBD><EFBFBD><EFBFBD>. 2020 <EFBFBD>.
|
|||
|
|
* Author: LeonidTitov
|
|||
|
|
*/
|
|||
|
|
|
|||
|
|
#include "MoveToPointWithTimeout.hh"
|
|||
|
|
|
|||
|
|
#include "../../math/math_inc.hh"
|
|||
|
|
|
|||
|
|
void technological::function::MoveToPointWithTimeout::configure( Setting & config ) {
|
|||
|
|
|
|||
|
|
MoveToPoint::Setting basic_setting;
|
|||
|
|
|
|||
|
|
basic_setting.on_position_timeout = config.on_position_timeout;
|
|||
|
|
basic_setting.retention_accuracy = config.retention_accuracy;
|
|||
|
|
|
|||
|
|
MoveToPoint::configure( basic_setting );
|
|||
|
|
|
|||
|
|
permit_speed_factor = config.permit_speed_factor;
|
|||
|
|
permit_time = config.permit_time;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void technological::function::MoveToPointWithTimeout::in_progress() {
|
|||
|
|
|
|||
|
|
if( compite_timer.delayFinished() ) {
|
|||
|
|
|
|||
|
|
move_failure = true;
|
|||
|
|
|
|||
|
|
MoveToPoint::stop();
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
short technological::function::MoveToPointWithTimeout::getState() const {
|
|||
|
|
|
|||
|
|
return move_failure ? FAILURE : MoveToPoint::getState();
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void technological::function::MoveToPointWithTimeout::stop() {
|
|||
|
|
|
|||
|
|
compite_timer.stop();
|
|||
|
|
|
|||
|
|
MoveToPoint::stop();
|
|||
|
|
|
|||
|
|
move_failure = false;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void technological::function::MoveToPointWithTimeout::on_start( Input & input, float start_point, systemic::time_t on_position_timeout ) {
|
|||
|
|
|
|||
|
|
float move_time = math::function::get_time(
|
|||
|
|
fabsf( input.position_stp - start_point ),
|
|||
|
|
input.speed_lim * permit_speed_factor,
|
|||
|
|
input.acc_lim
|
|||
|
|
);
|
|||
|
|
|
|||
|
|
systemic::time_t complite_timeout = systemic::seconds2time( move_time + permit_time ) + on_position_timeout;
|
|||
|
|
compite_timer.start( complite_timeout );
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
technological::function::MoveToPointWithTimeout::Setting::Setting() : permit_speed_factor(0.0f), permit_time(0.0f) {}
|
|||
|
|
|
|||
|
|
bool technological::function::MoveToPointWithTimeout::Setting::isValid() {
|
|||
|
|
|
|||
|
|
const Setting non_valid;
|
|||
|
|
|
|||
|
|
if( permit_speed_factor <= 0.0f || permit_speed_factor > 1.0f )
|
|||
|
|
permit_speed_factor = non_valid.permit_speed_factor;
|
|||
|
|
|
|||
|
|
if( permit_time <= 0.0f )
|
|||
|
|
permit_time = non_valid.permit_time;
|
|||
|
|
|
|||
|
|
MoveToPoint::Setting basic_setting;
|
|||
|
|
|
|||
|
|
basic_setting.on_position_timeout = on_position_timeout;
|
|||
|
|
basic_setting.retention_accuracy = retention_accuracy;
|
|||
|
|
|
|||
|
|
bool result = basic_setting.isValid();
|
|||
|
|
|
|||
|
|
on_position_timeout = basic_setting.on_position_timeout;
|
|||
|
|
retention_accuracy = basic_setting.retention_accuracy;
|
|||
|
|
|
|||
|
|
return result and permit_time != non_valid.permit_time and permit_speed_factor != non_valid.permit_speed_factor;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
technological::function::MoveToPointWithTimeout::MoveToPointWithTimeout(
|
|||
|
|
ResourceKeeper< technological::adapter::TieInterface<vector::ITechValue> > & control_system_wrapper,
|
|||
|
|
ResourceKeeper<vector::ITechValue> & speed_limit,
|
|||
|
|
ResourceKeeper<vector::ITechValue> & torque_limit,
|
|||
|
|
ResourceKeeper<vector::ITechValue> & acc_limit) : MoveToPoint(control_system_wrapper, speed_limit, torque_limit, acc_limit ),
|
|||
|
|
move_failure(false), permit_speed_factor(1.0f), permit_time(0.0f), compite_timer() {}
|