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 <interfaces/MotorInterface.h> 30 #include <logging/logger.h> 31 #include <config/config.h> 33 #include <utils/time/time.h> 58 void drive(
float trans_x,
float trans_y,
float rot );
79 virtual float calculate_rotation(
float current,
float desired,
float time_factor ) = 0;
86 virtual float calculate_translation(
float current,
float desired,
float time_factor) = 0;
121 current_.
x = current_.
y = current_.
rot = 0.f;
122 desired_.
x = desired_.
y = desired_.
rot = 0.f;
123 exec_.
x = exec_.
y = exec_.
rot = 0.f;
125 std::string cfg_prefix =
"/plugins/colli/motor_instruct/";
144 BaseMotorInstruct::set_command()
147 logger_->
log_warn(
"BaseMotorInstruct",
"Cannot set command, no writer for MotorInterface '%s'", motor_->
id());
154 float exec_trans = std::fabs(std::sqrt( exec_.
x*exec_.
x + exec_.
y*exec_.
y ));
155 if ( exec_trans >= 0.05 ) {
158 float reduction = 3. / exec_trans;
161 float vx_max = fabs( exec_.
x * reduction );
162 float vy_max = fabs( exec_.
y * reduction );
165 cmd.
x = std::fmin(std::fmax(exec_.
x, -vx_max), vx_max);
166 cmd.
y = std::fmin(std::fmax(exec_.
y, -vy_max), vy_max);
170 if ( fabs(exec_.
rot) >= 0.01 ) {
172 cmd.
rot = std::fmin(std::fmax(exec_.
rot, -2*M_PI), 2*M_PI) ;
189 exec_.
x = exec_.
y = exec_.
rot = 0.f;
206 float time_factor = 1.f;
215 exec_.
rot = calculate_rotation( current_.
rot, desired_.
rot, time_factor );
218 desired_.
x = trans_x;
219 desired_.
y = trans_y;
220 exec_.
x = calculate_translation( current_.
x, desired_.
x, time_factor );
221 exec_.
y = calculate_translation( current_.
y, desired_.
y, time_factor );
235 drive( 0.f, 0.f, 0.f );
float x
Translation in x-direction.
Configuration * config_
The fawkse config.
TransRotMessage Fawkes BlackBoard Interface Message.
Fawkes library namespace.
const char * id() const
Get identifier of interface.
float trans_dec_
Translation deceleration.
Storing Translation and rotation.
float y
Translation in y-direction.
virtual ~BaseMotorInstruct()
Desctructor.
BaseMotorInstruct(MotorInterface *motor, float frequency, Logger *logger, Configuration *config)
Constructor.
float rot
Rotation around z-axis.
float rot_acc_
Rotation acceleration.
void drive(float trans_x, float trans_y, float rot)
Try to realize the proposed values with respect to the maximum allowed values.
bool has_writer() const
Check if there is a writer for the interface.
float rot_dec_
Rotation deceleration.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
Logger * logger_
The fawkes logger.
unsigned int msgq_enqueue(Message *message)
Enqueue message at end of queue.
float des_vx() const
Get des_vx value.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
void stop()
Executes a soft stop with respect to calculate_translation and calculate_rotation.
float trans_acc_
Translation acceleration.
float des_omega() const
Get des_omega value.
The Basic of a Motorinstructor.
float des_vy() const
Get des_vy value.
MotorInterface Fawkes BlackBoard Interface.
Interface for configuration handling.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.