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 <interfaces/MotorInterface.h>
30 #include <logging/logger.h>
31 #include <config/config.h>
32 
33 #include <utils/time/time.h>
34 
35 #include <cmath>
36 #include <string>
37 
38 namespace fawkes
39 {
40 #if 0 /* just to make Emacs auto-indent happy */
41 }
42 #endif
43 
44 /** @class BaseMotorInstruct <plugins/colli/drive_realization/base_motor_instruct.h>
45  * The Basic of a Motorinstructor.
46  */
47 
49 {
50  public:
52  float frequency,
53  Logger* logger,
54  Configuration* config );
55  virtual ~BaseMotorInstruct();
56 
57  ///\brief Try to realize the proposed values with respect to the maximum allowed values.
58  void drive( float trans_x, float trans_y, float rot );
59 
60  ///\brief Executes a soft stop with respect to calculate_translation and calculate_rotation.
61  void stop();
62 
63  protected:
64  Logger* logger_; /**< The fawkes logger */
65  Configuration* config_; /**< The fawkse config */
66 
67  float trans_acc_; /**< Translation acceleration */
68  float trans_dec_; /**< Translation deceleration */
69  float rot_acc_; /**< Rotation acceleration */
70  float rot_dec_; /**< Rotation deceleration */
71 
72  private:
73  /** calculates rotation speed
74  * has to be implemented in its base classes!
75  * is for the smoothness of rotation transitions.
76  * calculate maximum allowed rotation change between proposed and desired values
77  *
78  */
79  virtual float calculate_rotation( float current, float desired, float time_factor ) = 0;
80 
81  /** calculates translation speed.
82  * has to be implemented in its base classes!
83  * is for the smoothness of translation transitions.
84  * calculate maximum allowed translation change between proposed and desired values
85  */
86  virtual float calculate_translation( float current, float desired, float time_factor) = 0;
87 
88  MotorInterface* motor_;
89 
90  colli_trans_rot_t current_;
91  colli_trans_rot_t desired_;
92  colli_trans_rot_t exec_;
93 
94  ///\brief setCommand sets the executable commands and sends them
95  void set_command( );
96 };
97 
98 
99 /* ************************************************************************** */
100 /* ******************** BASE IMPLEMENTATION DETAILS *********************** */
101 /* ************************************************************************** */
102 
103 /** Constructor. Initializes all constants and the local pointers.
104  * @param motor The MotorInterface with all the motor information
105  * @param frequency The frequency of the colli (should become deprecated!)
106  * @param logger The fawkes logger
107  * @param config The fawkes configuration
108  */
109 inline
111  float frequency,
112  Logger* logger,
113  Configuration* config )
114  : logger_( logger ),
115  config_( config ),
116  motor_( motor )
117 {
118  logger_->log_debug("BaseMotorInstruct", "(Constructor): Entering");
119 
120  // init all members, zero, just to be on the save side
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;
124 
125  std::string cfg_prefix = "/plugins/colli/motor_instruct/";
126  trans_acc_ = config_->get_float((cfg_prefix + "trans_acc").c_str());
127  trans_dec_ = config_->get_float((cfg_prefix + "trans_dec").c_str());
128  rot_acc_ = config_->get_float((cfg_prefix + "rot_acc").c_str());
129  rot_dec_ = config_->get_float((cfg_prefix + "rot_dec").c_str());
130 
131  logger_->log_debug("BaseMotorInstruct", "(Constructor): Exiting");
132 }
133 
134 /** Desctructor. */
135 inline
137 {
138  logger_->log_debug("BaseMotorInstruct", "(Destructor): Entering");
139  logger_->log_debug("BaseMotorInstruct", "(Destructor): Exiting");
140 }
141 
142 /** Sends the drive command to the motor. */
143 inline void
144 BaseMotorInstruct::set_command()
145 {
146  if( !motor_->has_writer() ) {
147  logger_->log_warn("BaseMotorInstruct", "Cannot set command, no writer for MotorInterface '%s'", motor_->id());
148  return;
149  }
150 
151  colli_trans_rot_t cmd = {0.f, 0.f, 0.f};
152 
153  // Translation borders
154  float exec_trans = std::fabs(std::sqrt( exec_.x*exec_.x + exec_.y*exec_.y ));
155  if ( exec_trans >= 0.05 ) {
156  //send command where the total translation is between [-3, 3]
157  //Calculate factor of reduction to reach 3m/s
158  float reduction = 3. / exec_trans;
159 
160  //Calculate positive maximum for vx and vy
161  float vx_max = fabs( exec_.x * reduction );
162  float vy_max = fabs( exec_.y * reduction );
163 
164  //Calculate new desired velocities
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);
167  }
168 
169  // Rotation borders
170  if ( fabs(exec_.rot) >= 0.01 ) {
171  //send command within [-2*pi, 2*pi]
172  cmd.rot = std::fmin(std::fmax(exec_.rot, -2*M_PI), 2*M_PI) ;
173  }
174 
175  // Send the commands to the motor. No controlling afterwards done!!!!
176  motor_->msgq_enqueue(new MotorInterface::TransRotMessage(cmd.x, cmd.y, cmd.rot));
177 }
178 
179 
180 /** Try to realize the proposed values with respect to the physical constraints of the robot.
181  * @param trans_x the proposed x translation velocity
182  * @param trans_y the proposed y translation velocity
183  * @param rot the proposed rotation velocity
184  */
185 inline void
186 BaseMotorInstruct::drive( float trans_x, float trans_y, float rot )
187 {
188  // initializing driving values (to be on the sure side of life)
189  exec_.x = exec_.y = exec_.rot = 0.f;
190 
191  /*
192  // timediff storie to realize how often one was called
193  Time currentTime;
194  currentTime.stamp();
195  long timediff = (currentTime - m_OldTimestamp).in_msec();
196  float time_factor = (float)timediff / (1000.f / frequency_);
197 
198  if (time_factor < 0.5) {
199  logger_->log_debug("BaseMotorInstruct","( Drive ): Blackboard timing(case 1) strange, time_factor is %f", time_factor);
200  } else if (time_factor > 2.0) {
201  logger_->log_debug("BaseMotorInstruct", "( Drive ): Blackboard timing(case 2) strange, time_factor is %f", time_factor);
202  }
203 
204  m_OldTimestamp = currentTime;
205  */
206  float time_factor = 1.f;
207 
208  // getting currently performed values
209  current_.x = motor_->des_vx();
210  current_.y = motor_->des_vy();
211  current_.rot = motor_->des_omega();
212 
213  // calculate maximum allowed rotation change between proposed and desired values
214  desired_.rot = rot;
215  exec_.rot = calculate_rotation( current_.rot, desired_.rot, time_factor );
216 
217  // calculate maximum allowed translation change between proposed and desired values
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 );
222 
223  // send the command to the motor
224  set_command();
225 }
226 
227 
228 /** Executes a soft stop with respect to calculate_translation and calculate_rotation
229  * if it is called several times
230  */
231 inline void
233 {
234  // with respect to the physical borders do a stop to 0.0, 0.0.
235  drive( 0.f, 0.f, 0.f );
236 }
237 
238 } // namespace fawkes
239 #endif
float x
Translation in x-direction.
Definition: types.h:61
Configuration * config_
The fawkse config.
TransRotMessage Fawkes BlackBoard Interface Message.
Fawkes library namespace.
const char * id() const
Get identifier of interface.
Definition: interface.cpp:661
float trans_dec_
Translation deceleration.
Storing Translation and rotation.
Definition: types.h:60
float y
Translation in y-direction.
Definition: types.h:62
virtual ~BaseMotorInstruct()
Desctructor.
BaseMotorInstruct(MotorInterface *motor, float frequency, Logger *logger, Configuration *config)
Constructor.
float rot
Rotation around z-axis.
Definition: types.h:63
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.
Definition: interface.cpp:834
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.
Definition: interface.cpp:903
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.
Definition: config.h:67
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
Interface for logging.
Definition: logger.h:34