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 
37 namespace fawkes {
38 
39 /** @class BaseMotorInstruct <plugins/colli/drive_realization/base_motor_instruct.h>
40  * The Basic of a Motorinstructor.
41  */
42 
44 {
45 public:
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 
55 protected:
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 
64 private:
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_;
84  colli_trans_rot_t exec_;
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. */
130 inline void
131 BaseMotorInstruct::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  */
173 inline void
174 BaseMotorInstruct::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  */
218 inline 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:65
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.
Definition: interface.cpp:652
bool has_writer() const
Check if there is a writer for the interface.
Definition: interface.cpp:817
unsigned int msgq_enqueue(Message *message)
Enqueue message at end of queue.
Definition: interface.cpp:882
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