99 lines
2.9 KiB
C++
99 lines
2.9 KiB
C++
/*
|
||
* MoveToPointWithTimeout.cpp
|
||
*
|
||
* Created on: 8 июл. 2020 г.
|
||
* 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() {}
|