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>
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)
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/";
131 BaseMotorInstruct::set_command()
135 "Cannot set command, no writer for MotorInterface '%s'",
140 colli_trans_rot_t cmd = {0.f, 0.f, 0.f};
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);
165 motor_->
msgq_enqueue(
new MotorInterface::TransRotMessage(cmd.x, cmd.y, cmd.rot));
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);
The Basic of a Motorinstructor.
virtual ~BaseMotorInstruct()
Desctructor.
float trans_acc_
Translation acceleration.
void drive(float trans_x, float trans_y, float rot)
Try to realize the proposed values with respect to the maximum allowed values.
BaseMotorInstruct(MotorInterface *motor, float frequency, Logger *logger, Configuration *config)
Constructor.
float rot_acc_
Rotation acceleration.
float rot_dec_
Rotation deceleration.
Logger * logger_
The fawkes logger.
void stop()
Executes a soft stop with respect to calculate_translation and calculate_rotation.
Configuration * config_
The fawkse config.
float trans_dec_
Translation deceleration.
Interface for configuration handling.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
const char * id() const
Get identifier of interface.
bool has_writer() const
Check if there is a writer for the interface.
unsigned int msgq_enqueue(Message *message)
Enqueue message at end of queue.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
MotorInterface Fawkes BlackBoard Interface.
float des_vy() const
Get des_vy value.
float des_omega() const
Get des_omega value.
float des_vx() const
Get des_vx value.
Fawkes library namespace.
Storing Translation and rotation.
float x
Translation in x-direction.
float y
Translation in y-direction.
float rot
Rotation around z-axis.