24 #ifndef _PLUGINS_COLLI_DRIVE_REALIZATION_BASE_MOTORINSTRUCT_H_
25 #define _PLUGINS_COLLI_DRIVE_REALIZATION_BASE_MOTORINSTRUCT_H_
27 #include "../common/types.h"
29 #include <config/config.h>
30 #include <interfaces/MotorInterface.h>
31 #include <logging/logger.h>
32 #include <utils/time/time.h>
43 class BaseMotorInstruct
46 BaseMotorInstruct(MotorInterface *motor,
float frequency, Logger *logger, Configuration *config);
50 void drive(
float trans_x,
float trans_y,
float rot);
71 virtual float calculate_rotation(
float current,
float desired,
float time_factor) = 0;
78 virtual float calculate_translation(
float current,
float desired,
float time_factor) = 0;
104 : logger_(logger), config_(config), motor_(motor)
106 logger_->log_debug(
"BaseMotorInstruct",
"(Constructor): Entering");
109 current_.x = current_.y = current_.rot = 0.f;
110 desired_.x = desired_.y = desired_.rot = 0.f;
111 exec_.x = exec_.y = exec_.rot = 0.f;
113 std::string cfg_prefix =
"/plugins/colli/motor_instruct/";
114 trans_acc_ = config_->get_float((cfg_prefix +
"trans_acc").c_str());
115 trans_dec_ = config_->get_float((cfg_prefix +
"trans_dec").c_str());
116 rot_acc_ = config_->get_float((cfg_prefix +
"rot_acc").c_str());
117 rot_dec_ = config_->get_float((cfg_prefix +
"rot_dec").c_str());
119 logger_->log_debug(
"BaseMotorInstruct",
"(Constructor): Exiting");
131 BaseMotorInstruct::set_command()
135 "Cannot set command, no writer for MotorInterface '%s'",
143 float exec_trans = std::fabs(std::sqrt(exec_.
x * exec_.
x + exec_.
y * exec_.
y));
144 if (exec_trans >= 0.05) {
147 float reduction = 3. / exec_trans;
150 float vx_max = fabsf(exec_.
x * reduction);
151 float vy_max = fabsf(exec_.
y * reduction);
154 cmd.
x = std::fminf(std::fmaxf(exec_.
x, -vx_max), vx_max);
155 cmd.
y = std::fminf(std::fmaxf(exec_.
y, -vy_max), vy_max);
159 if (fabs(exec_.
rot) >= 0.01) {
161 cmd.
rot = std::fmin(std::fmax(exec_.
rot, -2 * M_PI), 2 * M_PI);
177 exec_.
x = exec_.
y = exec_.
rot = 0.f;
194 float time_factor = 1.f;
203 exec_.
rot = calculate_rotation(current_.
rot, desired_.
rot, time_factor);
206 desired_.
x = trans_x;
207 desired_.
y = trans_y;
208 exec_.
x = calculate_translation(current_.
x, desired_.
x, time_factor);
209 exec_.
y = calculate_translation(current_.
y, desired_.
y, time_factor);
222 drive(0.f, 0.f, 0.f);