Fawkes API Fawkes Development Version
base_motor_instruct.h
1
2/***************************************************************************
3 * base_motor_instruct.h - Abstract base class for a motor instructor
4 *
5 * Created: Fri Oct 18 15:16:23 2013
6 * Copyright 2002 Stefan Jacobs
7 * 2013 Bahram Maleki-Fard
8 * 2014 Tobias Neumann
9 ****************************************************************************/
10
11/* This program is free software; you can redistribute it and/or modify
12 * it under the terms of the GNU General Public License as published by
13 * the Free Software Foundation; either version 2 of the License, or
14 * (at your option) any later version.
15 *
16 * This program is distributed in the hope that it will be useful,
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19 * GNU Library General Public License for more details.
20 *
21 * Read the full text in the LICENSE.GPL file in the doc directory.
22 */
23
24#ifndef _PLUGINS_COLLI_DRIVE_REALIZATION_BASE_MOTORINSTRUCT_H_
25#define _PLUGINS_COLLI_DRIVE_REALIZATION_BASE_MOTORINSTRUCT_H_
26
27#include "../common/types.h"
28
29#include <config/config.h>
30#include <interfaces/MotorInterface.h>
31#include <logging/logger.h>
32#include <utils/time/time.h>
33
34#include <cmath>
35#include <string>
36
37namespace fawkes {
38
39/** @class BaseMotorInstruct <plugins/colli/drive_realization/base_motor_instruct.h>
40 * The Basic of a Motorinstructor.
41 */
42
44{
45public:
46 BaseMotorInstruct(MotorInterface *motor, float frequency, Logger *logger, Configuration *config);
47 virtual ~BaseMotorInstruct();
48
49 ///\brief Try to realize the proposed values with respect to the maximum allowed values.
50 void drive(float trans_x, float trans_y, float rot);
51
52 ///\brief Executes a soft stop with respect to calculate_translation and calculate_rotation.
53 void stop();
54
55protected:
56 Logger * logger_; /**< The fawkes logger */
57 Configuration *config_; /**< The fawkse config */
58
59 float trans_acc_; /**< Translation acceleration */
60 float trans_dec_; /**< Translation deceleration */
61 float rot_acc_; /**< Rotation acceleration */
62 float rot_dec_; /**< Rotation deceleration */
63
64private:
65 /** calculates rotation speed
66 * has to be implemented in its base classes!
67 * is for the smoothness of rotation transitions.
68 * calculate maximum allowed rotation change between proposed and desired values
69 *
70 */
71 virtual float calculate_rotation(float current, float desired, float time_factor) = 0;
72
73 /** calculates translation speed.
74 * has to be implemented in its base classes!
75 * is for the smoothness of translation transitions.
76 * calculate maximum allowed translation change between proposed and desired values
77 */
78 virtual float calculate_translation(float current, float desired, float time_factor) = 0;
79
80 MotorInterface *motor_;
81
82 colli_trans_rot_t current_;
83 colli_trans_rot_t desired_;
85
86 ///\brief setCommand sets the executable commands and sends them
87 void set_command();
88};
89
90/* ************************************************************************** */
91/* ******************** BASE IMPLEMENTATION DETAILS *********************** */
92/* ************************************************************************** */
93
94/** Constructor. Initializes all constants and the local pointers.
95 * @param motor The MotorInterface with all the motor information
96 * @param frequency The frequency of the colli (should become deprecated!)
97 * @param logger The fawkes logger
98 * @param config The fawkes configuration
99 */
101 float frequency,
102 Logger * logger,
103 Configuration * config)
104: logger_(logger), config_(config), motor_(motor)
105{
106 logger_->log_debug("BaseMotorInstruct", "(Constructor): Entering");
107
108 // init all members, zero, just to be on the save side
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;
112
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());
118
119 logger_->log_debug("BaseMotorInstruct", "(Constructor): Exiting");
120}
121
122/** Desctructor. */
124{
125 logger_->log_debug("BaseMotorInstruct", "(Destructor): Entering");
126 logger_->log_debug("BaseMotorInstruct", "(Destructor): Exiting");
127}
128
129/** Sends the drive command to the motor. */
130inline void
131BaseMotorInstruct::set_command()
132{
133 if (!motor_->has_writer()) {
134 logger_->log_warn("BaseMotorInstruct",
135 "Cannot set command, no writer for MotorInterface '%s'",
136 motor_->id());
137 return;
138 }
139
140 colli_trans_rot_t cmd = {0.f, 0.f, 0.f};
141
142 // Translation borders
143 float exec_trans = std::fabs(std::sqrt(exec_.x * exec_.x + exec_.y * exec_.y));
144 if (exec_trans >= 0.05) {
145 //send command where the total translation is between [-3, 3]
146 //Calculate factor of reduction to reach 3m/s
147 float reduction = 3. / exec_trans;
148
149 //Calculate positive maximum for vx and vy
150 float vx_max = fabsf(exec_.x * reduction);
151 float vy_max = fabsf(exec_.y * reduction);
152
153 //Calculate new desired velocities
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);
156 }
157
158 // Rotation borders
159 if (fabs(exec_.rot) >= 0.01) {
160 //send command within [-2*pi, 2*pi]
161 cmd.rot = std::fmin(std::fmax(exec_.rot, -2 * M_PI), 2 * M_PI);
162 }
163
164 // Send the commands to the motor. No controlling afterwards done!!!!
165 motor_->msgq_enqueue(new MotorInterface::TransRotMessage(cmd.x, cmd.y, cmd.rot));
166}
167
168/** Try to realize the proposed values with respect to the physical constraints of the robot.
169 * @param trans_x the proposed x translation velocity
170 * @param trans_y the proposed y translation velocity
171 * @param rot the proposed rotation velocity
172 */
173inline void
174BaseMotorInstruct::drive(float trans_x, float trans_y, float rot)
175{
176 // initializing driving values (to be on the sure side of life)
177 exec_.x = exec_.y = exec_.rot = 0.f;
178
179 /*
180 // timediff storie to realize how often one was called
181 Time currentTime;
182 currentTime.stamp();
183 long timediff = (currentTime - m_OldTimestamp).in_msec();
184 float time_factor = (float)timediff / (1000.f / frequency_);
185
186 if (time_factor < 0.5) {
187 logger_->log_debug("BaseMotorInstruct","( Drive ): Blackboard timing(case 1) strange, time_factor is %f", time_factor);
188 } else if (time_factor > 2.0) {
189 logger_->log_debug("BaseMotorInstruct", "( Drive ): Blackboard timing(case 2) strange, time_factor is %f", time_factor);
190 }
191
192 m_OldTimestamp = currentTime;
193 */
194 float time_factor = 1.f;
195
196 // getting currently performed values
197 current_.x = motor_->des_vx();
198 current_.y = motor_->des_vy();
199 current_.rot = motor_->des_omega();
200
201 // calculate maximum allowed rotation change between proposed and desired values
202 desired_.rot = rot;
203 exec_.rot = calculate_rotation(current_.rot, desired_.rot, time_factor);
204
205 // calculate maximum allowed translation change between proposed and desired values
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);
210
211 // send the command to the motor
212 set_command();
213}
214
215/** Executes a soft stop with respect to calculate_translation and calculate_rotation
216 * if it is called several times
217 */
218inline void
220{
221 // with respect to the physical borders do a stop to 0.0, 0.0.
222 drive(0.f, 0.f, 0.f);
223}
224
225} // namespace fawkes
226#endif
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.
Definition: config.h:68
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
unsigned int msgq_enqueue(Message *message, bool proxy=false)
Enqueue message at end of queue.
Definition: interface.cpp:915
const char * id() const
Get identifier of interface.
Definition: interface.cpp:661
bool has_writer() const
Check if there is a writer for the interface.
Definition: interface.cpp:848
Interface for logging.
Definition: logger.h:42
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.
Definition: types.h:60
float x
Translation in x-direction.
Definition: types.h:61
float y
Translation in y-direction.
Definition: types.h:62
float rot
Rotation around z-axis.
Definition: types.h:63