Fawkes API  Fawkes Development Version
backward_drive_mode.cpp
1 
2 /***************************************************************************
3  * backward_drive_mode.cpp - Implementation of drive-mode "backward"
4  *
5  * Created: Fri Oct 18 15:16:23 2013
6  * Copyright 2002 Stefan Jacobs
7  * 2013-2014 Bahram Maleki-Fard
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "backward_drive_mode.h"
24 
25 #include <utils/math/angle.h>
26 #include <utils/math/common.h>
27 
28 namespace fawkes
29 {
30 #if 0 /* just to make Emacs auto-indent happy */
31 }
32 #endif
33 
34 /** @class BackwardDriveModule <plugins/colli/drive_modes/backward_drive_mode.h>
35  * This is the SlowBackward drive-module, for slow backward only movements.
36  */
37 
38 /** Constructor.
39  * @param logger The fawkes logger
40  * @param config The fawkes configuration
41  */
43  : AbstractDriveMode(logger, config)
44 {
45  logger_->log_debug("BackwardDriveModule", "(Constructor): Entering...");
47 
48  max_trans_ = config_->get_float( "/plugins/colli/drive_mode/normal/max_trans" );
49  max_rot_ = config_->get_float( "/plugins/colli/drive_mode/normal/max_rot" );
50 
51  logger_->log_debug("BackwardDriveModule", "(Constructor): Exiting");
52 }
53 
54 
55 /** Destruct your local values here!
56  */
58 {
59  logger_->log_debug("BackwardDriveModule", "(Destructor): Entering...");
61  logger_->log_debug("BackwardDriveModule", "(Destructor): Exiting");
62 }
63 
64 
65 /** Calculate by given variables a new rotation to give for the motor to minimize curvature.
66  *
67  * DOC.: This here is the most complicated part in realizing the colli. By the given information
68  * I have to calculate a rotation I want to achieve in an optimal way.
69  * Here this is solved in an interesting way:
70  * First, check how long the curvature is, we want to drive to the target. This is done by
71  * approximating the size of the triangle around this curvature given by collision and
72  * and targetpoint and the angle between those both. Afterwards the time we have to drive
73  * with constant speed is calculated. Now we have the time we want to turn the angle. By
74  * multiplying this with a high constant (here 4), we rotate faster than we want to, but
75  * so the curvatures are not so extraordinary big. Afterwards there is an proportional
76  * part added, so we have a control factor here. (P-Regler ;-) )
77  *
78  * @return A desired rotation.
79  */
80 float
81 BackwardDriveModule::backward_curvature( float dist_to_target, float dist_to_trajec, float alpha,
82  float cur_trans, float cur_rot )
83 {
84  return 1.2f*alpha;
85 }
86 
87 
88 /** Calculate by given variables a new translation to give for the motor to
89  * minimize distance to the target.
90  *
91  * DOC.: This here is a fairly easy routine after the previous one ;-). It calculates
92  * to a proposed rotation the translation part. This means, in relation to
93  * distances to target and trajec and the current values we calculate a new one.
94  *
95  * @return A desired translation.
96  */
97 float
98 BackwardDriveModule::backward_translation( float dist_to_target, float dist_to_front, float alpha,
99  float cur_trans, float cur_rot, float des_rot)
100 {
101  float des_trans = 0.f;
102 
103  if ( fabs( des_rot ) >= 0.f && fabs( des_rot ) <= 1.f )
104  des_trans = lin_interpol( fabs( des_rot ), 0.f, 1.f, 0.7f, fabs(max_trans_+0.1f) );
105 
106  else if ( fabs( des_rot ) > 1.f )
107  des_trans = lin_interpol( fabs( des_rot ), M_PI, 1.f, 0.f, 0.7f );
108 
109  // test the borders (no agressive behaviour!)
110  if ( des_trans > 0.f ) des_trans = 0.f;
111  if ( des_trans < max_trans_ ) des_trans = max_trans_;
112 
113  // OLD STUFF
114  // // check stopping on target and compare distances with choosen velocities
115  // if ( fabs( dist_to_target - dist_to_front ) < 0.2 )
116  // {
117  // if (stop_at_target_ == true)
118  // des_trans = min( des_trans, dist_to_target*1.5 );
119  // else
120  // ; // do not stop, so drive behind the target with full power
121  // }
122  // else
123  // {
124  // des_trans = min( des_trans, dist_to_front );
125  // }
126 
127  // NEW STUFF
128  float trans_target = 10000.f;
129  float trans_front = 10000.f;
130 
131  if ( stop_at_target_ == true )
132  trans_target = guarantee_trans_stop( dist_to_target, cur_trans, des_trans );
133 
134  // And the next collision point
135  if ( dist_to_front < dist_to_target )
136  trans_front = guarantee_trans_stop( (1.f*dist_to_front)/2.f, cur_trans, des_trans );
137  // NEW STUFF END HERE
138 
139  des_trans = std::min( des_trans, std::min( trans_target, trans_front ) );
140 
141  return des_trans;
142 }
143 
144 
145 /* ************************************************************************** */
146 /* *********************** U P D A T E ************************* */
147 /* ************************************************************************** */
148 
149 /** Calculate here your desired settings. What you desire is checked afterwards to the current
150  * settings of the physical boundaries, but take care also.
151  *
152  * How you do this is up to you, but be careful, our hardware is expensive!!!!
153  *
154  * Available are:
155  *
156  * target_ --> current target coordinates to drive to
157  * robot_ --> current robot coordinates
158  * robot_vel_ --> current Motor velocities
159  *
160  * local_target_ --> our local target found by the search component we want to reach
161  * local_trajec_ --> The point we would collide with, if we would drive WITHOUT Rotation
162  *
163  * orient_at_target_ --> Do we have to orient ourself at the target?
164  * stop_at_target_ --> Do we have to stop really ON the target?
165  *
166  * Afterwards filled should be:
167  *
168  * proposed_ --> Desired translation and rotation speed
169  *
170  * Those values are questioned after an update() was called.
171  */
172 void
174 {
175  proposed_.x = proposed_.y = proposed_.rot = 0.f;
176 
177  float dist_to_target = sqrt( sqr(local_target_.x) + sqr(local_target_.y) );
178  float alpha = normalize_mirror_rad(atan2( local_target_.y, local_target_.x ) + M_PI);
179  float dist_to_trajec = sqrt( sqr(local_trajec_.x) + sqr(local_trajec_.y) );
180 
181 
182  proposed_.rot = backward_curvature( dist_to_target, dist_to_trajec, alpha,
184 
185 
186  if ( fabs( alpha ) <= M_PI_2+0.1f )
187  proposed_.x = backward_translation( dist_to_target, dist_to_trajec, alpha,
189 
190  // last time border check............. IMPORTANT!!!
191  // because the motorinstructor just tests robots physical borders.
192  if ( dist_to_target >= 0.04f ) {
193  proposed_.x = std::min ( proposed_.x, max_trans_ );
194  proposed_.x = std::max ( proposed_.x, 0.f );
195  proposed_.x *= -1.f;
196 
197  if (proposed_.rot > max_rot_)
199 
200  if (proposed_.rot < -max_rot_)
202 
203 
204  if ( !stop_at_target_ && dist_to_target < 1.f ) {
205  // reduce rotation velocity to avoid wild rotations
206  if ( proposed_.rot > 0.5f )
207  proposed_.rot = 0.5f;
208  else if ( proposed_.rot < -0.5f )
209  proposed_.rot = -0.5f;
210  }
211  }
212 }
213 
214 } // namespace fawkes
float x
Translation in x-direction.
Definition: types.h:61
cart_coord_2d_t local_target_
local target
Logger * logger_
The fawkes logger.
Fawkes library namespace.
colli_trans_rot_t proposed_
proposed translation and rotation for next timestep
bool stop_at_target_
flag if stopping on or after target
float max_trans_
The maximum translation speed.
void update()
Calculate here your desired settings.
BackwardDriveModule(Logger *logger, Configuration *config)
Constructor.
float y
Translation in y-direction.
Definition: types.h:62
float rot
Rotation around z-axis.
Definition: types.h:63
double sqr(double x)
Fast square multiplication.
Definition: common.h:37
float max_rot_
The maximum rotation speed.
NavigatorInterface::DriveMode drive_mode_
the drive mode name
colli_trans_rot_t robot_vel_
current robot velocity
float normalize_mirror_rad(float angle_rad)
Normalize angle in radian between -PI (inclusive) and PI (exclusive).
Definition: angle.h:75
float guarantee_trans_stop(float distance, float current_trans, float desired_trans)
Get velocity that guarantees a stop for a given distance.
float lin_interpol(float x, float left, float right, float bot, float top)
Perform linear interpolation.
Configuration * config_
The fawkes configuration.
This is the base class which calculates drive modes.
float y
y coordinate
Definition: types.h:61
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
float robot_speed_
current robo translation velocity
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.
~BackwardDriveModule()
Destruct your local values here!
cart_coord_2d_t local_trajec_
local trajectory
Interface for logging.
Definition: logger.h:34
float x
x coordinate
Definition: types.h:60