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 /** @class BackwardDriveModule <plugins/colli/drive_modes/backward_drive_mode.h>
31  * This is the SlowBackward drive-module, for slow backward only movements.
32  */
33 
34 /** Constructor.
35  * @param logger The fawkes logger
36  * @param config The fawkes configuration
37  */
39 : AbstractDriveMode(logger, config)
40 {
41  logger_->log_debug("BackwardDriveModule", "(Constructor): Entering...");
43 
44  max_trans_ = config_->get_float("/plugins/colli/drive_mode/normal/max_trans");
45  max_rot_ = config_->get_float("/plugins/colli/drive_mode/normal/max_rot");
46 
47  logger_->log_debug("BackwardDriveModule", "(Constructor): Exiting");
48 }
49 
50 /** Destruct your local values here!
51  */
53 {
54  logger_->log_debug("BackwardDriveModule", "(Destructor): Entering...");
56  logger_->log_debug("BackwardDriveModule", "(Destructor): Exiting");
57 }
58 
59 /** Calculate by given variables a new rotation to give for the motor to minimize curvature.
60  *
61  * DOC.: This here is the most complicated part in realizing the colli. By the given information
62  * I have to calculate a rotation I want to achieve in an optimal way.
63  * Here this is solved in an interesting way:
64  * First, check how long the curvature is, we want to drive to the target. This is done by
65  * approximating the size of the triangle around this curvature given by collision and
66  * and targetpoint and the angle between those both. Afterwards the time we have to drive
67  * with constant speed is calculated. Now we have the time we want to turn the angle. By
68  * multiplying this with a high constant (here 4), we rotate faster than we want to, but
69  * so the curvatures are not so extraordinary big. Afterwards there is an proportional
70  * part added, so we have a control factor here. (P-Regler ;-) )
71  *
72  * @return A desired rotation.
73  */
74 float
75 BackwardDriveModule::backward_curvature(float dist_to_target,
76  float dist_to_trajec,
77  float alpha,
78  float cur_trans,
79  float cur_rot)
80 {
81  return 1.2f * alpha;
82 }
83 
84 /** Calculate by given variables a new translation to give for the motor to
85  * minimize distance to the target.
86  *
87  * DOC.: This here is a fairly easy routine after the previous one ;-). It calculates
88  * to a proposed rotation the translation part. This means, in relation to
89  * distances to target and trajec and the current values we calculate a new one.
90  *
91  * @return A desired translation.
92  */
93 float
94 BackwardDriveModule::backward_translation(float dist_to_target,
95  float dist_to_front,
96  float alpha,
97  float cur_trans,
98  float cur_rot,
99  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)
111  des_trans = 0.f;
112  if (des_trans < max_trans_)
113  des_trans = max_trans_;
114 
115  // OLD STUFF
116  // // check stopping on target and compare distances with choosen velocities
117  // if ( fabs( dist_to_target - dist_to_front ) < 0.2 )
118  // {
119  // if (stop_at_target_ == true)
120  // des_trans = min( des_trans, dist_to_target*1.5 );
121  // else
122  // ; // do not stop, so drive behind the target with full power
123  // }
124  // else
125  // {
126  // des_trans = min( des_trans, dist_to_front );
127  // }
128 
129  // NEW STUFF
130  float trans_target = 10000.f;
131  float trans_front = 10000.f;
132 
133  if (stop_at_target_ == true)
134  trans_target = guarantee_trans_stop(dist_to_target, cur_trans, des_trans);
135 
136  // And the next collision point
137  if (dist_to_front < dist_to_target)
138  trans_front = guarantee_trans_stop((1.f * dist_to_front) / 2.f, cur_trans, des_trans);
139  // NEW STUFF END HERE
140 
141  des_trans = std::min(des_trans, std::min(trans_target, trans_front));
142 
143  return des_trans;
144 }
145 
146 /* ************************************************************************** */
147 /* *********************** U P D A T E ************************* */
148 /* ************************************************************************** */
149 
150 /** Calculate here your desired settings. What you desire is checked afterwards to the current
151  * settings of the physical boundaries, but take care also.
152  *
153  * How you do this is up to you, but be careful, our hardware is expensive!!!!
154  *
155  * Available are:
156  *
157  * target_ --> current target coordinates to drive to
158  * robot_ --> current robot coordinates
159  * robot_vel_ --> current Motor velocities
160  *
161  * local_target_ --> our local target found by the search component we want to reach
162  * local_trajec_ --> The point we would collide with, if we would drive WITHOUT Rotation
163  *
164  * orient_at_target_ --> Do we have to orient ourself at the target?
165  * stop_at_target_ --> Do we have to stop really ON the target?
166  *
167  * Afterwards filled should be:
168  *
169  * proposed_ --> Desired translation and rotation speed
170  *
171  * Those values are questioned after an update() was called.
172  */
173 void
175 {
176  proposed_.x = proposed_.y = proposed_.rot = 0.f;
177 
178  float dist_to_target = sqrt(sqr(local_target_.x) + sqr(local_target_.y));
179  float alpha = normalize_mirror_rad(atan2(local_target_.y, local_target_.x) + M_PI);
180  float dist_to_trajec = sqrt(sqr(local_trajec_.x) + sqr(local_trajec_.y));
181 
182  proposed_.rot =
183  backward_curvature(dist_to_target, dist_to_trajec, alpha, -robot_speed_, -robot_vel_.rot);
184 
185  if (fabs(alpha) <= M_PI_2 + 0.1f)
186  proposed_.x = backward_translation(
187  dist_to_target, dist_to_trajec, alpha, -robot_speed_, -robot_vel_.rot, proposed_.rot);
188 
189  // last time border check............. IMPORTANT!!!
190  // because the motorinstructor just tests robots physical borders.
191  if (dist_to_target >= 0.04f) {
192  proposed_.x = std::min(proposed_.x, max_trans_);
193  proposed_.x = std::max(proposed_.x, 0.f);
194  proposed_.x *= -1.f;
195 
196  if (proposed_.rot > max_rot_)
198 
199  if (proposed_.rot < -max_rot_)
201 
202  if (!stop_at_target_ && dist_to_target < 1.f) {
203  // reduce rotation velocity to avoid wild rotations
204  if (proposed_.rot > 0.5f)
205  proposed_.rot = 0.5f;
206  else if (proposed_.rot < -0.5f)
207  proposed_.rot = -0.5f;
208  }
209  }
210 }
211 
212 } // 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:72
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:67
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:64
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:41
float x
x coordinate
Definition: types.h:66